openpilot v0.9.6 release

date: 2024-02-21T23:02:42
master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5

third_party/acados/include/acados/dense_qp/dense_qp_common.h

@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/dense_qp/dense_qp_daqp.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// daqp
+#include "daqp/include/types.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+
+typedef struct dense_qp_daqp_opts_
+{
+    DAQPSettings* daqp_opts;
+    int warm_start;
+} dense_qp_daqp_opts;
+
+
+typedef struct dense_qp_daqp_memory_
+{
+    double* lb_tmp;
+    double* ub_tmp;
+    int* idxb;
+    int* idxv_to_idxb;
+    int* idxs;
+    int* idxdaqp_to_idxs;
+
+    double* Zl;
+    double* Zu;
+    double* zl;
+    double* zu;
+    double* d_ls;
+    double* d_us;
+
+    double time_qp_solver_call;
+    int iter;
+    DAQPWorkspace * daqp_work;
+
+} dense_qp_daqp_memory;
+
+// opts
+acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+// memory 
+acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+// functions
+int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_

third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h

@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h

@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/dense_qp/dense_qp_qore.h

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h

@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_memory_reset(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_

third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h

@@ -0,0 +1,446 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+
+/// \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);
+    void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    // initalize this struct with default values
+    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
+
+    void *raw_memory; // Pointer to allocated memory, to be used for freeing
+} 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;
+
+    /// Pointer to allocated memory, to be used for freeing.
+    void *raw_memory;
+
+} 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]
+    double inf_norm_res;
+
+    void *raw_memory; // Pointer to allocated memory, to be used for freeing
+
+} 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;
+    int print_level;
+
+    // TODO: move to separate struct?
+    ocp_nlp_globalization_t globalization;
+    int full_step_dual;
+    int line_search_use_sufficient_descent;
+    int globalization_use_SOC;
+    double alpha_min;
+    double alpha_reduction;
+    double eps_sufficient_descent;
+} 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
+    struct blasfeo_dvec tmp;  // tmp
+    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);
+//
+void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
+
+/************************************************
+ * 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
+
+    // for globalization: -> move to module?!
+    ocp_nlp_out *tmp_nlp_out;
+    ocp_nlp_out *weight_merit_fun;
+    struct blasfeo_dvec tmp_nxu;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec dxnext_dy;
+
+} 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_alias_memory_to_submodules(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_submodules(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);
+//
+int ocp_nlp_precompute_common(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_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,
+            int check_early_termination);
+//
+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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h

@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_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);
+
+//
+void ocp_nlp_constraints_bgh_model_get(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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h

@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_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);
+//
+void ocp_nlp_constraints_bgp_model_get(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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h

@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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);
+    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);
+    void (*model_get)(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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h

@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+///
+/// \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_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);
+    void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+
+} 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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h

@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
+/// \brief This module implements convex-over-nonlinear costs of the form
+/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_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_conl_dims;
+
+//
+acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
+//
+void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    external_function_generic *conl_cost_fun;
+    external_function_generic *conl_cost_fun_jac_hess;
+    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_conl_model;
+
+//
+acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    bool gauss_newton_hess;  // dummy options, we always use a gauss-newton hessian
+} ocp_nlp_cost_conl_opts;
+
+//
+acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_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 ux in tmp_nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    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_conl_memory;
+
+//
+acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat W;             // hessian of outer loss function
+    struct blasfeo_dmat W_chol;        // cholesky factor of hessian of outer loss function
+    struct blasfeo_dmat Jt_ux;         // jacobian of inner residual function
+    struct blasfeo_dmat Jt_ux_tilde;   // jacobian of inner residual function plus gradient contribution of algebraic variables
+    struct blasfeo_dmat Jt_z;          // jacobian of inner residual function wrt algebraic variables
+    struct blasfeo_dmat tmp_nv_ny;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;
+} ocp_nlp_cost_conl_workspace;
+
+//
+acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_config_initialize_default(void *config);
+//
+void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_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_CONL_H_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h

@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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 nz;  // number of algebraic variables
+    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_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_nunx_nunx;
+    struct blasfeo_dmat tmp_nz_nz;
+    struct blasfeo_dmat tmp_nz_nunx;
+    struct blasfeo_dvec tmp_nunxnz;
+    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_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+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_

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h

@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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);
+//
+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;
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
+    int Cyt_or_scaling_changed;         ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
+} ocp_nlp_cost_ls_model;
+
+//
+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                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+// computations that are done once when solver is created
+void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h

@@ -0,0 +1,211 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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,z} \| y(x,u,z,p) - 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 nz;  // number of algebraic variables
+    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_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;
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
+} 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_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_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_dmat Vz;
+    struct blasfeo_dmat Cyt_tilde;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;
+    struct blasfeo_dvec tmp_nz;
+} 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_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_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);
+    // get shooting node gap x_next(x_n, u_n) - x_{n+1}
+    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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h

@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h

@@ -0,0 +1,192 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h

@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h

@@ -0,0 +1,146 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h

@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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
+ ************************************************/
+
+//
+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
+ ************************************************/
+//
+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_
+
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h

