casadi 3.6.4__cp39-none-manylinux2014_aarch64.whl → 3.6.6__cp39-none-manylinux2014_aarch64.whl

casadi/include/fatrop/ocp/OCPAdapter.hpp

@@ -0,0 +1,197 @@
+/*
+ * Fatrop - A fast trajectory optimization solver
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
+ *
+ * This file is part of Fatrop.
+ *
+ * Fatrop is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Fatrop is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
+#ifndef OCPEVALUATORINCLUDED
+#define OCPEVALUATORINCLUDED
+#include "OCPKKT.hpp"
+#include "OCPAbstract.hpp"
+#include "fatrop/solver/FatropData.hpp"
+#include "OCP.hpp"
+#include <memory>
+#include "fatrop/auxiliary/Common.hpp"
+#define OCPMACRO(type, name, suffix) type name##suffix = ((type)OCP->name)
+#define AUXMACRO(type, name, suffix) type name##suffix = ((type)OCP->aux.name)
+#define SOLVERMACRO(type, name, suffix) type name##suffix = ((type)name)
+#ifdef ENABLE_MULTITHREADING
+#include <omp.h>
+#endif
+
+// Example: you can make a for loop parallel with the following code. But to be further investigated to do this in an efficient way...
+// #ifdef ENABLE_MULTITHREADING
+// #pragma omp parallel for
+// <your for loop>
+// #endif
+
+namespace fatrop
+{
+    class OCPAdapter : public OCP // public OCP -> also include KKTmemory, OCPDims, ...
+    {
+    public:
+        OCPAdapter(const std::shared_ptr<OCPAbstract> &ocptempl_, const std::shared_ptr<FatropOptions> &options) : K(ocptempl_->get_horizon_length()),
+                                                                                                                   nuexpr(TransformRange<fatrop_int>(0, K, [&ocptempl_](fatrop_int k)
+                                                                                                                                                     { return ocptempl_->get_nu(k); })),
+                                                                                                                   nxexpr(TransformRange<fatrop_int>(0, K, [&ocptempl_](fatrop_int k)
+                                                                                                                                                     { return ocptempl_->get_nx(k); })),
+                                                                                                                   ngexpr(TransformRange<fatrop_int>(0, K, [&ocptempl_](fatrop_int k)
+                                                                                                                                                     { return ocptempl_->get_ng(k); })),
+                                                                                                                   ngineqexpr(TransformRange<fatrop_int>(0, K, [&ocptempl_](fatrop_int k)
+                                                                                                                                                         { return ocptempl_->get_ng_ineq(k); })),
+                                                                                                                   nstageparamsexpr(TransformRange<fatrop_int>(0, K, [&ocptempl_](fatrop_int k)
+                                                                                                                                                               { return ocptempl_->get_n_stage_params(k); })),
+                                                                                                                   offs_stageparams(offsets(nstageparamsexpr)), stageparams(sum(nstageparamsexpr), 0.0), globalparams(ocptempl_->get_n_global_params(), 0.0), options(options), ocptempl(ocptempl_)
+        {
+#ifdef ENABLE_MULTITHREADING
+            // check if environment variable OMP_NUM_THREADS is set
+            if (getenv("OMP_NUM_THREADS") == NULL)
+            {
+                throw std::runtime_error("Environment variable OMP_NUM_THREADS is not set. Please set it to the number of threads you want to use or compile fatrop with multithreading disabled.");
+            }
+#endif
+
+            // initialize the default parameters
+            ocptempl_->get_default_global_params(globalparams.data());
+            fatrop_int offs = 0;
+            for (fatrop_int k = 0; k < K; k++)
+            {
+                ocptempl_->get_default_stage_params(stageparams.data() + offs, k);
+                offs += ocptempl_->get_n_stage_params(k);
+            }
+            // initialize gradbuf
+            // for (fatrop_int k = 0; k < K; k++)
+            //     gradbuf.emplace_back(ocptempl_->get_nuk(k) + ocptempl_->get_nxk(k));
+            x_dummy = std::vector<double>(maxel(nxexpr), 0.0);
+        }
+        void reset() override
+        {
+        }
+        void print_kkt_matrix(OCPKKTMemory *OCP);
+        fatrop_int eval_lag_hess(
+            OCPKKTMemory *OCP,
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &lam) override;
+        fatrop_int eval_constr_jac(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars) override;
+        fatrop_int eval_contr_viol(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            FatropVecBF &constraint_violation) override;
+        fatrop_int eval_ineqs(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &primal_vars,
+            FatropVecBF &constraint_violation) override;
+        fatrop_int eval_obj_grad(
+            OCPKKTMemory *OCP,
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            FatropVecBF &gradient_x) override;
+        fatrop_int eval_obj(
+            OCPKKTMemory *OCP,
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            double &res) override;
+        fatrop_int integrate_dynamics(
+            OCPKKTMemory *OCP,
+            const fatrop_int k,
+            const FatropVecBF &uk,
+            const FatropVecBF &xk,
+            FatropVecBF &xkp1) override;
+        OCPDims get_ocp_dims() const override
+        {
+            return OCPDims(ocptempl->get_horizon_length(), nuexpr, nxexpr, ngexpr, ngineqexpr, nstageparamsexpr, ocptempl->get_n_global_params());
+        }
+
+    public:
+        void set_parameters(const std::vector<double> &stage_params_in, const std::vector<double> &global_params_in) override;
+        void set_initial_sol_guess(const std::shared_ptr<FatropData> &fatropdata, std::vector<double> &initial_u, std::vector<double> &initial_x) override;
+        void get_solution(const std::shared_ptr<FatropData> &fatropdata, std::vector<double> &u, std::vector<double> &x) override;
+        double *get_global_parameters()
+        {
+            if (globalparams.size() == 0)
+                return nullptr;
+            return globalparams.data();
+        }
+        double *get_stage_parameters()
+        {
+            if (stageparams.size() == 0)
+                return nullptr;
+            return stageparams.data();
+        }
+        std::vector<double> &get_global_parameters_vec()
+        {
+            return globalparams;
+        }
+        std::vector<double> &get_stage_parameters_vec()
+        {
+            return stageparams;
+        }
+        fatrop_int get_bounds(
+            FatropVecBF &lower,
+            FatropVecBF &upper) const override
+        {
+            fatrop_int offs = 0;
+            double *lower_p = ((VEC *)lower)->pa;
+            double *upper_p = ((VEC *)upper)->pa;
+            for (fatrop_int k = 0; k < K; k++)
+            {
+                ocptempl->get_bounds(lower_p + offs, upper_p + offs, k);
+                offs += ocptempl->get_ng_ineq(k);
+            }
+            return 0;
+        };
+        fatrop_int get_initial_sol_guess(
+            FatropVecBF &initial) const override
+        {
+            fatrop_int offs = 0;
+            for (fatrop_int k = 0; k < K; k++)
+            {
+                ocptempl->get_initial_uk(((VEC *)initial)->pa + offs, k);
+                offs += ocptempl->get_nu(k);
+                ocptempl->get_initial_xk(((VEC *)initial)->pa + offs, k);
+                offs += ocptempl->get_nx(k);
+            }
+            return 0;
+        }
+        // virtual fatrop_int GetDefaultParams(
+        //     FatropOptions &params)
+        // {
+        // };
+
+    public:
+        fatrop_int K;
+        FatropVector<fatrop_int> nuexpr;
+        FatropVector<fatrop_int> nxexpr;
+        FatropVector<fatrop_int> ngexpr;
+        FatropVector<fatrop_int> ngineqexpr;
+        FatropVector<fatrop_int> nstageparamsexpr;
+        FatropVector<fatrop_int> offs_stageparams;
+        std::vector<double> stageparams;
+        std::vector<double> globalparams;
+        std::vector<double> x_dummy;
+        // std::vector<VECBF> gradbuf;
+        std::shared_ptr<FatropOptions> options;
+
+    private:
+        std::shared_ptr<OCPAbstract> ocptempl;
+    };
+} // namespace fatrop
+
+#endif // OCPEVALUATORINCLUDED

casadi/include/fatrop/ocp/OCPCInterface.h

@@ -0,0 +1,289 @@
+/*
+ * Fatrop - A fast trajectory optimization solver
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
+ *
+ * This file is part of Fatrop.
+ *
+ * Fatrop is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Fatrop is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
+
+#ifndef OCPCINTERFACEINCLUDED
+#define OCPCINTERFACEINCLUDED
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Symbol visibility in DLLs */
+#ifndef FATROP_SYMBOL_EXPORT
+#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+#if defined(STATIC_LINKED)
+#define FATROP_SYMBOL_EXPORT
+#else
+#define FATROP_SYMBOL_EXPORT __declspec(dllexport)
+#endif
+#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+#define FATROP_SYMBOL_EXPORT __attribute__((visibility("default")))
+#else
+#define FATROP_SYMBOL_EXPORT
+#endif
+#endif
+
+#include <blasfeo.h>
+
+#define fatrop_int int
+
+#ifndef FATROP_OCP_SOLVER_IMPLEMENTATION
+    // Opaque types
+    typedef struct FatropOcpCSolver FatropOcpCSolver;
+#endif
+
+struct FatropOcpCDims
+{
+    const fatrop_int *ux_offs;
+    const fatrop_int *g_offs;
+    const fatrop_int *dyn_offs;
+    const fatrop_int *dyn_eq_offs;
+    const fatrop_int *g_ineq_offs;
+    const fatrop_int *ineq_offs;
+    fatrop_int max_nu;
+    fatrop_int max_nx;
+    fatrop_int max_ng;
+    fatrop_int max_ngineq;
+    fatrop_int n_ineqs;
+    fatrop_int K;
+    const fatrop_int *nu;
+    const fatrop_int *nx;
+    const fatrop_int *ng;
+    const fatrop_int *ng_ineq;
+};
+
+struct FatropOcpCStats
+{
+    double compute_sd_time;
+    double duinf_time;
+    double eval_hess_time;
+    double eval_jac_time;
+    double eval_cv_time;
+    double eval_grad_time;
+    double eval_obj_time;
+    double initialization_time;
+    double time_total;
+    int eval_hess_count;
+    int eval_jac_count;
+    int eval_cv_count;
+    int eval_grad_count;
+    int eval_obj_count;
+    int iterations_count;
+    int return_flag;
+};
+
+// Function pointer types
+typedef fatrop_int (*FatropOcpCGetDim_k)(fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCGetDim)(void* user_data);
+typedef fatrop_int (*FatropOcpCGetDouble_k)(double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCGetDouble)(double*, void* user_data);
+
+typedef fatrop_int (*FatropOcpCEval_BAbt_k)(const double*, const double*, const double*, const double*, const double*, struct blasfeo_dmat*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_RSQrqt_k)(const double*, const double*, const double*, const double*, const double*, const double*, const double*, const double*, struct blasfeo_dmat*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_Ggt_k)(const double*, const double*, const double*, const double*, struct blasfeo_dmat*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_Ggt_ineq_k)(const double*, const double*, const double*, const double*, struct blasfeo_dmat*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_b_k)(const double*, const double*, const double*, const double*, const double*, double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_g_k)(const double*, const double*, const double*, const double*, double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_gineq_k)(const double*, const double*, const double*, const double*, double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_rq_k)(const double*, const double*, const double*, const double*, const double*, double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCEval_L_k)(const double*, const double*, const double*, const double*, const double*, double*, fatrop_int, void* user_data);
+typedef fatrop_int (*FatropOcpCGetBounds_k)(double*, double*, fatrop_int, void* user_data);
+
+typedef fatrop_int (*FatropOcpCFullEvalLagHess)(double, const double*, const double*, const double*, const double*, struct blasfeo_dmat *, const struct FatropOcpCDims*, void*);
+typedef fatrop_int (*FatropOcpCFullEvalConstrJac)(const double*, const double*, const double*, struct blasfeo_dmat *, struct blasfeo_dmat *, struct blasfeo_dmat *, const struct FatropOcpCDims*, void*);
+typedef fatrop_int (*FatropOcpCFullEvalContrViol)(const double*, const double*, const double*, double *, const struct FatropOcpCDims*, void*);
+typedef fatrop_int (*FatropOcpCFullEvalObjGrad)(double, const double*, const double*, const double*, double *, const struct FatropOcpCDims*, void*);
+typedef fatrop_int (*FatropOcpCFullEvalObj)(double, const double*, const double*, const double*, double *, const struct FatropOcpCDims*, void*);
+
+typedef void (*FatropOcpCWrite)(const char* msg, int num);
+typedef void (*FatropOcpCFlush)(void);
+
+
+struct FatropOcpCInterface
+{
+    /// @brief number of states for time step k
+    /// @param k: time step
+    FatropOcpCGetDim_k get_nx;
+    /// @brief number of inputs for time step k
+    /// @param k: time step
+    FatropOcpCGetDim_k get_nu;
+    /// @brief number of equality constraints for time step k
+    /// @param k: time step
+    FatropOcpCGetDim_k get_ng;
+    /// @brief  number of stage parameters for time step k
+    /// @param k: time step
+    FatropOcpCGetDim_k get_n_stage_params;
+    /// @brief number of global parameters
+    FatropOcpCGetDim get_n_global_params;
+    /// @brief default stage parameters for time step k
+    /// @param stage_params: pointer to array of size n_stage_params_k
+    /// @param k: time step
+    FatropOcpCGetDouble_k get_default_stage_params;
+    /// @brief default global parameters
+    /// @param global_params: pointer to array of size n_global_params
+    FatropOcpCGetDouble get_default_global_params;
+    /// @brief number of inequality constraints for time step k
+    /// @param k: time step
+    FatropOcpCGetDim_k get_ng_ineq;
+    /// @brief horizon length
+    FatropOcpCGetDim get_horizon_length;
+    /// @brief  discretized dynamics
+    /// it evaluates the vertical concatenation of A_k^T, B_k^T, and b_k^T from the linearized dynamics x_{k+1} = A_k x_k + B_k u_k + b_k. 
+    /// The matrix is in column major format.
+    /// @param states_kp1: pointer to nx_{k+1}-array states of time step k+1
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to (nu+nx+1 x nu+nx)-matrix 
+    /// @param k: time step
+    FatropOcpCEval_BAbt_k eval_BAbt;
+    /// @brief  stagewise Lagrangian Hessian
+    /// It evaluates is the vertical concatenation of (1) the Hessian of the Lagrangian to the concatenation of (u_k, x_k) (2) the first order derivative of the Lagrangian Hessian to the concatenation of (u_k, x_k). 
+    /// The matrix is in column major format.
+    /// @param objective_scale: scale factor for objective function (usually 1.0)
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param lam_dyn_k: pointer to array dual variables for dynamics of time step k
+    /// @param lam_eq_k: pointer to array dual variables for equality constraints of time step k
+    /// @param lam_eq_ineq_k: pointer to array dual variables for inequality constraints of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to (nu+nx+1 x nu+nx)-matrix. 
+    /// @param k
+    /// @return
+    FatropOcpCEval_RSQrqt_k eval_RSQrqt;
+    /// @brief stagewise equality constraints Jacobian. 
+    /// It evaluates the vertical concatenation of (1) the Jacobian of the equality constraints to the concatenation of (u_k, x_k) (2) the equality constraints evaluated at u_k, x_k.
+    /// The matrix is in column major format.
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to (nu+nx+1 x ng)-matrix.
+    /// @param k: time step
+    /// @return
+    FatropOcpCEval_Ggt_k eval_Ggt;
+    /// @brief stagewise inequality constraints Jacobian. 
+    /// It evaluates the vertical concatenation of (1) the Jacobian of the inequality constraints to the concatenation of (u_k, x_k) (2) the inequality constraints evaluated at u_k, x_k. 
+    /// The matrix is in column major format.
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params_ko: pointer to array global parameters
+    /// @param res: pointer to (nu+nx+1 x ng_ineq)-matrix, column major format
+    /// @param k : time step
+    /// @return
+    FatropOcpCEval_Ggt_ineq_k eval_Ggt_ineq;
+    /// @brief the dynamics constraint violation (b_k = -x_{k+1} + f_k(u_k, x_k, p_k, p))
+    /// @param states_kp1: pointer to array states of time step k+1
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to array nx_{k+1}-vector
+    /// @param k: time step
+    /// @return
+    FatropOcpCEval_b_k eval_b;
+    /// @brief the equality constraint violation (g_k = g_k(u_k, x_k, p_k, p))
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to array ng-vector
+    /// @param k: time step
+    FatropOcpCEval_g_k eval_g;
+    /// @brief the inequality constraint violation (g_ineq_k = g_ineq_k(u_k, x_k, p_k, p))
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to array ng_ineq-vector
+    /// @param k: time step
+    FatropOcpCEval_gineq_k eval_gineq;
+    /// @brief gradient of the objective function (not the Lagrangian!) to the concatenation of (u_k, x_k)
+    /// @param objective_scale: pointer to objective scale
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to (nu+nx)-array
+    /// @param k: time step
+    FatropOcpCEval_rq_k eval_rq;
+    /// @brief objective function value 
+    /// @param objective_scale: pointer to array objective scale
+    /// @param inputs_k: pointer to array inputs of time step k
+    /// @param states_k: pointer to array states of time step k
+    /// @param stage_params_k: pointer to array stage parameters of time step k
+    /// @param global_params: pointer to array global parameters
+    /// @param res: pointer to double
+    /// @param k: time step
+    FatropOcpCEval_L_k eval_L;
+    /// @brief the bounds of the inequalites at stage k
+    /// @param lower: pointer to ng_ineq-vector
+    /// @param upper: pointer to ng_ineq-vector
+    /// @param k: time step
+    FatropOcpCGetBounds_k get_bounds;
+    /// @brief default initial guess for the states of stage k
+    /// @param xk: pointer to states of time step k 
+    /// @param k: time step
+    FatropOcpCGetDouble_k get_initial_xk;
+    /// @brief default initial guess for the inputs of stage k
+    /// @param uk: pointer to inputs of time step k
+    /// @param k: time step
+    FatropOcpCGetDouble_k get_initial_uk;
+
+    FatropOcpCFullEvalLagHess full_eval_lag_hess;
+    FatropOcpCFullEvalConstrJac full_eval_constr_jac;
+    FatropOcpCFullEvalContrViol full_eval_contr_viol;
+    FatropOcpCFullEvalObjGrad full_eval_obj_grad;
+    FatropOcpCFullEvalObj full_eval_obj;
+
+    void* user_data;
+};
+
+/* 
+    * @brief Create a new OCP solver
+    * @param ocp_interface: pointer to the OCP interface
+    * @param write: function pointer to write function (can be zero to indicate stdout)
+    * @param flush: function pointer to flush function (can be zero)
+    * @return pointer to the OCP solver
+    
+*/
+FATROP_SYMBOL_EXPORT struct FatropOcpCSolver* fatrop_ocp_c_create(struct FatropOcpCInterface* ocp_interface, FatropOcpCWrite write, FatropOcpCFlush flush);
+
+FATROP_SYMBOL_EXPORT fatrop_int fatrop_ocp_c_solve(struct FatropOcpCSolver*);
+
+/* -1 for not found, 0 for double, 1 for int, 2 for bool, 3 for string */
+FATROP_SYMBOL_EXPORT int fatrop_ocp_c_option_type(const char* name);
+FATROP_SYMBOL_EXPORT int fatrop_ocp_c_set_option_double(struct FatropOcpCSolver* s, const char* name, double val);
+FATROP_SYMBOL_EXPORT int fatrop_ocp_c_set_option_bool(struct FatropOcpCSolver* s, const char* name, int val);
+FATROP_SYMBOL_EXPORT int fatrop_ocp_c_set_option_int(struct FatropOcpCSolver* s, const char* name, int val);
+FATROP_SYMBOL_EXPORT int fatrop_ocp_c_set_option_string(struct FatropOcpCSolver* s, const char* name, const char* val);
+FATROP_SYMBOL_EXPORT const struct blasfeo_dvec* fatrop_ocp_c_get_primal(struct FatropOcpCSolver* s);
+FATROP_SYMBOL_EXPORT const struct blasfeo_dvec* fatrop_ocp_c_get_dual(struct FatropOcpCSolver* s);
+FATROP_SYMBOL_EXPORT const struct FatropOcpCDims* fatrop_ocp_c_get_dims(struct FatropOcpCSolver* s);
+FATROP_SYMBOL_EXPORT const struct FatropOcpCStats* fatrop_ocp_c_get_stats(struct FatropOcpCSolver* s);
+FATROP_SYMBOL_EXPORT void fatrop_ocp_c_destroy(struct FatropOcpCSolver*);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+
+#endif // OCPCINTERFACEINCLUDED

