Andrepatri devel

horizon/cpp/CMakeLists.txt

@@ -1,19 +1,29 @@
 project(horizon)
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.10)
+if(DEFINED ENV{CONDA_PREFIX})
+
+  set(CMAKE_INSTALL_PREFIX $ENV{CONDA_PREFIX}/ CACHE PATH "bindings install prefix" FORCE)
+
+endif()
 
 # options
 option(HORIZON_PROFILING OFF "enable profiling features")
+option(ILQR_FLOAT OFF "use float for data representation instead of double")
 
 if(${HORIZON_PROFILING})
     add_definitions(-DHORIZON_PROFILING)
 endif()
+if(${ILQR_FLOAT})
+    add_definitions(-DILQR_FLOAT)
+endif()
+
 
 find_package(Eigen3 REQUIRED)
 find_package(casadi 3.5.5 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
 set(CMAKE_POSITION_INDEPENDENT_CODE TRUE)
-set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD 17)
 
 # ilqr library
 add_library(horizon STATIC
@@ -39,7 +49,6 @@ add_library(sqp STATIC
     src/sqp.cpp
 )
 
-
 target_link_libraries(ilqr Eigen3::Eigen casadi pthread)
 target_link_libraries(sqp Eigen3::Eigen casadi ilqr)
 

horizon/cpp/pyilqr.cpp

@@ -50,9 +50,13 @@ PYBIND11_MODULE(pyilqr, m) {
             .def("getInputTrajectory", &IterativeLQR::getInputTrajectory)
             .def("getConstraintsValues", &IterativeLQR::getConstraintsValues)
             .def("getCostsValues", &IterativeLQR::getCostsValues)
+            .def("getResidualNorm", &IterativeLQR::getResidualNorm)
+            .def("getConstrValOnNodes", &IterativeLQR::getConstrValOnNodes)
+            .def("getCostValOnNodes", &IterativeLQR::getCostValOnNodes)
             .def("setInitialState", &IterativeLQR::setInitialState)
             .def("setInputInitialGuess", &IterativeLQR::setInputInitialGuess)
             .def("setStateInitialGuess", &IterativeLQR::setStateInitialGuess)
+            .def("reset", &IterativeLQR::reset)
             ;
 
 }

horizon/cpp/pyilqr_helpers.h

@@ -8,6 +8,8 @@
 #include <pybind11/stl.h>
 #include <pybind11/functional.h>
 
+#include "src/typedefs.h"
+
 namespace py = pybind11;
 using namespace horizon;
 
@@ -56,7 +58,7 @@ auto set_final_constraint_wrapper(IterativeLQR& self, py::object pyfn)
 auto set_inter_constraint_wrapper_single(IterativeLQR& self,
                                          std::vector<int> k,
                                          py::object f,
-                                         std::vector<Eigen::VectorXd> tgt)
+                                         std::vector<VectorXr> tgt)
 {
     self.setConstraint(k, to_cpp(f), tgt);
 }

horizon/cpp/pysqp_helpers.h

@@ -9,25 +9,25 @@
 #include <pybind11/stl.h>
 #include <pybind11/functional.h>
 #include <typeinfo>
+#include "src/typedefs.h"
 
 namespace py = pybind11;
 using namespace horizon;
 
-
 py::dict get_qpoases_options_mpc()
 {
     py::dict opts;
     opts["enableEqualities"] = true;
     opts["initialStatusBounds"] = "inactive";
     opts["numRefinementSteps"] = 0;
     opts["enableDriftCorrection"] = 0;
-    opts["terminationTolerance"] = 10e9 * std::numeric_limits<double>::epsilon();
+    opts["terminationTolerance"] = 10e9 * std::numeric_limits<Real>::epsilon();
     opts["enableFlippingBounds"] = false;
     opts["enableNZCTests"] = false;
     opts["enableRamping"] = false;
     opts["enableRegularisation"] = true;
     opts["numRegularisationSteps"] = 2;
-    opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits<double>::epsilon();
+    opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits<Real>::epsilon();
     return opts;
 }
 
@@ -38,7 +38,7 @@ py::dict get_qpoases_options_reliable()
     opts["enableEqualities"] = false;
     opts["numRefinementSteps"] = 2;
     opts["enableFullLITest"] = true;
-    opts["epsLITests"] = 10e5 * std::numeric_limits<double>::epsilon();
+    opts["epsLITests"] = 10e5 * std::numeric_limits<Real>::epsilon();
     opts["maxDualJump"] = 10e8;
     opts["enableCholeskyRefactorisation"] = 1;
     return opts;
