casadi 3.6.5__cp36-none-manylinux2014_aarch64.whl → 3.6.7__cp36-none-manylinux2014_aarch64.whl

casadi/include/fatrop/ocp/StageOCPApplication.hpp

@@ -0,0 +1,242 @@
+/*
+ * 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 BASICOCPAPPLICATIONINCLUDED
+#define BASICOCPAPPLICATIONINCLUDED
+#include "fatrop/solver/FatropStats.hpp"
+#include "fatrop/solver/FatropAlg.hpp"
+#include "fatrop/ocp/StageOCPExpressions.hpp"
+#include <map>
+#include <fstream>
+#include <sstream>
+#include <string>
+
+namespace fatrop
+{
+    // forward declarations to hide the implementation details
+    class Journaller;
+    class FatropAlg;
+    class StageOCP;
+    class NLPDims;
+    class FatropVecBF;
+    class FatropNLP;
+    class FatropOptions;
+    class FatropData;
+    class OCPAdapter;
+    class FatropPrinter;
+    class OCPAbstract;
+    class OCPDims;
+    class OCP;
+    class FatropSolution
+    {
+    public:
+        // void GetPrimalSolution(vector<double> &result);
+        // defautl copy constructor
+        FatropSolution(const FatropSolution &other) = default;
+        const std::vector<double> &primal_solution() { return sol_primal_; };
+
+    protected:
+        FatropSolution();
+        void set_dims(const NLPDims &dims);
+        void set_primal_solution(const FatropVecBF &sol);
+        void set_solution(const FatropVecBF &sol_primal, const FatropVecBF &sol_dual, const FatropVecBF &sol_zL, const FatropVecBF &sol_zU);
+
+    protected:
+        std::vector<double> sol_primal_;
+        std::vector<double> sol_dual_;
+        std::vector<double> sol_zL_;
+        std::vector<double> sol_zU_;
+        friend class NLPApplication;
+    };
+    // TODO move this class to a separate file
+    class NLPApplication
+    {
+    public:
+        NLPApplication();
+
+    protected:
+        void build(const std::shared_ptr<FatropNLP> &nlp, const std::shared_ptr<FatropNLP> &nlp_resto);
+
+    public:
+        fatrop_int optimize() const;
+        // TODO: make this protected and use last_solution instead and choose other name
+        const FatropVecBF &last_solution_primal() const;
+        const FatropVecBF &last_solution_dual() const;
+        const FatropVecBF &last_solution_zL() const;
+        const FatropVecBF &last_solution_zU() const;
+        FatropVecBF &initial_guess_primal() const;
+        FatropVecBF &initial_guess_dual() const;
+        FatropVecBF &initial_guess_zL() const;
+        FatropVecBF &initial_guess_zU() const;
+        FatropStats get_stats() const;
+        NLPDims get_nlp_dims();
+        template <typename T>
+        void set_option(const std::string &option_name, T value);
+
+    public:
+        void set_initial(const FatropSolution &initial_guess) const;
+        void set_initial(const std::vector<double> &initial_guess_primal_) const;
+        const FatropOptions &get_options() const;
+
+    protected:
+        std::shared_ptr<FatropOptions> fatropoptions_;
+        std::shared_ptr<FatropData> fatropdata_;
+        std::shared_ptr<FatropNLP> nlp_;
+        bool dirty = true;
+        std::shared_ptr<FatropPrinter> printer_;
+        friend class OcpSolverDriver;
+        
+    private:
+        const std::shared_ptr<OCPAbstract> ocp_;
+        std::shared_ptr<Journaller> journaller_;
+        std::shared_ptr<FatropAlg> fatropalg_;
+    };
+
+    class OCPApplication : public NLPApplication
+    {
+    public:
+        OCPApplication(const std::shared_ptr<OCP> &ocp);
+        OCPApplication();
+        void build();
+    public:
+        using NLPApplication::set_initial;
+        void set_initial(std::vector<double> &initial_u, std::vector<double> &initial_x);
+        OCPDims get_ocp_dims();
+        std::shared_ptr<OCP> ocp_;
+    };
+
+    // TODO move this class to a separate file
+    class OCPAbstractApplication : public OCPApplication
+    {
+    public:
+        OCPAbstractApplication(const std::shared_ptr<OCPAbstract> &ocp);
+        void set_params(const std::vector<double> &global_params, const std::vector<double> &stage_params);
+
+    protected:
+        std::shared_ptr<OCPAdapter> adapter;
+
+    protected:
+        std::vector<double> &global_parameters();
+        std::vector<double> &stage_parameters();
+    };
+
+    struct StageOCPSolution : public FatropSolution
+    {
+    public:
+        StageOCPSolution(const std::shared_ptr<OCP> &app);
+        void get_u(std::vector<double> &result) const;
+        void get_x(std::vector<double> &result) const;
+        int get_nx() const
+        {
+            return nx;
+        };
+        int get_nu() const
+        {
+            return nu;
+        };
+        int get_K() const
+        {
+            return K;
+        };
+
+    protected:
+        StageOCPSolution();
+        void set_dims(const OCPDims &dims);
+        void set_parameters(const std::vector<double> &global_params, const std::vector<double> &stage_params);
+        fatrop_int nx;
+        fatrop_int nu;
+        fatrop_int n_stage_params;
+        fatrop_int n_global_params;
+        fatrop_int K;
+
+    public:
+        // todo make this deprecated, only use Eval
+        void sample(const StageControlGridSampler &sampler, std::vector<double> &result) const;
+        std::vector<double> evaluate(const StageExpressionEvaluatorBase &evaluator) const;
+        void evaluate(const StageExpressionEvaluatorBase &evaluator, std::vector<double> &result) const;
+
+    protected:
+        std::vector<double> global_params;
+        std::vector<double> stage_params;
+        friend class StageOCPApplication;
+    };
+
+    // struct StageOCPApplicationAbstract : public OCPApplication
+    // {
+    //     StageOCPApplicationAbstract(const shared_ptr<StageOCP> &ocp);
+    // };
+    class StageOCPApplication : public OCPAbstractApplication
+    {
+    public:
+        StageOCPApplication(const std::shared_ptr<StageOCP> &ocp);
+
+        void set_initial_u(const std::vector<double> &initial_guess_u) const;
+        void set_initial_x(const std::vector<double> &initial_guess_x) const;
+
+    public:
+        class AppParameterSetter : public ParameterSetter
+        {
+        public:
+            AppParameterSetter(const std::shared_ptr<OCPAdapter> &adapter, const std::shared_ptr<ParameterSetter> &ps);
+
+        public:
+            void set_value(const double value[]);
+            void set_value(const std::initializer_list<double> il_);
+            void set_value(const std::vector<double> il_)
+            {
+                set_value(il_.data());
+            };
+
+        private:
+            const std::shared_ptr<OCPAdapter> adapter_;
+        };
+
+    public:
+        StageExpressionEvaluatorFactory get_expression(const std::string &sampler_name);
+        StageExpressionEvaluatorFactory get_expression_evaluator(const std::shared_ptr<StageExpression> &expr);
+        AppParameterSetter get_parameter_setter(const std::string &setter_name);
+        void set_value(const std::string &setter_name, const std::vector<double>& value);
+        void build();
+        fatrop_int optimize();
+        const StageOCPSolution &last_solution() const;
+        // const FatropSolution &last_solution() const { return last_solution(); };
+        const std::vector<std::string> parameter_names() const;
+        const std::vector<std::string> stage_expression_names() const;
+
+    public:
+        const fatrop_int nx_;
+        const fatrop_int nu_;
+        const fatrop_int n_stage_params_;
+        const fatrop_int K_;
+
+    private:
+        StageOCPSolution last_solution_;
+
+    protected:
+        std::map<std::string, std::shared_ptr<StageExpression>> stage_expressions;
+        std::map<std::string, std::shared_ptr<ParameterSetter>> param_setters;
+        friend class StageOCPApplicationFactory;
+    };
+
+    class StageOCPApplicationFactory
+    {
+    public:
+        static StageOCPApplication from_rockit_interface(const std::string &functions, const std::string &json_spec_file);
+    };
+}
+#endif // BASICOCPAPPLICATIONINCLUDED

casadi/include/fatrop/ocp/StageOCPExpressions.hpp

@@ -0,0 +1,182 @@
+/*
+ * 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 BASICOCPSAMPLERSINCLUDED
+#define BASICOCPSAMPLERSINCLUDED
+#include <memory>
+#include <vector>
+#include <fatrop/auxiliary/Common.hpp>
+// todo change the name of this file to evaluators
+namespace fatrop
+{
+    class EvalBase;
+    class StageExpression
+    {
+    public:
+        virtual void evaluate(const double *u, const double *x, const double *global_params, const double *stage_params, double *res) const = 0;
+        virtual fatrop_int n_rows() const = 0;
+        virtual fatrop_int n_cols() const= 0;
+        fatrop_int size() const;
+    };
+    class IndexEpression : public StageExpression
+    {
+    public:
+        IndexEpression(const bool control, const std::vector<fatrop_int> offsets_in, const std::vector<fatrop_int> offsets_out);
+        void evaluate(const double *u, const double *x, const double *global_params, const double *stage_params, double *res) const override;
+        fatrop_int n_rows() const override;
+        fatrop_int n_cols() const override;
+
+    private:
+        const fatrop_int _no_var;
+        const std::vector<fatrop_int> _offsets_in;
+        const std::vector<fatrop_int> _offsets_out;
+        const bool _control;
+    };
+    class EvalBaseSE : public StageExpression
+    {
+    public:
+        EvalBaseSE(const std::shared_ptr<EvalBase> &evalbase);
+        void evaluate(const double *u, const double *x, const double *global_params, const double *stage_params, double *res) const override;
+        fatrop_int n_rows() const override;
+        fatrop_int n_cols() const override;
+
+    private:
+        std::shared_ptr<EvalBase> evalbase_;
+        const fatrop_int n_rows_;
+        const fatrop_int n_cols_;
+    };
+    class StageExpressionEvaluatorBase
+    {
+    public:
+        virtual fatrop_int size() const = 0;
+        virtual fatrop_int n_rows() const= 0;
+        virtual fatrop_int n_cols() const = 0;
+        virtual fatrop_int evaluate(const std::vector<double> &solution, const std::vector<double> &global_params, const std::vector<double> &stage_params, std::vector<double> &result) const = 0;
+        std::vector<double> evaluate(const std::vector<double> &solution, const std::vector<double> &global_params, const std::vector<double> &stage_params) const
+        {
+            std::vector<double> res(size());
+            evaluate(solution, global_params, stage_params, res);
+            return res;
+        };
+        virtual ~StageExpressionEvaluatorBase() = default;
+
+    protected:
+        // virtual fatrop_int Evaluate()
+    };
+    class StageControlGridSampler : public StageExpressionEvaluatorBase
+    {
+    public:
+        StageControlGridSampler(fatrop_int nu, fatrop_int nx, fatrop_int no_stage_params, fatrop_int K, const std::shared_ptr<StageExpression> &eval);
+        fatrop_int evaluate(const std::vector<double> &solution, const std::vector<double> &global_params, const std::vector<double> &stage_params, std::vector<double> &result) const;
+        fatrop_int size() const;
+        fatrop_int n_rows() const;
+        fatrop_int n_cols() const;
+        fatrop_int K() const;
+        const fatrop_int nu;
+        const fatrop_int nx;
+        const fatrop_int no_stage_params;
+        const fatrop_int K_;
+
+    private:
+        std::shared_ptr<StageExpression> eval_;
+    };
+
+    class OCPTimeStepSampler : public StageExpressionEvaluatorBase
+    {
+    public:
+        OCPTimeStepSampler(fatrop_int nu, fatrop_int nx, fatrop_int no_stage_params, fatrop_int K, fatrop_int k, const std::shared_ptr<StageExpression> &eval) : nu(nu), nx(nx), no_stage_params(no_stage_params), K_(K), k_(k), eval_(eval)
+        {
+        }
+        fatrop_int evaluate(const std::vector<double> &solution, const std::vector<double> &global_params, const std::vector<double> &stage_params, std::vector<double> &result) const
+        {
+            eval_->evaluate(solution.data() + (k_ < K_ - 1 ? k_ * (nu + nx) : (K_ - 2) * (nu + nx)), solution.data() + (k_ < K_ - 1 ? k_ * (nu + nx) + nu : (K_ - 1) * (nu + nx)), global_params.data(), stage_params.data() + k_ * no_stage_params, result.data());
+            return 0;
+        }
+        fatrop_int size() const
+        {
+            return eval_->size();
+        }
+        fatrop_int n_rows() const
+        {
+            return eval_->n_rows();
+        }
+        fatrop_int n_cols() const
+        {
+            return eval_->n_cols();
+        }
+        const fatrop_int nu;
+        const fatrop_int nx;
+        const fatrop_int no_stage_params;
+        const fatrop_int K_;
+        const fatrop_int k_;
+
+    private:
+        std::shared_ptr<StageExpression> eval_;
+    };
+    class StageExpressionEvaluatorFactory
+    {
+    public:
+        StageExpressionEvaluatorFactory(const std::shared_ptr<StageExpression> &eval, fatrop_int nu, fatrop_int nx, fatrop_int no_stage_params, fatrop_int K) : nu(nu), nx(nx), no_stage_params(no_stage_params), K(K), eval_(eval){};
+        // at_t0()
+        OCPTimeStepSampler at_t0() const
+        {
+            return OCPTimeStepSampler(nu, nx, no_stage_params, K, 0, eval_);
+        }
+        // at_tf()
+        OCPTimeStepSampler at_tf() const
+        {
+            return OCPTimeStepSampler(nu, nx, no_stage_params, K, K - 1, eval_);
+        }
+        // at_tk(fatrop_int k)
+        OCPTimeStepSampler at_tk(const fatrop_int k) const
+        {
+            return OCPTimeStepSampler(nu, nx, no_stage_params, K, k, eval_);
+        }
+        // evaluate at control grid
+        StageControlGridSampler at_control() const
+        {
+            return StageControlGridSampler(nu, nx, no_stage_params, K, eval_);
+        }
+        const fatrop_int nu;
+        const fatrop_int nx;
+        const fatrop_int no_stage_params;
+        const fatrop_int K;
+        std::shared_ptr<StageExpression> eval_;
+    };
+
+    class ParameterSetter
+    {
+    public:
+        ParameterSetter(const std::vector<fatrop_int> &offsets_in, const std::vector<fatrop_int> &offsets_out, const fatrop_int no_stage_params, const fatrop_int no_var, const fatrop_int K, const bool global);
+        void set_value(std::vector<double> &global_params, std::vector<double> &stage_params, const double value[]);
+        void set_value(std::vector<double> &global_params, std::vector<double> &stage_params, const std::initializer_list<double> il_);
+
+    private:
+        const std::vector<fatrop_int> _offsets_in;
+        const std::vector<fatrop_int> _offsets_out;
+        const fatrop_int no_stage_params;
+
+    protected:
+        const fatrop_int _no_var;
+        const fatrop_int K;
+        const bool _global;
+    };
+
+} // namespace fatrop
+
+#endif //  BASICOCPSAMPLERSINCLUDED

casadi/include/fatrop/ocp/UStageEvalAbstract.hpp

@@ -0,0 +1,168 @@
+#pragma once
+extern "C"
+{
+#include <blasfeo.h>
+}
+#define MAT blasfeo_dmat
+namespace fatrop
+{
+        class UStageEvalAbstract
+        {
+        public:
+            /// @brief number of states for time step k
+            /// @param k: time step
+            virtual int get_nx() const = 0;
+            /// @brief number of inputs for time step k
+            /// @param k: time step
+            virtual int get_nu() const = 0;
+            /// @brief number of equality constraints for time step k
+            /// @param k: time step
+            virtual int get_ng() const = 0;
+            /// @brief  number of stage parameters for time step k
+            /// @param k: time step
+            virtual int get_n_stage_params() const = 0;
+            /// @brief default global parameters
+            /// @param global_params: pointer to array of size n_global_params
+            virtual int get_ng_ineq() const = 0;
+            /// @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
+            virtual int eval_BAbt(
+                const double *states_kp1,
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                MAT *res) = 0;
+            /// 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
+            virtual int eval_RSQrqt(
+                const double *objective_scale,
+                const double *inputs_k,
+                const double *states_k,
+                const double *lam_dyn_k,
+                const double *lam_eq_k,
+                const double *lam_eq_ineq_k,
+                const double *stage_params_k,
+                const double *global_params,
+                MAT *res) = 0;
+            /// @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.
+            /// @return
+            virtual int eval_Ggt(
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                MAT *res) = 0;
+            /// @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
+            virtual int eval_Ggt_ineq(
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                MAT *res) = 0;
+            /// @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
+            /// @return
+            virtual int eval_b(
+                const double *states_kp1,
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                double *res) = 0;
+            /// @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
+            virtual int eval_g(
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                double *res) = 0;
+            /// @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
+            virtual int eval_gineq(
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                double *res) = 0;
+            /// @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
+            virtual int eval_rq(
+                const double *objective_scale,
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                double *res) = 0;
+            /// @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
+            virtual int eval_L(
+                const double *objective_scale,
+                const double *inputs_k,
+                const double *states_k,
+                const double *stage_params_k,
+                const double *global_params,
+                double *res) = 0;
+            /// @brief the bounds of the inequalites at stage k
+            /// @param lower: pointer to ng_ineq-vector
+            /// @param upper: pointer to ng_ineq-vector
+            virtual int get_bounds(double *lower, double *upper) const = 0;
+            /// @brief default initial guess for the states of stage k
+            /// @param xk: pointer to states of time step k
+        };
+}