casadi/include/fatrop/ocp/OCPDims.hpp

@@ -0,0 +1,60 @@
+/*
+ * Fatrop - A fast trajectory optimization solver
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
+ *
+ * This file is part of Fatrop.
+ *
+ * Fatrop is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Fatrop is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
+#ifndef FATROP_OCPDIMS_INCLUDED
+#define FATROP_OCPDIMS_INCLUDED
+#include "fatrop/auxiliary/FatropVector.hpp"
+#include "fatrop/auxiliary/Common.hpp"
+#include "fatrop/templates/NLPAlg.hpp"
+#include <vector>
+namespace fatrop
+{
+    /** \brief  this class contains the problem dimensions of a standard ocp*/
+    struct OCPDims : public NLPDims
+    {
+    public:
+        OCPDims(const fatrop_int K, const FatropVector<fatrop_int> &nu, const FatropVector<fatrop_int> &nx, const FatropVector<fatrop_int> &ng, const FatropVector<fatrop_int> &ng_ineq, const FatropVector<fatrop_int> &n_stage_params, const fatrop_int n_global_parameters) : NLPDims({sum(nx + nu), sum(ng + ng_ineq + nx) - nx.get(0), sum(ng_ineq)}),
+                                                                                                                                                                                                                                                                                 K(K), nu(nu), nx(nx), ng(ng), ng_ineq(ng_ineq), n_stage_params(n_stage_params), n_global_params(n_global_parameters),
+                                                                                                                                                                                                                                                                                 n_u_tot(sum(nu)),
+                                                                                                                                                                                                                                                                                 n_x_tot(sum(nx)),
+                                                                                                                                                                                                                                                                                 n_b_tot(n_x_tot - nx.get(0)),
+                                                                                                                                                                                                                                                                                 n_g_tot(sum(ng)),
+                                                                                                                                                                                                                                                                                 n_g_ineq_tot(sum(ng_ineq)), n_stage_params_tot(sum(n_stage_params)){};
+        /// horizon length
+        const fatrop_int K;
+        /// input vector size
+        const FatropVector<fatrop_int> nu;
+        /// state vector size
+        const FatropVector<fatrop_int> nx;
+        // number of stagewise equality constraints
+        const FatropVector<fatrop_int> ng;
+        // number of stagewise inequality constraints
+        const FatropVector<fatrop_int> ng_ineq;
+        // number of stage parameters
+        const FatropVector<fatrop_int> n_stage_params;
+        // number of global parameters
+        const fatrop_int n_global_params;
+        const fatrop_int n_u_tot;
+        const fatrop_int n_x_tot;
+        const fatrop_int n_b_tot;
+        const fatrop_int n_g_tot;
+        const fatrop_int n_g_ineq_tot;
+        const fatrop_int n_stage_params_tot;
+    };
+} // namespace fatrop
+#endif // FATROP_OCPDIMS_INCLUDED

