casadi 3.6.6__cp27-none-win_amd64.whl → 3.6.7__cp27-none-win_amd64.whl

casadi/include/fatrop/auxiliary/Common.hpp

@@ -29,5 +29,6 @@
 namespace fatrop
 {
     bool CompareLessEqual(double lhs, double rhs);
+    bool CompareLessEqual(double lhs, double rhs, double ref);
 } // namespace fatrop
 #endif //  COMMONINCLUDED

casadi/include/fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp

@@ -326,7 +326,7 @@ namespace fatrop
             blasfeo_dvecse(nels(), val, vec_, offset());
         }
 
-        void copy(const FatropVecBF &fm);
+        void copy(const FatropVecBF &fm) const;
         void copyto(std::vector<double> &dest) const;
         void operator=(const std::vector<double> &fm);
         /** \brief set data pointer*/
@@ -381,6 +381,7 @@ namespace fatrop
     double LinfScaled(const FatropVecBF &va, const FatropVecBF &scales);
     double minabs(const FatropVecBF &va);
     double L1(const FatropVecBF &va);
+    double sumsqr(const FatropVecBF &va);
 
     /** \brief this class is used for the allocation of a blasfeo vector, the dimsensions are set from a vector */
     class FatropMemoryVecBF

casadi/include/fatrop/fatrop.hpp

