NLOptConSettings.hpp

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <map>

#include <boost/property_tree/info_parser.hpp>

namespace ct {
namespace optcon {



struct LineSearchSettings
{
    enum TYPE
    {
        NONE = 0,      
        SIMPLE,        
        ARMIJO,        
        GOLDSTEIN,  
        NUM_TYPES
    };

    std::map<TYPE, std::string> lineSearchTypeToString = {{NONE, "NONE (take full-step updates with alpha=1.0)"},
        {SIMPLE, "Simple Backtracking with cost/merit"}, {ARMIJO, "ARMIJO-style Backtracking for single-shooting"},
        {GOLDSTEIN, "GOLDSTEIN backtracking using Riccati matrices"}};

    std::map<std::string, TYPE> stringToLineSearchType = {
        {"NONE", NONE}, {"SIMPLE", SIMPLE}, {"ARMIJO", ARMIJO}, {"GOLDSTEIN", GOLDSTEIN}};


    LineSearchSettings()
        : type(NONE),
          adaptive(false),
          maxIterations(10),
          alpha_0(1.0),
          alpha_max(1.0),
          n_alpha(0.5),
          armijo_parameter(0.01),
          debugPrint(false)
    {
    }

    bool parametersOk() const { return (alpha_0 > 0.0) && (n_alpha > 0.0) && (n_alpha < 1.0) && (alpha_max > 0.0); }
    TYPE type;            
    bool adaptive;        
    size_t maxIterations; 
    double alpha_0;       
    double alpha_max;     
    double
        n_alpha; 
    double armijo_parameter; 
    bool debugPrint;         
    void print() const
    {
        std::cout << "Line Search Settings: " << std::endl;
        std::cout << "=====================" << std::endl;
        std::cout << "type:\t" << lineSearchTypeToString.at(type) << std::endl;
        std::cout << "adaptive:\t" << adaptive << std::endl;
        std::cout << "maxIter:\t" << maxIterations << std::endl;
        std::cout << "alpha_0:\t" << alpha_0 << std::endl;
        std::cout << "alpha_max:\t" << alpha_max << std::endl;
        std::cout << "n_alpha:\t" << n_alpha << std::endl;
        std::cout << "armijo_parameter:\t" << armijo_parameter << std::endl;
        std::cout << "debugPrint:\t" << debugPrint << std::endl;
        std::cout << "              =======" << std::endl;
        std::cout << std::endl;
    }

    void load(const std::string& filename, bool verbose = true, const std::string& ns = "line_search")
    {
        if (verbose)
            std::cout << "Trying to load line search settings from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        std::string ls_type = pt.get<std::string>(ns + ".type");
        if (stringToLineSearchType.find(ls_type) != stringToLineSearchType.end())
        {
            type = stringToLineSearchType[ls_type];
        }
        else
        {
            std::cout << "Invalid line search type specified in config, should be one of the following:" << std::endl;

            for (auto it = stringToLineSearchType.begin(); it != stringToLineSearchType.end(); it++)
            {
                std::cout << it->first << std::endl;
            }

            exit(-1);
        }


        maxIterations = pt.get<size_t>(ns + ".maxIterations");
        alpha_0 = pt.get<double>(ns + ".alpha_0");
        n_alpha = pt.get<double>(ns + ".n_alpha");
        debugPrint = pt.get<bool>(ns + ".debugPrint");
        alpha_max = alpha_0;
        adaptive = false;

        try
        {
            armijo_parameter = pt.get<double>(ns + ".armijo_parameter");
        } catch (...)
        {
        }

        try
        {
            adaptive = pt.get<bool>(ns + ".adaptive");
        } catch (...)
        {
        }

        try
        {
            alpha_max = pt.get<double>(ns + ".alpha_max");
        } catch (...)
        {
        }

        if (verbose)
        {
            std::cout << "Loaded line search settings from " << filename << ": " << std::endl;
            print();
        }
    }
};



struct LQOCSolverSettings
{
public:
    LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10) {}

    bool lqoc_debug_print;
    int num_lqoc_iterations;  

    void print() const
    {
        std::cout << "======================= LQOCSolverSettings =====================" << std::endl;
        std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl;
        std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl;
    }