@@ -61,15 +61,15 @@ bool setOption(const std::string& key, const std::string& solver_key, py::handle
 bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict)
 {
     // -- sqp options --//
-    if(setOption    <double>        (key,   "beta",                             value, dict)) return true;
-    if(setOption    <double>        (key,   "eps_regularization",               value, dict)) return true;
-    if(setOption    <double>        (key,   "alpha_min",                        value, dict)) return true;
+    if(setOption    <Real>        (key,   "beta",                             value, dict)) return true;
+    if(setOption    <Real>        (key,   "eps_regularization",               value, dict)) return true;
+    if(setOption    <Real>        (key,   "alpha_min",                        value, dict)) return true;
     if(setOption    <int>           (key,   "max_iter",                         value, dict)) return true;
     if(setOption    <bool>          (key,   "reinitialize_qpsolver",            value, dict)) return true;
-    if(setOption    <double>        (key,   "merit_derivative_tolerance",       value, dict)) return true;
-    if(setOption    <double>        (key,   "merit_eps",                        value, dict)) return true;
-    if(setOption    <double>        (key,   "constraint_violation_tolerance",   value, dict)) return true;
-    if(setOption    <double>        (key,   "solution_convergence",             value, dict)) return true;
+    if(setOption    <Real>        (key,   "merit_derivative_tolerance",       value, dict)) return true;
+    if(setOption    <Real>        (key,   "merit_eps",                        value, dict)) return true;
+    if(setOption    <Real>        (key,   "constraint_violation_tolerance",   value, dict)) return true;
+    if(setOption    <Real>        (key,   "solution_convergence",             value, dict)) return true;
     if(setOption    <bool>          (key,   "use_golden_ratio_update",          value, dict)) return true;
     // -- qpoases options --//
     if(setOption    <bool>          (key,   "sparse",                           value, dict)) return true;
@@ -78,7 +78,7 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict)
     if(setOption    <int>           (key,   "max_schur",                        value, dict)) return true;
     if(setOption    <std::string>   (key,   "linsol_plugin",                    value, dict)) return true;
     if(setOption    <int>           (key,   "nWSR",                             value, dict)) return true;
-    if(setOption    <double>        (key,   "CPUtime",                          value, dict)) return true;
+    if(setOption    <Real>        (key,   "CPUtime",                          value, dict)) return true;
     if(setOption    <std::string>   (key,   "printLevel",                       value, dict)) return true;
     if(setOption    <bool>          (key,   "enableRamping",                    value, dict)) return true;
     if(setOption    <bool>          (key,   "enableFarBounds",                  value, dict)) return true;
@@ -89,42 +89,42 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict)
     if(setOption    <int>           (key,   "enableDriftCorrection",            value, dict)) return true;
     if(setOption    <int>           (key,   "enableCholeskyRefactorisation",    value, dict)) return true;
     if(setOption    <bool>          (key,   "enableEqualities",                 value, dict)) return true;
-    if(setOption    <double>        (key,   "terminationTolerance",             value, dict)) return true;
-    if(setOption    <double>        (key,   "boundTolerance",                   value, dict)) return true;
-    if(setOption    <double>        (key,   "boundRelaxation",                  value, dict)) return true;
-    if(setOption    <double>        (key,   "epsNum",                           value, dict)) return true;
-    if(setOption    <double>        (key,   "epsDen",                           value, dict)) return true;
-    if(setOption    <double>        (key,   "maxPrimalJump",                    value, dict)) return true;
-    if(setOption    <double>        (key,   "maxDualJump",                      value, dict)) return true;
-    if(setOption    <double>        (key,   "initialRamping",                   value, dict)) return true;
-    if(setOption    <double>        (key,   "finalRamping",                     value, dict)) return true;
-    if(setOption    <double>        (key,   "initialFarBounds",                 value, dict)) return true;
-    if(setOption    <double>        (key,   "growFarBounds",                    value, dict)) return true;
+    if(setOption    <Real>        (key,   "terminationTolerance",             value, dict)) return true;
+    if(setOption    <Real>        (key,   "boundTolerance",                   value, dict)) return true;
+    if(setOption    <Real>        (key,   "boundRelaxation",                  value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsNum",                           value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsDen",                           value, dict)) return true;
+    if(setOption    <Real>        (key,   "maxPrimalJump",                    value, dict)) return true;
+    if(setOption    <Real>        (key,   "maxDualJump",                      value, dict)) return true;
+    if(setOption    <Real>        (key,   "initialRamping",                   value, dict)) return true;
+    if(setOption    <Real>        (key,   "finalRamping",                     value, dict)) return true;
+    if(setOption    <Real>        (key,   "initialFarBounds",                 value, dict)) return true;
+    if(setOption    <Real>        (key,   "growFarBounds",                    value, dict)) return true;
     if(setOption    <std::string>   (key,   "initialStatusBounds",              value, dict)) return true;
