openpilot v0.9.6 release

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

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_