casadi/include/fatrop/ocp/OCPInitializer.hpp

@@ -0,0 +1,41 @@
+/*
+ * Fatrop - A fast trajectory optimization solver
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
+ *
+ * This file is part of Fatrop.
+ *
+ * Fatrop is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Fatrop is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
+#ifndef OCPINITIALIZERINCLUDED
+#define OCPINITIALIZERINCLUDED
+#include "OCPKKT.hpp"
+#include "fatrop/auxiliary/Common.hpp"
+#include "fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp"
+#define OCPMACRO(type, name, suffix) type name##suffix = ((type)OCP->name)
+#define AUXMACRO(type, name, suffix) type name##suffix = ((type)OCP->aux.name)
+#define SOLVERMACRO(type, name, suffix) type name##suffix = ((type)name)
+namespace fatrop
+{
+    class OCPInitializer
+    {
+    public:
+        /** \brief this method adapts KKT system for initialization, JAC and GRAD are assumed evaluated !! */
+        fatrop_int modify_kkt_ls_dual_estimate(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &grad);
+        fatrop_int intialize_slack_variables(
+            OCPKKTMemory *OCP,
+            FatropVecBF &s);
+    };
+} // namespace fatrop
+#endif //  OCPINITIALIZERINCLUDED

casadi/include/fatrop/ocp/OCPKKT.hpp