    void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings")
    {
        if (verbose)
            std::cout << "Trying to load LQOCSolverSettings config from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        try
        {
            num_lqoc_iterations = pt.get<int>(ns + ".num_lqoc_iterations");
        } catch (...)
        {
        }
        try
        {
            lqoc_debug_print = pt.get<bool>(ns + ".lqoc_debug_print");
        } catch (...)
        {
        }
    }
};


class NLOptConSettings
{
public:
    enum NLOCP_ALGORITHM
    {
        GNMS = 0,   
        ILQR,       
        MS_ILQR,    
        SS_OL,      
        SS_CL,      
        GNMS_M_OL,  
        GNMS_M_CL,  
        NUM_TYPES
    };

    enum LQOCP_SOLVER
    {
        GNRICCATI_SOLVER = 0,
        HPIPM_SOLVER = 1
    };

    using APPROXIMATION = typename core::SensitivityApproximationSettings::APPROXIMATION;


    NLOptConSettings()
        : integrator(ct::core::IntegrationType::RK4),
          discretization(APPROXIMATION::BACKWARD_EULER),
          timeVaryingDiscretization(false),
          nlocp_algorithm(GNMS),
          lqocp_solver(GNRICCATI_SOLVER),
          loggingPrefix("alg"),
          epsilon(1e-5),
          dt(0.001),
          K_sim(1),                    
          K_shot(1),                   
          min_cost_improvement(1e-5),  
          maxDefectSum(1e-5),
          meritFunctionRho(0.0),
          meritFunctionRhoConstraints(1.0),
          max_iterations(100),
          fixedHessianCorrection(false),
          recordSmallestEigenvalue(false),
          nThreads(4),
          nThreadsEigen(4),
          lineSearchSettings(),
          debugPrint(false),
          printSummary(true),
          useSensitivityIntegrator(false),
          logToMatlab(false)
    {
    }

    ct::core::IntegrationType integrator;  
    APPROXIMATION discretization;
    bool timeVaryingDiscretization;
    NLOCP_ALGORITHM nlocp_algorithm;  
    LQOCP_SOLVER lqocp_solver;        
    std::string loggingPrefix;        
    double epsilon;                   
    double dt;                        
    int K_sim;                        
    int K_shot;                       
    double min_cost_improvement;      
    double maxDefectSum;              
    double meritFunctionRho;          
    double meritFunctionRhoConstraints;  
    int max_iterations;  
    bool fixedHessianCorrection;    
    bool recordSmallestEigenvalue;  
    int nThreads;                   
    size_t
        nThreadsEigen;  
    LineSearchSettings lineSearchSettings;  
    LQOCSolverSettings lqoc_solver_settings;
    bool debugPrint;
    bool printSummary;
    bool useSensitivityIntegrator;
    bool logToMatlab;  



    int computeK(double timeHorizon) const
    {
        if (timeHorizon < 0.0)
        {
            throw std::runtime_error("time Horizon is negative");
        }
        return std::max(1, (int)std::lround(timeHorizon / dt));
    }

    double getSimulationTimestep() const { return (dt / (double)K_sim); }

    bool closedLoopShooting() const { return nlocAlgorithmToClosedLoopShooting.at(nlocp_algorithm); }

    bool isSingleShooting() const { return nlocAlgorithmToSingleShooting.at(nlocp_algorithm); }