@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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_
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h

@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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 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;
+    double time_solution_sensitivities;
+
+    // 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);
+//
+void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+
+
+/************************************************
+ * 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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h

@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \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)
+
+} 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;
+    double time_solution_sensitivities;
+
+    // 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
+ ************************************************/
+//
+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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_qp/ocp_qp_common.h

@@ -0,0 +1,180 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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 (*memory_reset)(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_

third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h

@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h

@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h

@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    int status;
+
+} 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_memory_reset(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_

third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h

@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    int status;
+
+} 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_

third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h

@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    int status;
+
+} 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_

third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    int status;
+
+} 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_

third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h

@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    int status;
+
+} 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_

third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h

@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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);
+    void (*dims_get)(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);
+    void (*memory_reset)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
+    acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
+    int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
+    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_

third_party/acados/include/acados/sim/sim_collocation_utils.h

@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+
+
+typedef enum
+{
+    GAUSS_LEGENDRE,
+    GAUSS_RADAU_IIA,
+} sim_collocation_type;
+
+
+//
+// acados_size_t gauss_legendre_nodes_work_calculate_size(int ns);
+//
+// void gauss_legendre_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_tableau_work_calculate_size(int ns);
+//
+// void calculate_butcher_tableau_from_nodes(int ns, double *nodes, double *b, double *A, void *work);
+//
+void calculate_butcher_tableau(int ns, sim_collocation_type collocation_type, double *c_vec,
+                               double *b_vec, double *A_mat, void *work);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_

third_party/acados/include/acados/sim/sim_common.h

@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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
+    sim_collocation_type collocation_type;
+
+    // for explicit integrators: newton_iter == 0 && scheme == NULL
+    // && jac_reuse=false
+    int newton_iter;
+    bool jac_reuse;
+    // Newton_scheme *scheme;
+
+    double newton_tol; // optinally used in implicit integrators
+
+    // 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_

third_party/acados/include/acados/sim/sim_erk_integrator.h

@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/sim/sim_gnsf.h

@@ -0,0 +1,364 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/sim/sim_irk_integrator.h

@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h

@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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;
+    // int init_K;
+
+	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_

third_party/acados/include/acados/utils/external_function_generic.h

@@ -0,0 +1,242 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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)(void);
+    int (*casadi_n_out)(void);
+    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)(void);
+    int (*casadi_n_out)(void);
+    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_

third_party/acados/include/acados/utils/math.h

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_UTILS_MATH_H_
+#define ACADOS_UTILS_MATH_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/utils/types.h"
+
+#if defined(__MABX2__)
+double fmax(double a, double b);
+int isnan(double x);
+#endif
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+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_

third_party/acados/include/acados/utils/mem.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/utils/print.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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
+// TODO: move printing routines below that print qp structures to HPIPM!
+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_in_to_file(FILE *file, ocp_qp_in *qp_in);
+
+void print_ocp_qp_out(ocp_qp_out *qp_out);
+
+void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);
+
+void print_ocp_qp_res(ocp_qp_res *qp_res);
+
+void print_dense_qp_in(dense_qp_in *qp_in);
+// 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_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_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_

third_party/acados/include/acados/utils/strsep.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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_

third_party/acados/include/acados/utils/timing.h

@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#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(__MABX2__)
+
+#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_

third_party/acados/include/acados/utils/types.h

@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_TYPES_H_
+#define ACADOS_UTILS_TYPES_H_
+
+/* Symbol visibility in DLLs */
+#ifndef ACADOS_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define ACADOS_SYMBOL_EXPORT
+    #else
+      #define ACADOS_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && ((__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
+    #define ACADOS_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define ACADOS_SYMBOL_EXPORT
+  #endif
+#endif
+
+#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_NAN_DETECTED,
+    ACADOS_MAXITER,
+    ACADOS_MINSTEP,
+    ACADOS_QP_FAILURE,
+    ACADOS_READY,
+};
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_TYPES_H_