casadi 3.6.5__cp312-none-macosx_11_0_arm64.whl → 3.6.6__cp312-none-macosx_11_0_arm64.whl

casadi/include/fatrop/ocp/OCPLSRiccati.hpp

@@ -0,0 +1,191 @@
+/*
+ * 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 OCPLSRICCATIINCLUDED
+#define OCPLSRICCATIINCLUDED
+#include "OCPKKT.hpp"
+#include "OCPLinearSolver.hpp"
+#include "fatrop/solver/FatropPrinter.hpp"
+#include <cmath>
+#include <memory>
+#include "fatrop/solver/FatropOptions.hpp"
+#include "fatrop/auxiliary/Common.hpp"
+#define SUMMATION_ALG kahan_sum
+namespace fatrop
+{
+    bool check_reg(const fatrop_int m, MAT *sA, const fatrop_int ai, const fatrop_int aj);
+    class OCPLSRiccati : public OCPLinearSolver
+    {
+    public:
+        OCPLSRiccati(const OCPDims &dims, const std::shared_ptr<FatropOptions> &options, const std::shared_ptr<FatropPrinter> &printer);
+            // solve a KKT system
+            fatrop_int solve_pd_sys(
+                OCPKKTMemory *OCP,
+                const double inertia_correction_w,
+                const double inertia_correction_c,
+                const FatropVecBF &ux,
+                const FatropVecBF &lam,
+                const FatropVecBF &delta_s,
+                const FatropVecBF &sigma_total,
+                const FatropVecBF &gradb_total) override;
+        // solve a KKT system
+        fatrop_int solve_pd_sys_degenerate(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &gradb_total);
+        // solve a KKT system
+        int
+        solve_pd_sys_normal(
+            OCPKKTMemory *OCP,
+            const double inertia_correction,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &gradb_total);
+        fatrop_int get_rhs(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &gradb_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb) override;
+        fatrop_int compute_pd_sys_times_vec(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb) override;
+        fatrop_int solve_rhs(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb) override;
+        fatrop_int solve_rhs_normal(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb) ;
+        fatrop_int solve_rhs_degenerate(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb);
+        FatropMemoryMatBF Ppt;
+        FatropMemoryMatBF Hh;
+        FatropMemoryMatBF AL;
+        FatropMemoryMatBF RSQrqt_tilde;
+        FatropMemoryMatBF Ggt_stripe;
+        FatropMemoryMatBF Ggt_tilde;
+        FatropMemoryMatBF GgLt;
+        FatropMemoryMatBF RSQrqt_hat;
+        FatropMemoryMatBF Llt;
+        FatropMemoryMatBF Llt_shift; // needed because feature not implemented yet
+        FatropMemoryMatBF GgIt_tilde;
+        FatropMemoryMatBF GgLIt;
+        FatropMemoryMatBF HhIt;
+        FatropMemoryMatBF PpIt_hat;
+        FatropMemoryMatBF LlIt;
+        FatropMemoryMatBF Ggt_ineq_temp;
+        FatropMemoryVecBF rhs_rq;
+        FatropMemoryVecBF rhs_b;
+        FatropMemoryVecBF rhs_g;
+        FatropMemoryVecBF rhs_g_ineq;
+        FatropMemoryVecBF rhs_gradb;
+        FatropMemoryVecBF rhs_rq2;
+        FatropMemoryVecBF rhs_b2;
+        FatropMemoryVecBF rhs_g2;
+        FatropMemoryVecBF rhs_g_ineq2;
+        FatropMemoryVecBF rhs_gradb2;
+        FatropMemoryVecBF ux_test;
+        FatropMemoryVecBF lam_test;
+        FatropMemoryVecBF delta_s_test;
+        FatropMemoryVecBF v_Ppt;
+        FatropMemoryVecBF v_Hh;
+        FatropMemoryVecBF v_AL;
+        FatropMemoryVecBF v_RSQrqt_tilde;
+        FatropMemoryVecBF v_Ggt_stripe;
+        FatropMemoryVecBF v_Ggt_tilde;
+        FatropMemoryVecBF v_GgLt;
+        FatropMemoryVecBF v_RSQrqt_hat;
+        FatropMemoryVecBF v_Llt;
+        FatropMemoryVecBF v_Llt_shift;
+        FatropMemoryVecBF v_GgIt_tilde;
+        FatropMemoryVecBF v_GgLIt;
+        FatropMemoryVecBF v_HhIt;
+        FatropMemoryVecBF v_PpIt_hat;
+        FatropMemoryVecBF v_LlIt;
+        FatropMemoryVecBF v_Ggt_ineq_temp;
+        MemoryPermMat Pl;
+        MemoryPermMat Pr;
+        MemoryPermMat PlI;
+        MemoryPermMat PrI;
+        FatropVector<fatrop_int> gamma;
+        FatropVector<fatrop_int> rho;
+        fatrop_int rankI = 0;
+        // struct LastUsed
+        // {
+        //     fatrop_int rankI = 0;
+        //     double inertia_correction_w = 0;
+        //     double inertia_correction_c = 0;
+        //     double kappa_d = 0;
+        //     double mu = 0;
+        // } lastused_;
+        std::shared_ptr<FatropOptions> options_;
+        std::shared_ptr<FatropPrinter> printer_;
+        bool it_ref = true;
+    };
+};     // namespace
+#endif // OCPLSRICCATIINCLUDED

casadi/include/fatrop/ocp/OCPLSScaler.hpp

@@ -0,0 +1,66 @@
+#ifndef OCPLSSCALER_HPP
+#define OCPLSSCALER_HPP
+#include "OCPLSRiccati.hpp"
+#include <vector>
+#include <algorithm>
+namespace fatrop
+{
+    class OCPLSScaler
+    {
+    public:
+        OCPLSScaler(const OCPDims &dims) : K_(dims.K), ng_(dims.ng), nu_(dims.nu), nx_(dims.nx), ng_tot_(dims.n_g_tot), scales_(ng_tot_){};
+        void scale_kkt(OCPKKTMemory &kkt)
+        {
+            MAT *Ggt_p = (MAT *)kkt.Ggt[0];
+            int offs = 0;
+            for (int k = 0; k < K_; k++)
+            {
+                for (int i = 0; i < ng_[k]; i++)
+                {
+                    double scale = 1.0/(std::max)(inf_col(nu_[k] + nx_[k], Ggt_p + k, 0, i), 1e-8); // 1e-8 is a small number to avoid division by zero
+                    if (scale > 1e-3 && scale < 1e3) scale = 1.0; // if the scale is not very small or large, we do not scale
+                    if(scale != 1.0) COLSC(nu_[k] + nx_[k] + 1,  scale, Ggt_p + k, 0, i);
+                    VECEL((VEC*)scales_, offs) = scale;
+                    offs++;
+                }
+            }
+        }
+        void scale_lam(const FatropVecBF &lam, const int ai)
+        {
+            VECMUL(ng_tot_, (VEC*)scales_, 0, (VEC*) lam, ai, (VEC*) lam, ai);
+        }
+        void restore_kkt(OCPKKTMemory &kkt)
+        {
+            MAT *Ggt_p = (MAT *)kkt.Ggt[0];
+            int offs = 0;
+            for (int k = 0; k < K_; k++)
+            {
+                for (int i = 0; i < ng_[k]; i++)
+                {
+                    double scale = VECEL((VEC*)scales_, offs);
+                    if (scale != 1.0) COLSC(nu_[k] + nx_[k] + 1, 1.0/scale, Ggt_p + k, 0, i);
+                    offs++;
+                }
+            }
+        }
+
+    private:
+        static double inf_col(const int kmax, MAT *A, const int ai, const int aj)
+        {
+            double res = 0.0;
+            for (int i = 0; i < kmax; i++)
+            {
+                res = (std::max)(res, std::abs(MATEL(A, ai + i, aj)));
+            }
+            return res;
+        }
+        const int K_;
+        std::vector<int> ng_;
+        std::vector<int> nu_;
+        std::vector<int> nx_;
+        const int ng_tot_;
+        VECBF scales_;
+    };
+
+} // namespace fatrop
+#endif

casadi/include/fatrop/ocp/OCPLinearSolver.hpp

@@ -0,0 +1,75 @@
+/*
+ * 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 NLPLSINCLUDED
+#define NLPLSINCLUDED
+#include "fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp"
+#include "OCPKKT.hpp"
+#include "fatrop/auxiliary/Common.hpp"
+namespace fatrop
+{
+    class OCPLinearSolver
+    {
+    public:
+        virtual fatrop_int solve_pd_sys(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &gradb_total) = 0;
+        virtual fatrop_int compute_pd_sys_times_vec(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb_total) = 0;
+        virtual fatrop_int solve_rhs(
+            OCPKKTMemory *OCP,
+            const double inertia_correction_w,
+            const double inertia_correction_c,
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &sigma_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb_total) = 0;
+        virtual fatrop_int get_rhs(
+            OCPKKTMemory *OCP,
+            const FatropVecBF &gradb_total,
+            const FatropVecBF &rhs_rq,
+            const FatropVecBF &rhs_b,
+            const FatropVecBF &rhs_g,
+            const FatropVecBF &rhs_g_ineq,
+            const FatropVecBF &rhs_gradb) = 0;
+    };
+
+} // namespace fatrop
+#endif //  NLPLSINCLUDED

casadi/include/fatrop/ocp/OCPNoScaling.hpp

@@ -0,0 +1,42 @@
+/*
+ * 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 NOSCALINGMETHODINCLUDED
+#define NOSCALINGMETHODINCLUDED
+#include "fatrop/solver/FatropData.hpp"
+#include "fatrop/templates/NLPAlg.hpp"
+#include "fatrop/solver/AlgStrategy.hpp"
+#include "OCPScalingMethod.hpp"
+#include "fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp"
+#include <memory>
+#include "fatrop/auxiliary/Common.hpp"
+namespace fatrop
+{
+    class OCPNoScaling : public OCPScalingMethod
+    {
+    public:
+        OCPNoScaling(const std::shared_ptr<FatropOptions> &fatrop_params);
+        virtual fatrop_int compute_scalings(
+            OCPKKTMemory *OCP,
+            double &obj_scale,
+            FatropVecBF &x_scales,
+            FatropVecBF &lam_scales, const FatropVecBF &grad_curr);
+    };
+
+} // namespace fatrop
+#endif // !SCALINGMETHODINCLUDED

casadi/include/fatrop/ocp/OCPScalingMethod.hpp

@@ -0,0 +1,42 @@
+/*
+ * 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 SCALINGMETHODINCLUDED
+#define SCALINGMETHODINCLUDED
+#include "fatrop/solver/FatropData.hpp"
+#include "fatrop/templates/NLPAlg.hpp"
+#include "fatrop/solver/AlgStrategy.hpp"
+#include "fatrop/ocp/OCPKKT.hpp"
+#include <memory>
+#include "fatrop/auxiliary/Common.hpp"
+namespace fatrop
+{
+    class OCPScalingMethod : public AlgStrategy // this is an OCP strategy
+    {
+    public:
+        OCPScalingMethod(const std::shared_ptr<FatropOptions> &fatrop_params) : AlgStrategy(fatrop_params){};
+        virtual fatrop_int compute_scalings(
+            OCPKKTMemory *OCP,
+            double &obj_scale,
+            FatropVecBF &x_scales,
+            FatropVecBF &lam_scales,
+            const FatropVecBF &grad_curr) = 0;
+    };
+
+} // namespace fatrop
+#endif // !SCALINGMETHODINCLUDED