    void print() const
    {
        std::cout << "======================= NLOptCon Settings =====================" << std::endl;
        std::cout << "===============================================================" << std::endl;
        std::cout << "integrator: " << integratorToString.at(integrator) << std::endl;
        std::cout << "discretization: " << discretizationToString.at(discretization) << std::endl;
        std::cout << "time varying discretization: " << timeVaryingDiscretization << std::endl;
        std::cout << "nonlinear OCP algorithm: " << nlocAlgorithmToString.at(nlocp_algorithm) << std::endl;
        std::cout << "linear-quadratic OCP solver: " << lqocSolverToString.at(lqocp_solver) << std::endl;
        std::cout << "dt:\t" << dt << std::endl;
        std::cout << "K_sim:\t" << K_sim << std::endl;
        std::cout << "K_shot:\t" << K_shot << std::endl;
        std::cout << "maxIter:\t" << max_iterations << std::endl;
        std::cout << "min cost improvement:\t" << min_cost_improvement << std::endl;
        std::cout << "max defect sum:\t" << maxDefectSum << std::endl;
        std::cout << "merit function rho defects:\t" << meritFunctionRho << std::endl;
        std::cout << "merit function rho constraints:\t" << meritFunctionRhoConstraints << std::endl;
        std::cout << "fixedHessianCorrection:\t" << fixedHessianCorrection << std::endl;
        std::cout << "recordSmallestEigenvalue:\t" << recordSmallestEigenvalue << std::endl;
        std::cout << "epsilon:\t" << epsilon << std::endl;
        std::cout << "nThreads:\t" << nThreads << std::endl;
        std::cout << "nThreadsEigen:\t" << nThreadsEigen << std::endl;
        std::cout << "loggingPrefix:\t" << loggingPrefix << std::endl;
        std::cout << "debugPrint:\t" << debugPrint << std::endl;
        std::cout << "printSummary:\t" << printSummary << std::endl;
        std::cout << "useSensitivityIntegrator:\t" << useSensitivityIntegrator << std::endl;
        std::cout << "logToMatlab:\t" << logToMatlab << std::endl;
        std::cout << std::endl;

        lineSearchSettings.print();

        lqoc_solver_settings.print();

        std::cout << "===============================================================" << std::endl;
    }


    bool parametersOk() const
    {
        if (dt <= 0)
        {
            std::cout << "Invalid parameter dt in NLOptConSettings, needs to be > 0. dt currently is " << dt
                      << std::endl;
            return false;
        }

        if (K_sim <= 0)
        {
            std::cout << "Invalid parameter K_sim in NLOptConSettings, needs to be >= 1. K_sim currently is " << K_sim
                      << std::endl;
            return false;
        }

        if (K_shot <= 0)
        {
            std::cout << "Invalid parameter K_shot in NLOptConSettings, needs to be >= 1. K_shot currently is "
                      << K_shot << std::endl;
            return false;
        }

        // TODO need thorough check if this is really the case ....
        if ((K_shot > 1) && (nlocp_algorithm == ILQR))
        {
            std::cout << "Invalid parameter: for iLQR K_shot needs to be 1. K_shot currently is " << K_shot
                      << std::endl;
            return false;
        }

        if (nThreads > 100 || nThreadsEigen > 100)
        {
            std::cout << "Number of threads should not exceed 100." << std::endl;
            return false;
        }
        return (lineSearchSettings.parametersOk());
    }