-    if(setOption    <double>        (key,   "epsFlipping",                      value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsFlipping",                      value, dict)) return true;
     if(setOption    <int>           (key,   "numRegularisationSteps",           value, dict)) return true;
-    if(setOption    <double>        (key,   "epsRegularisation",                value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsRegularisation",                value, dict)) return true;
     if(setOption    <int>           (key,   "numRefinementSteps",               value, dict)) return true;
-    if(setOption    <double>        (key,   "epsIterRef",                       value, dict)) return true;
-    if(setOption    <double>        (key,   "epsLITests",                       value, dict)) return true;
-    if(setOption    <double>        (key,   "epsNZCTests",                      value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsIterRef",                       value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsLITests",                       value, dict)) return true;
+    if(setOption    <Real>        (key,   "epsNZCTests",                      value, dict)) return true;
     if(setOption    <bool>          (key,   "enableInertiaCorrection",          value, dict)) return true;
     // -- osqp options --//
     if(setOption    <bool>          (key,   "warm_start_primal",                value, dict)) return true;
     if(setOption    <bool>          (key,   "warm_start_dual",                  value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.rho",                         value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.sigma",                       value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.rho",                         value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.sigma",                       value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.scaling",                     value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.adaptive_rho",                value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.adaptive_rho_interval",       value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.adaptive_rho_tolerance",      value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.adaptive_rho_tolerance",      value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.max_iter",                    value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.eps_abs",                     value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.eps_rel",                     value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.eps_prim_inf",                value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.eps_dual_inf",                value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.alpha",                       value, dict)) return true;
-    if(setOption    <double>        (key,   "osqp.delta",                       value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.eps_abs",                     value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.eps_rel",                     value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.eps_prim_inf",                value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.eps_dual_inf",                value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.alpha",                       value, dict)) return true;
+    if(setOption    <Real>        (key,   "osqp.delta",                       value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.polish",                      value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.polish_refine_iter",          value, dict)) return true;
     if(setOption    <int>           (key,   "osqp.verbose",                     value, dict)) return true;
@@ -189,10 +189,10 @@ bool gSX(SQPGaussNewton<casadi::SX>& self, py::object g, bool reinitialize_qp_so
 }
 
 auto callMX(SQPGaussNewton<casadi::MX>& self,
-            const Eigen::VectorXd& x0,
-            const Eigen::VectorXd& p,
-            const Eigen::VectorXd& lbx, const Eigen::VectorXd& ubx,
-            const Eigen::VectorXd& lbg, const Eigen::VectorXd& ubg)
+            const VectorXr& x0,
+            const VectorXr& p,
+            const VectorXr& lbx, const VectorXr& ubx,
+            const VectorXr& lbg, const VectorXr& ubg)
 {
     casadi::DM _x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_;
     casadi_utils::toCasadiMatrix(x0, _x0_);
@@ -204,22 +204,22 @@ auto callMX(SQPGaussNewton<casadi::MX>& self,
     casadi::DMDict tmp = self.solve(_x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_);
 
     py::dict solution;
-    Eigen::VectorXd x;
+    VectorXr x;
     casadi_utils::toEigen(tmp.at("x"), x);
     solution["x"] = x;
-    solution["f"] = double(tmp.at("f")(0));
-    solution["g"] = double(tmp.at("g")(0));
+    solution["f"] = Real(tmp.at("f")(0));
+    solution["g"] = Real(tmp.at("g")(0));
     return solution;
 }
 
 
 auto callSX(SQPGaussNewton<casadi::SX>& self,
-            const Eigen::VectorXd& x0,
-            const Eigen::VectorXd& p,
-            const Eigen::VectorXd& lbx,
-            const Eigen::VectorXd& ubx,
-            const Eigen::VectorXd& lbg,
-            const Eigen::VectorXd& ubg)
+            const VectorXr& x0,
+            const VectorXr& p,
+            const VectorXr& lbx,
+            const VectorXr& ubx,
+            const VectorXr& lbg,
+            const VectorXr& ubg)
 {
     casadi::DM _x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_;
     casadi_utils::toCasadiMatrix(x0, _x0_);
@@ -231,11 +231,11 @@ auto callSX(SQPGaussNewton<casadi::SX>& self,
     casadi::DMDict tmp = self.solve(_x0_, _p_, _lbx_, _ubx_, _lbg_, _ubg_);
 
     py::dict solution;
-    Eigen::VectorXd x;
+    VectorXr x;
     casadi_utils::toEigen(tmp.at("x"), x);
     solution["x"] = x;
-    solution["f"] = double(tmp.at("f")(0));
-    solution["g"] = double(tmp.at("g")(0));
+    solution["f"] = Real(tmp.at("f")(0));
+    solution["g"] = Real(tmp.at("g")(0));
     return solution;
 }
 

horizon/cpp/src/codegen_function.cpp

@@ -8,10 +8,15 @@
 #include <dlfcn.h>
 
 #include "wrapped_function.h"
+#include "typedefs.h"
 
 namespace
 {
 
+using Real=horizon::Real;
+using MatrixXr=horizon::MatrixXr;
+using VectorXr=horizon::VectorXr;
+
 class RestoreCwd
 {
 public:
@@ -36,14 +41,14 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio
     casadi_utils::WrappedFunction fw = f;
     casadi_utils::WrappedFunction gw = g;
 
-    std::vector<Eigen::VectorXd> input(f.n_in());
+    std::vector<VectorXr> input(f.n_in());
 
     for(int iter = 0; iter < 10; iter++)
     {
 
         for(int i = 0; i < f.n_in(); i++)
         {
-            Eigen::VectorXd u;
+            VectorXr u;
             u.setRandom(f.size1_in(i));
             input[i] = 100*u;
         }
@@ -59,7 +64,7 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio
 
         for(int i = 0; i < f.n_out(); i++)
         {
-            double err = (fw.getOutput(i) - gw.getOutput(i)).lpNorm<Eigen::Infinity>();
+            Real err = (fw.getOutput(i) - gw.getOutput(i)).lpNorm<Eigen::Infinity>();
 
             if(err > 1e-16)
             {
@@ -80,7 +85,8 @@ bool check_function_consistency(const casadi::Function &f, const casadi::Functio
 
 
 
-casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string dir)
+casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string dir,
+                                    bool verbose)
 {    
     // save cwd
     RestoreCwd rcwd = get_current_dir_name();
@@ -102,9 +108,11 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string
     std::string fname = f.name() + "_generated_" + std::to_string(hash);
 
     if(access((fname + ".so").c_str(), F_OK) == 0)
-    {
-        std::cout << "exists: loading " << fname << "... \n";
-
+    {   
+        if (verbose) {
+            std::cout << "exists: loading " << fname << "... \n";
+        }
+
         auto handle = dlopen(("./" + fname + ".so").c_str(), RTLD_NOW);
 
         if(!handle)
@@ -126,20 +134,31 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string
     }
 
     // else, generate and compile
-    f.generate(fname + ".c");
-
-    std::cout << "not found: compiling " << fname << "... \n";
+    #ifdef ILQR_FLOAT
+        casadi::Dict opts={{"casadi_real", "float"}};
+    #else
+        casadi::Dict opts={{"casadi_real", "double"}};
+    #endif
+    f.generate(fname + ".c",opts);
+
+    if (verbose) {
+        std::cout << "not found: compiling " << fname << "... \n";
+    }
 
     int ret = system(("clang -fPIC -shared -O3 -march=native " + fname + ".c -o " + fname + ".so").c_str());
+    // removed -march=native to allow (maybe) more cross compatibility
+    // int ret = system(("clang -fPIC -shared -O3 " + fname + ".c -o " + fname + ".so").c_str());
 
     if(ret != 0)
     {
         std::cout << "failed to compile generated function " << fname << "\n";
         return f;
     }
 
-    std::cout << "loading " << fname << "... \n";
-
+    if (verbose) {
+        std::cout << "loading " << fname << "... \n";
+    }
+
     auto handle = dlopen(("./" + fname + ".so").c_str(), RTLD_NOW);
 
     if(!handle)

horizon/cpp/src/codegen_function.h

@@ -6,7 +6,8 @@
 namespace horizon { namespace utils {
 
 casadi::Function codegen(const casadi::Function& f,
-                         std::string dir=".");
+                        std::string dir=".",
+                        bool verbose = false);
 
 } }