casadi 3.6.5__cp39-none-macosx_11_0_arm64.whl → 3.6.7__cp39-none-macosx_11_0_arm64.whl

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