    void load(const std::string& filename, bool verbose = true, const std::string& ns = "alg")
    {
        if (verbose)
            std::cout << "Trying to load NLOptCon config from " << filename << ": " << std::endl;

        boost::property_tree::ptree pt;
        boost::property_tree::read_info(filename, pt);

        try
        {
            epsilon = pt.get<double>(ns + ".epsilon");
        } catch (...)
        {
        }
        try
        {
            timeVaryingDiscretization = pt.get<bool>(ns + ".timeVaryingDiscretization");
        } catch (...)
        {
        }
        try
        {
            min_cost_improvement = pt.get<double>(ns + ".min_cost_improvement");
        } catch (...)
        {
        }
        try
        {
            maxDefectSum = pt.get<double>(ns + ".maxDefectSum");
        } catch (...)
        {
        }
        try
        {
            meritFunctionRho = pt.get<double>(ns + ".meritFunctionRho");
        } catch (...)
        {
        }
        try
        {
            meritFunctionRhoConstraints = pt.get<double>(ns + ".meritFunctionRhoConstraints");
        } catch (...)
        {
        }
        try
        {
            max_iterations = pt.get<int>(ns + ".max_iterations");
        } catch (...)
        {
        }
        try
        {
            nThreads = pt.get<int>(ns + ".nThreads");
        } catch (...)
        {
        }
        try
        {
            loggingPrefix = pt.get<std::string>(ns + ".loggingPrefix");
        } catch (...)
        {
        }
        try
        {
            debugPrint = pt.get<bool>(ns + ".debugPrint");
        } catch (...)
        {
        }
        try
        {
            printSummary = pt.get<bool>(ns + ".printSummary");
        } catch (...)
        {
        }
        try
        {
            useSensitivityIntegrator = pt.get<bool>(ns + ".useSensitivityIntegrator");
        } catch (...)
        {
        }
        try
        {
            logToMatlab = pt.get<bool>(ns + ".logToMatlab");
        } catch (...)
        {
        }
        try
        {
            dt = pt.get<double>(ns + ".dt");
        } catch (...)
        {
        }
        try
        {
            K_sim = pt.get<int>(ns + ".K_sim");
        } catch (...)
        {
        }
        try
        {
            K_shot = pt.get<int>(ns + ".K_shot");
        } catch (...)
        {
        }
        try
        {
            nThreadsEigen = pt.get<size_t>(ns + ".nThreadsEigen");
        } catch (...)
        {
        }
        try
        {
            recordSmallestEigenvalue = pt.get<bool>(ns + ".recordSmallestEigenvalue");
        } catch (...)
        {
        }
        try
        {
            fixedHessianCorrection = pt.get<bool>(ns + ".fixedHessianCorrection");
        } catch (...)
        {
        }

        try
        {
            lineSearchSettings.load(filename, verbose, ns + ".line_search");
        } catch (...)
        {
        }
        try
        {
            lqoc_solver_settings.load(filename, verbose, ns + ".lqoc_solver_settings");
        } catch (...)
        {
        }


        try
        {
            std::string integratorStr = pt.get<std::string>(ns + ".integrator");
            if (stringToIntegrator.find(integratorStr) != stringToIntegrator.end())
            {
                integrator = stringToIntegrator[integratorStr];
            }
            else
            {
                std::cout << "Invalid integrator specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToIntegrator.begin(); it != stringToIntegrator.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

            std::string discretizationStr = pt.get<std::string>(ns + ".discretization");
            if (stringToDiscretization.find(discretizationStr) != stringToDiscretization.end())
            {
                discretization = stringToDiscretization[discretizationStr];
            }
            else
            {
                std::cout << "Invalid discretization specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToDiscretization.begin(); it != stringToDiscretization.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

            std::string nlocp_algorithmStr = pt.get<std::string>(ns + ".nlocp_algorithm");
            if (stringToNlocAlgorithm.find(nlocp_algorithmStr) != stringToNlocAlgorithm.end())
            {
                nlocp_algorithm = stringToNlocAlgorithm[nlocp_algorithmStr];
            }
            else
            {
                std::cout << "Invalid nlocp_algorithm specified in config, should be one of the following:"
                          << std::endl;

                for (auto it = stringToNlocAlgorithm.begin(); it != stringToNlocAlgorithm.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }


            std::string locp_solverStr = pt.get<std::string>(ns + ".locp_solver");
            if (stringToLqocSolver.find(locp_solverStr) != stringToLqocSolver.end())
            {
                lqocp_solver = stringToLqocSolver[locp_solverStr];
            }
            else
            {
                std::cout << "Invalid locp_solver specified in config, should be one of the following:" << std::endl;

                for (auto it = stringToLqocSolver.begin(); it != stringToLqocSolver.end(); it++)
                {
                    std::cout << it->first << std::endl;
                }

                exit(-1);
            }

        } catch (...)
        {
        }

        if (verbose)
        {
            std::cout << "Loaded NLOptCon config from " << filename << ": " << std::endl;
            print();
        }
    }



    static NLOptConSettings fromConfigFile(const std::string& filename,
        bool verbose = true,
        const std::string& ns = "alg")
    {
        NLOptConSettings settings;
        settings.load(filename, true, ns);
        return settings;
    }

private:
    std::map<ct::core::IntegrationType, std::string> integratorToString = {{ct::core::IntegrationType::EULER, "Euler"},
        {ct::core::IntegrationType::RK4, "Runge-Kutta 4th Order"},
        {ct::core::IntegrationType::MODIFIED_MIDPOINT, "Modified midpoint"},
        {ct::core::IntegrationType::ODE45, "ode45"}, {ct::core::IntegrationType::RK5VARIABLE, "RK5 variable step"},
        {ct::core::IntegrationType::RK78, "RK78"}, {ct::core::IntegrationType::BULIRSCHSTOER, "Bulirsch-Stoer"},
        {ct::core::IntegrationType::EULERCT, "Euler (CT)"},
        {ct::core::IntegrationType::RK4CT, "Runge-Kutta 4th Order (CT"},
        {ct::core::IntegrationType::EULER_SYM, "Symplectic Euler"},
        {ct::core::IntegrationType::RK_SYM, "Symplectic Runge Kutta"}};

    std::map<std::string, ct::core::IntegrationType> stringToIntegrator = {{"Euler", ct::core::IntegrationType::EULER},
        {"RK4", ct::core::IntegrationType::RK4}, {"MODIFIED_MIDPOINT", ct::core::IntegrationType::MODIFIED_MIDPOINT},
        {"ODE45", ct::core::IntegrationType::ODE45}, {"RK5VARIABLE", ct::core::IntegrationType::RK5VARIABLE},
        {"RK78", ct::core::IntegrationType::RK78}, {"BULIRSCHSTOER", ct::core::IntegrationType::BULIRSCHSTOER},
        {"EulerCT", ct::core::IntegrationType::EULERCT}, {"RK4CT", ct::core::IntegrationType::RK4CT},
        {"Euler_Sym", ct::core::IntegrationType::EULER_SYM}, {"Rk_Sym", ct::core::IntegrationType::RK_SYM}};


    std::map<APPROXIMATION, std::string> discretizationToString = {{APPROXIMATION::FORWARD_EULER, "Forward_euler"},
        {APPROXIMATION::BACKWARD_EULER, "Backward_euler"}, {APPROXIMATION::SYMPLECTIC_EULER, "Symplectic_euler"},
        {APPROXIMATION::TUSTIN, "Tustin"}, {APPROXIMATION::MATRIX_EXPONENTIAL, "Matrix_exponential"}};

    std::map<std::string, APPROXIMATION> stringToDiscretization = {{"Forward_euler", APPROXIMATION::FORWARD_EULER},
        {"Backward_euler", APPROXIMATION::BACKWARD_EULER}, {"Symplectic_euler", APPROXIMATION::SYMPLECTIC_EULER},
        {"Tustin", APPROXIMATION::TUSTIN}, {"Matrix_exponential", APPROXIMATION::MATRIX_EXPONENTIAL}};


    std::map<NLOCP_ALGORITHM, std::string> nlocAlgorithmToString = {{GNMS, "GNMS (Gauss-Newton Multiple Shooting)"},
        {ILQR, "ILQR (iterative linear-quadratic optimal control)"}, {MS_ILQR, "MS_ILQR (multiple-shooting iLQR)"},
        {SS_OL, "SS_OL (open-loop Single Shooting)"}, {SS_CL, "SS_CL (closed-loop Single Shooting)"},
        {GNMS_M_OL, "GNMS_M_OL (GNMS(M) with open-loop shooting)"},
        {GNMS_M_CL, "GNMS_M_CL (GNMS(M) with closed-loop shooting)"}};

    std::map<std::string, NLOCP_ALGORITHM> stringToNlocAlgorithm = {{"GNMS", GNMS}, {"ILQR", ILQR},
        {"MS_ILQR", MS_ILQR}, {"SS_OL", SS_OL}, {"SS_CL", SS_CL}, {"GNMS_M_OL", GNMS_M_OL}, {"GNMS_M_CL", GNMS_M_CL}};

    std::map<NLOCP_ALGORITHM, bool> nlocAlgorithmToClosedLoopShooting = {{GNMS, false}, {ILQR, true}, {MS_ILQR, true},
        {SS_OL, false}, {SS_CL, true}, {GNMS_M_OL, false}, {GNMS_M_CL, true}};

    std::map<NLOCP_ALGORITHM, bool> nlocAlgorithmToSingleShooting = {{GNMS, false}, {ILQR, true}, {MS_ILQR, false},
        {SS_OL, true}, {SS_CL, true}, {GNMS_M_OL, false}, {GNMS_M_CL, false}};

    std::map<LQOCP_SOLVER, std::string> lqocSolverToString = {
        {GNRICCATI_SOLVER, "GNRICCATI_SOLVER"}, {HPIPM_SOLVER, "HPIPM_SOLVER"}};

    std::map<std::string, LQOCP_SOLVER> stringToLqocSolver = {
        {"GNRICCATI_SOLVER", GNRICCATI_SOLVER}, {"HPIPM_SOLVER", HPIPM_SOLVER}};
};
}  // namespace optcon
}  // namespace ct