@@ -0,0 +1,69 @@
+/*
+ * Fatrop - A fast trajectory optimization solver
+ *  Copyright (C) 2022 - 2024 Lander Vanroye, KU Leuven. All rights reserved.
+ *
+ * This file is part of Fatrop.
+ *
+ * Fatrop is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Fatrop is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with Fatrop.  If not, see <http://www.gnu.org/licenses/>. */
+#ifndef FATROP_OCP_KKT_INCLUDED
+#define FATROP_OCP_KKT_INCLUDED
+#include "fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp"
+#include "OCPDims.hpp"
+#include "fatrop/auxiliary/VectorUtils.hpp"
+#include "fatrop/auxiliary/FatropVector.hpp"
+#include "fatrop/auxiliary/Common.hpp"
+namespace fatrop
+{
+    /** \brief this class contains all information to represent the KKT system of an equality constrained OCP */
+    class OCPKKTMemory
+    {
+    public:
+        OCPKKTMemory(const OCPDims &dims);
+        OCPKKTMemory(const OCPKKTMemory &cpy) = delete;
+        fatrop_int K;
+        FatropVector<fatrop_int> nu;
+        FatropVector<fatrop_int> nx;
+        FatropVector<fatrop_int> ng;
+        FatropVector<fatrop_int> ng_ineq;
+        /// small-scale Hessian
+        FatropMemoryMatBF RSQrqt;
+        /// small-scale Jacobian dynamics
+        FatropMemoryMatBF BAbt;
+        /// small-scale Jacobian stagewise eq constraints
+        FatropMemoryMatBF Ggt;
+        /// small-scale Jacobian stagewise ineq constraints
+        FatropMemoryMatBF Ggt_ineq;
+        class OCPAux
+        {
+        public:
+            OCPAux(const OCPDims &dims);
+            /// offset arrays are used for efficiency
+            const FatropVector<fatrop_int> ux_offs;
+            /// offset arrays are used for efficiency
+            const FatropVector<fatrop_int> g_offs;
+            const FatropVector<fatrop_int> dyn_offs;
+            const FatropVector<fatrop_int> dyn_eq_offs;
+            const FatropVector<fatrop_int> g_ineq_offs;
+            const FatropVector<fatrop_int> ineq_offs;
+            fatrop_int max_nu;
+            fatrop_int max_nx;
+            fatrop_int max_ng;
+            fatrop_int max_ngineq;
+            fatrop_int n_ineqs;
+        };
+        OCPAux aux;
+    };
+
+} // namespace fatrop
+#endif // FATROP_OCP_KKT_INCLUDED