@@ -0,0 +1,39 @@
+/*
+ * 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_INCLUDED
+#define FATROP_INCLUDED
+#include "fatrop/ocp/StageOCPApplication.hpp"
+// #include "fatrop/blasfeo_wrapper/LinearAlgebraBlasfeo.hpp"
+// #include "fatrop/ocp/OCPKKT.hpp"
+// #include "fatrop/ocp/OCPAdapter.hpp"
+// #include "fatrop/ocp/OCPAbstact.hpp"
+// #include "fatrop/auxiliary/FatropVector.hpp"
+// #include "fatrop/solver/FatropAlg.hpp"
+// #include "fatrop/solver/FatropData.hpp"
+// #include "fatrop/ocp/OCPScalingMethod.hpp"
+// #include "fatrop/ocp/OCPNoScaling.hpp"
+// #include "fatrop/solver/AlgStrategy.hpp"
+// #include "fatrop/solver/FatropOptions.hpp"
+// #include "fatrop/function_evaluation/CasadiCodegen.hpp"
+// #include "fatrop/solver/AlgBuilder.hpp"
+// #include "fatrop/ocp/StageOCPApplication.hpp"
+// #include "fatrop/ocp/FatropOCPBuilder.hpp"
+
+// #include "SparseSolvers/InterfaceMUMPS.hpp"
+#endif //FATROP_INCLUDED

casadi/include/fatrop/ocp/FatropOCPResto.hpp

@@ -0,0 +1,299 @@
+#pragma once
+#include "FatropOCP.hpp"
+#include "fatrop/solver/FatropOptions.hpp"
+#include <memory>
+namespace fatrop
+{
+    class FatropOCPResto : public FatropNLP
+    {
+    public:
+        FatropOCPResto(const std::shared_ptr<FatropOCP> &orig, const std::shared_ptr<FatropOptions> &opts) : orig_(orig), orig_dims_(orig->get_nlp_dims()), lower_(orig_dims_.nineqs), upper_(orig_dims_.nineqs), x_start_(orig_dims_.nvars), s_start_(orig_dims_.nineqs), x_tmp_(orig_dims_.nvars), s_tmp_(orig_dims_.nineqs), upper_bounded_(orig_dims_.nineqs), lower_bounded_(orig_dims_.nineqs), slack_dummy_(orig_dims_.nineqs), sigma_dummy_(orig_dims_.nineqs), gradb_dummy_(orig_dims_.nineqs), zl_dummy_(orig_dims_.nineqs), zu_dummy_(orig_dims_.nineqs), sigma_cache_(orig_dims_.nineqs * 3), gradb_cache_(orig_dims_.nineqs * 3)
+        {
+            opts->register_option(DoubleOption::lower_bounded("resto_rho", "Resto L1 penalty parameter", &rho, 1000., 0.0));
+            opts->register_option(DoubleOption::lower_bounded("resto_xi", "Resto xi parameter", &xi, 1., 0.0));
+            auto lower_v = lower_[0];
+            auto upper_v = upper_[0];
+            orig_->get_bounds(lower_v, upper_v);
+            n_p = orig_dims_.nineqs;
+            n_n = orig_dims_.nineqs;
+            for (fatrop_int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                bool lower_bounded = !std::isinf(lower_v.at(i));
+                bool upper_bounded = !std::isinf(upper_v.at(i));
+                upper_bounded_[i] = upper_bounded;
+                lower_bounded_[i] = lower_bounded;
+            }
+            this_dims_.nvars = orig_dims_.nvars;
+            this_dims_.nineqs = orig_dims_.nineqs + n_n + n_p;
+            this_dims_.neqs = orig_dims_.neqs;
+        }
+        virtual void pre_solve(const FatropVecBF &x_init, const FatropVecBF &s_init) override
+        {
+            x_start_[0].copy(x_init);
+            s_start_[0].copy(s_init.block(0, orig_dims_.nineqs));
+        };
+        virtual fatrop_int eval_lag_hess(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            const FatropVecBF &lam) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_lag_hess(0.0, primal_vars, slack_dummy_v, lam);
+            // compute delta's
+            axpby(xi, primal_vars, -xi, x_start_[0], x_tmp_[0]);
+            axpby(xi, slack_vars.block(0, orig_dims_.nineqs), -xi, s_start_[0], s_tmp_[0]);
+            // iterate over all RSQrq's
+
+            {
+                OCPKKTMemory* OCP =&this->orig_->ocpkktmemory_;
+                // horizon length
+                fatrop_int K = OCP->K;
+                // offsets
+                const fatrop_int *offs_ux = (const fatrop_int *)OCP->aux.ux_offs.data();
+                OCPMACRO(MAT *, RSQrqt, _p);
+                VEC* x_tmp_p = (VEC*) x_tmp_[0];
+                OCPMACRO(fatrop_int *, nu, _p);
+                OCPMACRO(fatrop_int *, nx, _p);
+                for (fatrop_int k = 0; k < K; k++)
+                {
+                    fatrop_int nu_k = nu_p[k];
+                    fatrop_int nx_k = nx_p[k];
+                    fatrop_int offs_ux_k = offs_ux[k];
+                    // add xi to RSQrq diagonal
+                    DIARE(nx_k + nu_k, xi, RSQrqt_p + k, 0, 0);
+                    // add gradient to rq
+                    ROWAD(nu_k + nx_k, 1.0, x_tmp_p, offs_ux_k, RSQrqt_p+k, nu_k + nx_k, 0);
+                }
+            }
+            return 0;
+        };
+
+        virtual fatrop_int eval_constr_jac(
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_constr_jac(primal_vars, slack_dummy_v);
+            return 0;
+        };
+        void update_slack_vars(const FatropVecBF &slack_vars, FatropVecBF &slack_dummy)
+        {
+            fatrop_int offs_n = orig_dims_.nineqs;
+            fatrop_int offs_p = orig_dims_.nineqs + n_n;
+            for (fatrop_int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                slack_dummy.at(i) = slack_vars.at(i) - slack_vars.at(i + offs_n) + slack_vars.at(i + offs_p);
+            }
+        }
+        virtual fatrop_int eval_constraint_viol(
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            FatropVecBF &constraint_violation) override
+        {
+            FatropVecBF slack_dummy_v = slack_dummy_[0];
+            update_slack_vars(slack_vars, slack_dummy_v);
+            orig_->eval_constraint_viol(primal_vars, slack_dummy_v, constraint_violation);
+            return 0;
+        };
+        virtual fatrop_int eval_obj_grad(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            FatropVecBF &gradient_x,
+            FatropVecBF &gradient_s) override
+        {
+            // compute delta's
+            axpby(xi, primal_vars, -xi, x_start_[0], gradient_x);
+            axpby(xi, slack_vars.block(0, orig_dims_.nineqs), -xi, s_start_[0], gradient_s.block(0, orig_dims_.nineqs));
+            gradient_s.block(orig_dims_.nineqs, n_n + n_p) = rho;
+            return 0;
+        };
+        virtual fatrop_int eval_obj(
+            double obj_scale,
+            const FatropVecBF &primal_vars,
+            const FatropVecBF &slack_vars,
+            double &res) override
+        {
+            // compute delta's
+            axpby(1.0, primal_vars, -1.0, x_start_[0], x_tmp_[0]);
+            axpby(1.0, slack_vars.block(0, orig_dims_.nineqs), -1.0, s_start_[0], s_tmp_[0]);
+            res = 0.5*xi*(sumsqr(x_tmp_[0]) + sumsqr(s_tmp_[0]));
+            res += rho * sum(slack_vars.block(orig_dims_.nineqs, n_n + n_p));
+            return 0;
+        };
+        void get_initialization(const FatropVecBF &primal_vars_orig, FatropVecBF &intialization)
+        {
+            intialization.block(0, orig_dims_.nvars).copy(primal_vars_orig.block(0, orig_dims_.nvars));
+        }
+        virtual fatrop_int eval_dual_inf(
+            double obj_scale,
+            const FatropVecBF &lam,
+            const FatropVecBF &grad_x,
+            const FatropVecBF &grad_s,
+            FatropVecBF &du_inf_x, FatropVecBF &du_inf_s_wo_z) override
+        {
+            auto du_inf_s_wo_z_or = du_inf_s_wo_z.block(0, orig_dims_.nineqs);
+            orig_->eval_dual_inf(obj_scale, lam, grad_x, grad_s, du_inf_x, du_inf_s_wo_z_or);
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            axpby(1.0, lam_I, 1.0, grad_s.block(orig_dims_.nineqs, n_n), du_inf_s_wo_z.block(orig_dims_.nineqs, n_n));
+            axpby(-1.0, lam_I, 1.0, grad_s.block(orig_dims_.nineqs + n_n, n_p), du_inf_s_wo_z.block(orig_dims_.nineqs + n_n, n_p));
+            return 0;
+        }
+        void update_sigma_gradb(double inertia, const FatropVecBF &sigma_s, const FatropVecBF &sigma_n, const FatropVecBF &sigma_p, const FatropVecBF &gradb_s, const FatropVecBF &gradb_n, const FatropVecBF &gradb_p, const FatropVecBF &sigma_update, const FatropVecBF &gradb_update)
+        {
+            for (int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                double sigma_updt = 1.0 / (1.0 / (sigma_s.at(i) + inertia + xi) + 1.0 / (sigma_n.at(i) + inertia) + 1.0 / (sigma_p.at(i) + inertia));
+                sigma_update.at(i) = sigma_updt - inertia;
+                gradb_update.at(i) = ((gradb_s.at(i)) / (sigma_s.at(i) + inertia + xi) - (gradb_n.at(i)) / (sigma_n.at(i) + inertia) + (gradb_p.at(i)) / (sigma_p.at(i) + inertia)) * sigma_updt;
+            }
+        }
+        void update_delta_snp(double inertia, const FatropVecBF &sigma_s, const FatropVecBF &sigma_n, const FatropVecBF &sigma_p, const FatropVecBF &gradb_s, const FatropVecBF &gradb_n, const FatropVecBF &gradb_p, const FatropVecBF &lam_I, const FatropVecBF &delta_s, const FatropVecBF &delta_n, const FatropVecBF &delta_p)
+        {
+            for (int i = 0; i < orig_dims_.nineqs; ++i)
+            {
+                double lam_I_i = lam_I.at(i);
+                delta_s.at(i) = (-gradb_s.at(i) + lam_I_i) / (sigma_s.at(i) + inertia+xi);
+                delta_n.at(i) = (-gradb_n.at(i) - lam_I_i) / (sigma_n.at(i) + inertia);
+                delta_p.at(i) = (-gradb_p.at(i) + lam_I_i) / (sigma_p.at(i) + inertia);
+            }
+        }
+
+        virtual fatrop_int solve_pd_sys(
+            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
+        {
+            // (todo) check if quadratic penalty on delta s is treated right
+            inertia_correction_w_cache = inertia_correction_w;
+            sigma_cache_[0].copy(sigma_total);
+            gradb_cache_[0].copy(gradb_total);
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            auto sigma_s = sigma_total.block(0, orig_dims_.nineqs);
+            auto sigma_n = sigma_total.block(orig_dims_.nineqs, n_n);
+            auto sigma_p = sigma_total.block(orig_dims_.nineqs + n_n, n_p);
+            auto gradb_s = gradb_total.block(0, orig_dims_.nineqs);
+            auto gradb_n = gradb_total.block(orig_dims_.nineqs, n_n);
+            auto gradb_p = gradb_total.block(orig_dims_.nineqs + n_n, n_p);
+            auto delta_s_or = delta_s.block(0, orig_dims_.nineqs);
+            auto delta_n = delta_s.block(orig_dims_.nineqs, n_n);
+            auto delta_p = delta_s.block(orig_dims_.nineqs + n_n, n_p);
+            update_sigma_gradb(inertia_correction_w, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, sigma_dummy_[0], gradb_dummy_[0]);
+            int ret = orig_->solve_pd_sys(inertia_correction_w, inertia_correction_c, ux, lam, delta_s.block(0, orig_dims_.nineqs), sigma_dummy_[0], gradb_dummy_[0]);
+            update_delta_snp(inertia_correction_w, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, lam_I, delta_s_or, delta_n, delta_p);
+            return ret;
+        };
+        virtual fatrop_int solve_soc_rhs(
+            const FatropVecBF &ux,
+            const FatropVecBF &lam,
+            const FatropVecBF &delta_s,
+            const FatropVecBF &cosntraint_violation) override
+        {
+            auto lam_I = lam.block(orig_dims_.neqs - orig_dims_.nineqs, orig_dims_.nineqs);
+            auto sigma_s = sigma_cache_[0].block(0, orig_dims_.nineqs);
+            auto sigma_n = sigma_cache_[0].block(orig_dims_.nineqs, n_n);
+            auto sigma_p = sigma_cache_[0].block(orig_dims_.nineqs + n_n, n_p);
+            auto gradb_s = gradb_cache_[0].block(0, orig_dims_.nineqs);
+            auto gradb_n = gradb_cache_[0].block(orig_dims_.nineqs, n_n);
+            auto gradb_p = gradb_cache_[0].block(orig_dims_.nineqs + n_n, n_p);
+            auto delta_s_or = delta_s.block(0, orig_dims_.nineqs);
+            auto delta_n = delta_s.block(orig_dims_.nineqs, n_n);
+            auto delta_p = delta_s.block(orig_dims_.nineqs + n_n, n_p);
+            int ret = orig_->solve_soc_rhs(ux, lam, delta_s.block(0, orig_dims_.nineqs), cosntraint_violation);
+            update_delta_snp(inertia_correction_w_cache + xi, sigma_s, sigma_n, sigma_p, gradb_s, gradb_n, gradb_p, lam_I, delta_s_or, delta_n, delta_p);
+            return ret;
+        }
+        virtual NLPDims get_nlp_dims() const override { return this_dims_; };
+        virtual fatrop_int compute_scalings(
+            double &obj_scale,
+            FatropVecBF &x_scales,
+            FatropVecBF &lam_scales,
+            const FatropVecBF &grad_curr_x, const FatropVecBF &grad_curr_s) override
+        {
+            return orig_->compute_scalings(obj_scale, x_scales, lam_scales, grad_curr_x, grad_curr_s);
+        };
+        virtual fatrop_int initialize_slacks(double mu0,
+                                             FatropVecBF &s_curr) override
+        {
+            auto s_curr_or = s_curr.block(0, orig_dims_.nineqs);
+            auto n_curr = s_curr.block(orig_dims_.nineqs, n_n);
+            auto p_curr = s_curr.block(orig_dims_.nineqs + n_n, n_p);
+            FatropVecBF ineq_viol = s_tmp_[0];
+            int ret = orig_->initialize_slacks(mu0, ineq_viol);
+            for (int i = 0; i < orig_dims_.nineqs; i++)
+            {
+                double viol = 0.0;
+                viol = (ineq_viol.at(i));
+                double n_init = (mu0 - rho * viol) / (2 * rho) + std::sqrt(std::pow((mu0 - rho * viol) / (2 * rho), 2) + mu0 * viol / (2 * rho));
+                // if viol >>>> 0 -> n_init = 0 if viol <<< 0 n_init = viol
+                n_curr.at(i) = n_init;
+                p_curr.at(i) = viol + n_init;
+            }
+            return 0;
+        }
+        virtual fatrop_int initialize_dual(
+            const FatropVecBF &grad_x,
+            const FatropVecBF &grad_s,
+            FatropVecBF &dlam,
+            const FatropVecBF &zL,
+            const FatropVecBF &zU) override
+        {
+            // todo check if this is correct
+            // return orig_->initialize_dual(grad_x, grad_s, dlam, zL.block(0, orig_dims_.nineqs), zU.block(0, orig_dims_.nineqs));
+            return 0;
+        };
+        virtual fatrop_int get_bounds(
+            FatropVecBF &lower,
+            FatropVecBF &upper) const override
+        {
+            orig_->get_bounds(lower, upper);
+            lower.block(orig_dims_.nineqs, n_n) = 0.0;
+            lower.block(orig_dims_.nineqs + n_n, n_p) = 0.0;
+            upper.block(orig_dims_.nineqs, n_n) = std::numeric_limits<double>::infinity();
+            upper.block(orig_dims_.nineqs + n_n, n_p) = std::numeric_limits<double>::infinity();
+            return 0;
+        };
+        virtual fatrop_int get_initial_sol_guess(
+            FatropVecBF &initial) const override
+        {
+            orig_->get_initial_sol_guess(initial);
+            return 0;
+        }
+        virtual void update_mu(double mu) override
+        {
+            xi = std::sqrt(mu);
+            orig_->update_mu(mu);
+        };
+        virtual void reset() override
+        {
+            orig_->reset();
+        };
+        std::shared_ptr<FatropOCP> orig_;
+        NLPDims orig_dims_;
+        NLPDims this_dims_;
+        FatropMemoryVecBF lower_, upper_;
+        FatropMemoryVecBF x_start_, s_start_;
+        FatropMemoryVecBF x_tmp_, s_tmp_;
+        std::vector<bool> upper_bounded_;
+        std::vector<bool> lower_bounded_;
+        fatrop_int n_p = 0;
+        fatrop_int n_n = 0;
+        FatropMemoryVecBF slack_dummy_;
+        FatropMemoryVecBF sigma_dummy_;
+        FatropMemoryVecBF gradb_dummy_;
+        FatropMemoryVecBF zl_dummy_;
+        FatropMemoryVecBF zu_dummy_;
+        double inertia_correction_w_cache = 0.0;
+        FatropMemoryVecBF sigma_cache_;
+        FatropMemoryVecBF gradb_cache_;
+        double rho = 1e4;
+        double xi = 1.0;
+    };
+};

casadi/include/fatrop/ocp/OCPLSRiccati.hpp

@@ -186,6 +186,13 @@ namespace fatrop
         std::shared_ptr<FatropOptions> options_;
         std::shared_ptr<FatropPrinter> printer_;
         bool it_ref = true;
+        bool perturbed_mode = false;
+        double perturbed_mode_param = 1e-6;
+        int min_it_ref = 0;
+        int max_it_ref = 5;
+        double it_ref_acc = 1e-8;
+        double lu_fact_tol = 1e-5;
+        bool diagnostic = false;
     };
 };     // namespace
 #endif // OCPLSRICCATIINCLUDED

casadi/include/fatrop/ocp/StageOCPApplication.hpp

@@ -70,7 +70,7 @@ namespace fatrop
         NLPApplication();
 
     protected:
-        void build(const std::shared_ptr<FatropNLP> &nlp);
+        void build(const std::shared_ptr<FatropNLP> &nlp, const std::shared_ptr<FatropNLP> &nlp_resto);
 
     public:
         fatrop_int optimize() const;

casadi/include/fatrop/solver/AlgBuilder.hpp

@@ -26,9 +26,9 @@ namespace fatrop
     class AlgBuilder
     {
     public:
-        void build_fatrop_algorithm_objects(const std::shared_ptr<FatropNLP> &nlp,
+        void build_fatrop_algorithm_objects(const std::shared_ptr<FatropNLP> &nlp,const std::shared_ptr<FatropNLP> &nlp_resto,
                                             const std::shared_ptr<FatropOptions> &fatropparams,
-                                            std::shared_ptr<FatropData> &fatropdata,
+                                            std::shared_ptr<FatropData> &fatropdata, std::shared_ptr<FatropData> &fatropdata_resto,
                                             std::shared_ptr<Journaller> &journaller)
         {
             if (fatropprinter_ == nullptr)
@@ -36,10 +36,13 @@ namespace fatrop
                 fatropprinter_ = std::make_shared<FatropPrinter>();
             }
             fatropdata = std::make_shared<FatropData>(nlp->get_nlp_dims(), fatropparams, fatropprinter_);
+            fatropdata_resto = std::make_shared<FatropData>(nlp_resto->get_nlp_dims(), fatropparams, fatropprinter_);
             journaller = std::make_shared<Journaller>(fatropparams->maxiter + 1, fatropprinter_);
             fatropdata_ = fatropdata; // keep this around for building the algorithm
+            fatropdata_resto_ = fatropdata_resto;
             journaller_ = journaller;
             nlp_ = nlp;
+            nlp_resto_ = nlp_resto;
             fatropoptions_ = fatropparams;
         }
         void set_printer(const std::shared_ptr<FatropPrinter> &printer)
@@ -51,13 +54,20 @@ namespace fatrop
             // TODO unsafe if maxiter is changed during application
             std::shared_ptr<Filter> filter = std::make_shared<Filter>(fatropoptions_->maxiter + 1);
             std::shared_ptr<LineSearch> linesearch = std::make_shared<BackTrackingLineSearch>(fatropoptions_, nlp_, fatropdata_, filter, journaller_, fatropprinter_);
-            return std::make_shared<FatropAlg>(nlp_, fatropdata_, fatropoptions_, filter, linesearch, journaller_, fatropprinter_);
+            std::shared_ptr<Filter> filter_resto = std::make_shared<Filter>(fatropoptions_->maxiter + 1);
+            std::shared_ptr<LineSearch> linesearch_resto = std::make_shared<BackTrackingLineSearch>(fatropoptions_, nlp_resto_, fatropdata_resto_, filter_resto, journaller_, fatropprinter_);
+            std::shared_ptr<FatropAlg> orig_alg = std::make_shared<FatropAlg>(nlp_, fatropdata_, fatropoptions_, filter, linesearch, journaller_, fatropprinter_, nullptr, nullptr, false);
+            std::shared_ptr<FatropAlg> resto_alg = std::make_shared<FatropAlg>(nlp_resto_, fatropdata_resto_, fatropoptions_, filter_resto, linesearch_resto, journaller_, fatropprinter_, orig_alg, nullptr, true);
+            orig_alg-> set_resto_alg(resto_alg);
+            return orig_alg;
         }
 
     private:
         std::shared_ptr<FatropNLP> nlp_;
+        std::shared_ptr<FatropNLP> nlp_resto_;
         std::shared_ptr<FatropOptions> fatropoptions_;
         std::shared_ptr<FatropData> fatropdata_;
+        std::shared_ptr<FatropData> fatropdata_resto_;
         std::shared_ptr<Journaller> journaller_;
         std::shared_ptr<FatropPrinter> fatropprinter_;
     };

casadi/include/fatrop/solver/FatropAlg.hpp

@@ -47,7 +47,8 @@ namespace fatrop
             const std::shared_ptr<Filter> &filter,
             const std::shared_ptr<LineSearch> &linesearch,
             const std::shared_ptr<Journaller> &journaller, 
-            const std::shared_ptr<FatropPrinter> &printer);
+            const std::shared_ptr<FatropPrinter> &printer, 
+            const std::shared_ptr<FatropAlg>&orig_, const std::shared_ptr<FatropAlg>&resto_alg_, bool resto_problem);
         void initialize() ;
         void reset() ;
         void set_bounds(const std::vector<double> &lower, const std::vector<double> &upper) ;
@@ -62,8 +63,11 @@ namespace fatrop
         double eval_objective_curr();
         double eval_objective_trial();
         fatrop_int eval_dual_infeasiblity();
-        fatrop_int perform_initializiation();
+        fatrop_int perform_initializiation_dual();
         fatrop_int solve_pd_sys(double inertia_correction_w, double inertia_correction_c, double mu);
+        fatrop_int start_resto_alg(double mu, int iter);
+        fatrop_int return_from_resto_alg(double mu);
+        bool resto_stop_crit();
         std::shared_ptr<FatropNLP> fatropnlp_;
         std::shared_ptr<FatropData> fatropdata_;
         std::shared_ptr<FatropOptions> fatropoptions_;
@@ -81,6 +85,10 @@ namespace fatrop
         double acceptable_tol;
         fatrop_int acceptable_iter;
         fatrop_int maxiter;
+        void set_resto_alg(const std::shared_ptr<FatropAlg> &resto_alg)
+        {
+            resto_alg_ = resto_alg;
+        };
 
     private:
         double lammax;
@@ -103,6 +111,11 @@ namespace fatrop
         double recalc_y_feas_tol;
         // bool first_try_watchdog;
         FatropStats stats;
+        std::weak_ptr<FatropAlg> orig_;
+        std::shared_ptr<FatropAlg> resto_alg_;
+        bool resto_problem_ = false;
+        fatrop_int start_iter_ = 0;
+        fatrop_int iter_count_ = 0;
     };
 } // namespace fatrop
 #endif // FATROPALGINCLUDED

casadi/include/fatrop/solver/FatropOptions.hpp

@@ -22,6 +22,7 @@
 #include <map>
 #include <type_traits>
 #include <iostream>
+#include <vector>
 #include "fatrop/auxiliary/Common.hpp"
 namespace fatrop
 {
@@ -82,10 +83,10 @@ namespace fatrop
         void register_option(const BooleanOption &option);
         void register_option(const StringOption &option);
         friend auto operator<<(std::ostream &os, const FatropOptions &m) -> std::ostream &;
-        std::map<std::string, DoubleOption> numeric_options;
-        std::map<std::string, IntegerOption> integer_options;
-        std::map<std::string, BooleanOption> boolean_options;
-        std::map<std::string, StringOption> string_options;
+        std::map<std::string, std::vector<DoubleOption>> numeric_options;
+        std::map<std::string, std::vector<IntegerOption>> integer_options;
+        std::map<std::string, std::vector<BooleanOption>> boolean_options;
+        std::map<std::string, std::vector<StringOption>> string_options;
         std::map<std::string, std::string> prebuilt_string;
         std::map<std::string, double> prebuilt_double;
     };

casadi/include/fatrop/solver/IterationData.hpp

@@ -38,12 +38,13 @@ namespace fatrop
         double alpha_pr = 0.0;
         double alpha_du = 0.0;
         char type = 'x';
+        bool resto = false;
     };
     class Journaller
     {
     public:
         Journaller(const fatrop_int maxiter, const std::shared_ptr<FatropPrinter> &printer);
-        void print_iterations();
+        void print_iterations(bool no_header = false);
         void push();
         void reset();
         fatrop_int print_count = 0;

casadi/include/fatrop/solver/LineSearch.hpp

@@ -43,7 +43,7 @@ namespace fatrop
             const std::shared_ptr<FatropNLP> &nlp,
             const std::shared_ptr<FatropData> &fatropdata, const std::shared_ptr<FatropPrinter> &printer);
         virtual LineSearchInfo find_acceptable_trial_point(double mu, bool small_sd, bool from_backup) = 0;
-        inline fatrop_int eval_constr_viol_trial();
+        fatrop_int eval_constr_viol_trial();
         double eval_obj_trial();
         void reset();
         virtual fatrop_int update_trial_step(double alpha_pr, double alpha_du) const;

casadi/include/fatrop/templates/NLPAlg.hpp

@@ -92,11 +92,13 @@ namespace fatrop
             FatropVecBF &upper) const = 0;
         virtual fatrop_int get_initial_sol_guess(
             FatropVecBF &initial) const = 0;
+        virtual void pre_solve(const FatropVecBF& x_init,const FatropVecBF& s_init){};
         // virtual fatrop_int GetDefaultParams(
         //     FatropOptions &params) const = 0;
         virtual fatrop_int Callback(FatropVecBF& primal_vars){return 0;};
         virtual void finalize(){};
         virtual void reset(){};
+        virtual void update_mu(double mu){};
     };
 } // namespace fatrop
 #endif // NLPINCLUDED

casadi/include/highs/HConfig.h

@@ -12,7 +12,7 @@
 #define HIGHS_HAVE_BUILTIN_CLZ
 /* #undef HIGHS_HAVE_BITSCAN_REVERSE */
 
-#define HIGHS_GITHASH "5ce7a2753"
+#define HIGHS_GITHASH "5ce7a275"
 #define HIGHS_VERSION_MAJOR 1
 #define HIGHS_VERSION_MINOR 7
 #define HIGHS_VERSION_PATCH 2