diff --git a/horizon/cpp/CMakeLists.txt b/horizon/cpp/CMakeLists.txt index 5be4a710..088cf03d 100644 --- a/horizon/cpp/CMakeLists.txt +++ b/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) diff --git a/horizon/cpp/pyilqr.cpp b/horizon/cpp/pyilqr.cpp index 038bb883..f7eca490 100644 --- a/horizon/cpp/pyilqr.cpp +++ b/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) ; } diff --git a/horizon/cpp/pyilqr_helpers.h b/horizon/cpp/pyilqr_helpers.h index 84940da6..f1d8c621 100644 --- a/horizon/cpp/pyilqr_helpers.h +++ b/horizon/cpp/pyilqr_helpers.h @@ -8,6 +8,8 @@ #include #include +#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 k, py::object f, - std::vector tgt) + std::vector tgt) { self.setConstraint(k, to_cpp(f), tgt); } diff --git a/horizon/cpp/pysqp_helpers.h b/horizon/cpp/pysqp_helpers.h index b7650fdd..9d75b365 100644 --- a/horizon/cpp/pysqp_helpers.h +++ b/horizon/cpp/pysqp_helpers.h @@ -9,11 +9,11 @@ #include #include #include +#include "src/typedefs.h" namespace py = pybind11; using namespace horizon; - py::dict get_qpoases_options_mpc() { py::dict opts; @@ -21,13 +21,13 @@ py::dict get_qpoases_options_mpc() opts["initialStatusBounds"] = "inactive"; opts["numRefinementSteps"] = 0; opts["enableDriftCorrection"] = 0; - opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); + opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); opts["enableFlippingBounds"] = false; opts["enableNZCTests"] = false; opts["enableRamping"] = false; opts["enableRegularisation"] = true; opts["numRegularisationSteps"] = 2; - opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); + opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::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::epsilon(); + opts["epsLITests"] = 10e5 * std::numeric_limits::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 (key, "beta", value, dict)) return true; - if(setOption (key, "eps_regularization", value, dict)) return true; - if(setOption (key, "alpha_min", value, dict)) return true; + if(setOption (key, "beta", value, dict)) return true; + if(setOption (key, "eps_regularization", value, dict)) return true; + if(setOption (key, "alpha_min", value, dict)) return true; if(setOption (key, "max_iter", value, dict)) return true; if(setOption (key, "reinitialize_qpsolver", value, dict)) return true; - if(setOption (key, "merit_derivative_tolerance", value, dict)) return true; - if(setOption (key, "merit_eps", value, dict)) return true; - if(setOption (key, "constraint_violation_tolerance", value, dict)) return true; - if(setOption (key, "solution_convergence", value, dict)) return true; + if(setOption (key, "merit_derivative_tolerance", value, dict)) return true; + if(setOption (key, "merit_eps", value, dict)) return true; + if(setOption (key, "constraint_violation_tolerance", value, dict)) return true; + if(setOption (key, "solution_convergence", value, dict)) return true; if(setOption (key, "use_golden_ratio_update", value, dict)) return true; // -- qpoases options --// if(setOption (key, "sparse", value, dict)) return true; @@ -78,7 +78,7 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict) if(setOption (key, "max_schur", value, dict)) return true; if(setOption (key, "linsol_plugin", value, dict)) return true; if(setOption (key, "nWSR", value, dict)) return true; - if(setOption (key, "CPUtime", value, dict)) return true; + if(setOption (key, "CPUtime", value, dict)) return true; if(setOption (key, "printLevel", value, dict)) return true; if(setOption (key, "enableRamping", value, dict)) return true; if(setOption (key, "enableFarBounds", value, dict)) return true; @@ -89,42 +89,42 @@ bool checkOptions(const std::string& key, py::handle& value, casadi::Dict& dict) if(setOption (key, "enableDriftCorrection", value, dict)) return true; if(setOption (key, "enableCholeskyRefactorisation", value, dict)) return true; if(setOption (key, "enableEqualities", value, dict)) return true; - if(setOption (key, "terminationTolerance", value, dict)) return true; - if(setOption (key, "boundTolerance", value, dict)) return true; - if(setOption (key, "boundRelaxation", value, dict)) return true; - if(setOption (key, "epsNum", value, dict)) return true; - if(setOption (key, "epsDen", value, dict)) return true; - if(setOption (key, "maxPrimalJump", value, dict)) return true; - if(setOption (key, "maxDualJump", value, dict)) return true; - if(setOption (key, "initialRamping", value, dict)) return true; - if(setOption (key, "finalRamping", value, dict)) return true; - if(setOption (key, "initialFarBounds", value, dict)) return true; - if(setOption (key, "growFarBounds", value, dict)) return true; + if(setOption (key, "terminationTolerance", value, dict)) return true; + if(setOption (key, "boundTolerance", value, dict)) return true; + if(setOption (key, "boundRelaxation", value, dict)) return true; + if(setOption (key, "epsNum", value, dict)) return true; + if(setOption (key, "epsDen", value, dict)) return true; + if(setOption (key, "maxPrimalJump", value, dict)) return true; + if(setOption (key, "maxDualJump", value, dict)) return true; + if(setOption (key, "initialRamping", value, dict)) return true; + if(setOption (key, "finalRamping", value, dict)) return true; + if(setOption (key, "initialFarBounds", value, dict)) return true; + if(setOption (key, "growFarBounds", value, dict)) return true; if(setOption (key, "initialStatusBounds", value, dict)) return true; - if(setOption (key, "epsFlipping", value, dict)) return true; + if(setOption (key, "epsFlipping", value, dict)) return true; if(setOption (key, "numRegularisationSteps", value, dict)) return true; - if(setOption (key, "epsRegularisation", value, dict)) return true; + if(setOption (key, "epsRegularisation", value, dict)) return true; if(setOption (key, "numRefinementSteps", value, dict)) return true; - if(setOption (key, "epsIterRef", value, dict)) return true; - if(setOption (key, "epsLITests", value, dict)) return true; - if(setOption (key, "epsNZCTests", value, dict)) return true; + if(setOption (key, "epsIterRef", value, dict)) return true; + if(setOption (key, "epsLITests", value, dict)) return true; + if(setOption (key, "epsNZCTests", value, dict)) return true; if(setOption (key, "enableInertiaCorrection", value, dict)) return true; // -- osqp options --// if(setOption (key, "warm_start_primal", value, dict)) return true; if(setOption (key, "warm_start_dual", value, dict)) return true; - if(setOption (key, "osqp.rho", value, dict)) return true; - if(setOption (key, "osqp.sigma", value, dict)) return true; + if(setOption (key, "osqp.rho", value, dict)) return true; + if(setOption (key, "osqp.sigma", value, dict)) return true; if(setOption (key, "osqp.scaling", value, dict)) return true; if(setOption (key, "osqp.adaptive_rho", value, dict)) return true; if(setOption (key, "osqp.adaptive_rho_interval", value, dict)) return true; - if(setOption (key, "osqp.adaptive_rho_tolerance", value, dict)) return true; + if(setOption (key, "osqp.adaptive_rho_tolerance", value, dict)) return true; if(setOption (key, "osqp.max_iter", value, dict)) return true; - if(setOption (key, "osqp.eps_abs", value, dict)) return true; - if(setOption (key, "osqp.eps_rel", value, dict)) return true; - if(setOption (key, "osqp.eps_prim_inf", value, dict)) return true; - if(setOption (key, "osqp.eps_dual_inf", value, dict)) return true; - if(setOption (key, "osqp.alpha", value, dict)) return true; - if(setOption (key, "osqp.delta", value, dict)) return true; + if(setOption (key, "osqp.eps_abs", value, dict)) return true; + if(setOption (key, "osqp.eps_rel", value, dict)) return true; + if(setOption (key, "osqp.eps_prim_inf", value, dict)) return true; + if(setOption (key, "osqp.eps_dual_inf", value, dict)) return true; + if(setOption (key, "osqp.alpha", value, dict)) return true; + if(setOption (key, "osqp.delta", value, dict)) return true; if(setOption (key, "osqp.polish", value, dict)) return true; if(setOption (key, "osqp.polish_refine_iter", value, dict)) return true; if(setOption (key, "osqp.verbose", value, dict)) return true; @@ -189,10 +189,10 @@ bool gSX(SQPGaussNewton& self, py::object g, bool reinitialize_qp_so } auto callMX(SQPGaussNewton& 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& 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& 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& 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; } diff --git a/horizon/cpp/src/codegen_function.cpp b/horizon/cpp/src/codegen_function.cpp index 3abab605..74e569fd 100644 --- a/horizon/cpp/src/codegen_function.cpp +++ b/horizon/cpp/src/codegen_function.cpp @@ -8,10 +8,15 @@ #include #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 input(f.n_in()); + std::vector 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(); + Real err = (fw.getOutput(i) - gw.getOutput(i)).lpNorm(); 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,11 +134,20 @@ 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) { @@ -138,8 +155,10 @@ casadi::Function horizon::utils::codegen(const casadi::Function &f, std::string 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) diff --git a/horizon/cpp/src/codegen_function.h b/horizon/cpp/src/codegen_function.h index 171ede33..84ff9f4d 100644 --- a/horizon/cpp/src/codegen_function.h +++ b/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); } } diff --git a/horizon/cpp/src/horizon_parser.cpp b/horizon/cpp/src/horizon_parser.cpp index 93645e7e..c1613e2c 100644 --- a/horizon/cpp/src/horizon_parser.cpp +++ b/horizon/cpp/src/horizon_parser.cpp @@ -15,13 +15,13 @@ Problem::Variable::Ptr Problem::yaml_to_variable(YAML::Node item) try { - auto lb = var_data["lb"].as>(); - auto ub = var_data["ub"].as>(); - auto ini = var_data["initial_guess"].as>(); + auto lb = var_data["lb"].as>(); + auto ub = var_data["ub"].as>(); + auto ini = var_data["initial_guess"].as>(); - v->lb = Eigen::MatrixXd::Map(lb.data(), size, lb.size()/size); - v->ub = Eigen::MatrixXd::Map(ub.data(), size, ub.size()/size); - v->initial_guess = Eigen::MatrixXd::Map(ini.data(), size, ini.size()/size); + v->lb = MatrixXr::Map(lb.data(), size, lb.size()/size); + v->ub = MatrixXr::Map(ub.data(), size, ub.size()/size); + v->initial_guess = MatrixXr::Map(ini.data(), size, ini.size()/size); } catch(YAML::Exception&) { @@ -30,8 +30,8 @@ Problem::Variable::Ptr Problem::yaml_to_variable(YAML::Node item) try { - auto values = var_data["values"].as>(); - v->value = Eigen::MatrixXd::Map(values.data(), size, values.size()/size); + auto values = var_data["values"].as>(); + v->value = MatrixXr::Map(values.data(), size, values.size()/size); } catch(YAML::Exception&) { @@ -90,11 +90,11 @@ Problem::Function::Ptr Problem::yaml_to_function(std::pair>(); - auto ub = var_data["ub"].as>(); + auto lb = var_data["lb"].as>(); + auto ub = var_data["ub"].as>(); - fun->lb = Eigen::MatrixXd::Map(lb.data(), f.size1_out(0), lb.size()/f.size1_out(0)); - fun->ub = Eigen::MatrixXd::Map(ub.data(), f.size1_out(0), ub.size()/f.size1_out(0)); + fun->lb = MatrixXr::Map(lb.data(), f.size1_out(0), lb.size()/f.size1_out(0)); + fun->ub = MatrixXr::Map(ub.data(), f.size1_out(0), ub.size()/f.size1_out(0)); } catch(YAML::Exception& e) { @@ -113,8 +113,8 @@ void Problem::from_yaml(YAML::Node problem_yaml) // dt try { - dt = problem_yaml["solver"]["dt"].as(); - std::cout << "problem_yaml[solver][dt].as() = " << dt << "\n"; + dt = problem_yaml["solver"]["dt"].as(); + std::cout << "problem_yaml[solver][dt].as() = " << dt << "\n"; } catch(YAML::Exception& e) { diff --git a/horizon/cpp/src/horizon_parser.h b/horizon/cpp/src/horizon_parser.h index ebfdacfa..7d2201b7 100644 --- a/horizon/cpp/src/horizon_parser.h +++ b/horizon/cpp/src/horizon_parser.h @@ -9,12 +9,16 @@ #include #include "wrapped_function.h" +#include "typedefs.h" namespace horizon { class Problem { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; public: @@ -30,7 +34,7 @@ class Problem std::string name; casadi::SX sym; - Eigen::MatrixXd lb, ub, value, initial_guess; + MatrixXr lb, ub, value, initial_guess; int size(); }; @@ -41,7 +45,7 @@ class Problem std::string name; casadi::Function fun; - Eigen::MatrixXd lb, ub; + MatrixXr lb, ub; std::vector nodes; }; @@ -52,14 +56,14 @@ class Problem std::vector state_vec, input_vec; casadi::SX x, u; - Eigen::MatrixXd xlb, xub, ulb, uub, x_ini, u_ini; + MatrixXr xlb, xub, ulb, uub, x_ini, u_ini; casadi::Function dynamics; casadi_utils::WrappedFunction inv_dyn; int N; - double dt; + Real dt; private: diff --git a/horizon/cpp/src/ilqr.cpp b/horizon/cpp/src/ilqr.cpp index a99fa255..352d84b5 100644 --- a/horizon/cpp/src/ilqr.cpp +++ b/horizon/cpp/src/ilqr.cpp @@ -3,8 +3,7 @@ #include #include - -utils::Timer::TocCallback on_timer_toc = [](const char*, double){}; +utils::Timer::TocCallback on_timer_toc = [](const char*, Real){}; template std::type_info const& var_type(V const& v){ @@ -36,7 +35,7 @@ T value_or(const IterativeLQR::OptionDict& opt, std::string key, T dfl) } } -void set_param_inputs(std::shared_ptr> params, +void set_param_inputs(std::shared_ptr> params, int k, casadi_utils::WrappedFunction& f) @@ -84,35 +83,39 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, { // set options _hxx_reg_base _verbose = value_or(opt, "ilqr.verbose", 0); + _debug = value_or(opt, "ilqr.debug", 0); _log = value_or(opt, "ilqr.log", 0); + _log_iterations = value_or(opt, "ilqr.log_iterations", 0); + _codegen_verbose = value_or(opt, "ilqr.codegen_verbose", 0); _rti = value_or(opt, "ilqr.rti", 0); - _step_length = value_or(opt, "ilqr.step_length", 1.0); + _step_length = value_or(opt, "ilqr.step_length", 1.0); - _rho_base = value_or(opt, "ilqr.rho_base", 0.0); + _rho_base = value_or(opt, "ilqr.rho_base", 0.0); _rho = _rho_base; - _rho_growth_factor = value_or(opt, "ilqr.rho_growth_factor", 10.0); + _rho_growth_factor = value_or(opt, "ilqr.rho_growth_factor", 10.0); _enable_auglag = value_or(opt, "ilqr.enable_auglag", 0); _use_kkt_solver = value_or(opt, "ilqr.use_kkt_solver", 0); - _hxx_reg = value_or(opt, "ilqr.hxx_reg", 0.0); - _hxx_reg_base = value_or(opt, "ilqr.hxx_reg_base", 0.0); - _hxx_reg_growth_factor = value_or(opt, "ilqr.hxx_reg_growth_factor", 1e3); - _huu_reg = value_or(opt, "ilqr.huu_reg", 0.0); - _kkt_reg = value_or(opt, "ilqr.kkt_reg", 0.0); - _line_search_accept_ratio = value_or(opt, "ilqr.line_search_accept_ratio", 1e-4); - _alpha_min = value_or(opt, "ilqr.alpha_min", 1e-3); - _svd_threshold = value_or(opt, "ilqr.svd_threshold", 1e-6); - _constraint_violation_threshold = value_or(opt, "ilqr.constraint_violation_threshold", 1e-6); - _defect_norm_threshold = value_or(opt, "ilqr.defect_norm_threshold", 1e-6); - _merit_der_threshold = value_or(opt, "ilqr.merit_der_threshold", 1e-3); - _step_length_threshold = value_or(opt, "ilqr.step_length_threshold", 1e-9); + _hxx_reg = value_or(opt, "ilqr.hxx_reg", 0.0); + _hxx_reg_base = value_or(opt, "ilqr.hxx_reg_base", 0.0); + _hxx_reg_growth_factor = value_or(opt, "ilqr.hxx_reg_growth_factor", 1e3); + _huu_reg = value_or(opt, "ilqr.huu_reg", 0.0); + _kkt_reg = value_or(opt, "ilqr.kkt_reg", 0.0); + _line_search_accept_ratio = value_or(opt, "ilqr.line_search_accept_ratio", 1e-4); + _alpha_min = value_or(opt, "ilqr.alpha_min", 1e-3); + _svd_threshold = value_or(opt, "ilqr.svd_threshold", 1e-6); + _constraint_violation_threshold = value_or(opt, "ilqr.constraint_violation_threshold", 1e-6); + _defect_norm_threshold = value_or(opt, "ilqr.defect_norm_threshold", 1e-6); + _merit_der_threshold = value_or(opt, "ilqr.merit_der_threshold", 1e-3); + _step_length_threshold = value_or(opt, "ilqr.step_length_threshold", 1e-9); _closed_loop_forward_pass = value_or(opt, "ilqr.closed_loop_forward_pass", 1); _codegen_workdir = value_or(opt, "ilqr.codegen_workdir", "/tmp"); + _codegen_enabled = value_or(opt, "ilqr.codegen_enabled", 0); _enable_line_search = value_or(opt, "ilqr.enable_line_search", 1); - _it_filt.beta = value_or(opt, "ilqr.filter_beta", 0.99); - _it_filt.gamma = value_or(opt, "ilqr.filter_gamma", 0.20); + _it_filt.beta = value_or(opt, "ilqr.filter_beta", 0.99); + _it_filt.gamma = value_or(opt, "ilqr.filter_gamma", 0.20); _use_it_filter = value_or(opt, "ilqr.use_filter", 0); auto decomp_type_str = value_or(opt, "ilqr.constr_decomp_type", "qr"); @@ -127,7 +130,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, _hxx_reg = std::max(_hxx_reg_base, _hxx_reg); // set timer callback - on_timer_toc = [this](const char * name, double usec) + on_timer_toc = [this](const char * name, Real usec) { _prof_info.timings[name].push_back(usec); }; @@ -141,8 +144,8 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, // codegen if needed if(_codegen_enabled) { - fdyn = utils::codegen(fdyn, _codegen_workdir); - fdyn_jac = utils::codegen(fdyn_jac, _codegen_workdir); + fdyn = utils::codegen(fdyn, _codegen_workdir, _codegen_verbose); + fdyn_jac = utils::codegen(fdyn_jac, _codegen_workdir, _codegen_verbose); } for(auto& d : _dyn) @@ -200,7 +203,7 @@ IterativeLQR::IterativeLQR(cs::Function fdyn, } } -void IterativeLQR::setStateBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub) +void IterativeLQR::setStateBounds(const MatrixXr& lb, const MatrixXr& ub) { if(_x_lb.rows() != lb.rows() || _x_lb.cols() != lb.cols() || _x_ub.rows() != ub.rows() || _x_ub.cols() != ub.cols() @@ -216,7 +219,7 @@ void IterativeLQR::setStateBounds(const Eigen::MatrixXd& lb, const Eigen::Matrix } -void IterativeLQR::setInputBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub) +void IterativeLQR::setInputBounds(const MatrixXr& lb, const MatrixXr& ub) { if(_u_lb.rows() != lb.rows() || _u_lb.cols() != lb.cols() || _u_ub.rows() != ub.rows() || _u_ub.cols() != ub.cols() @@ -238,7 +241,7 @@ void IterativeLQR::setCost(std::vector indices, const casadi::Function& int auto c = std::make_shared(); // initialize constraint value map - _cost_values[inter_cost.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); + _cost_values[inter_cost.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); // add to map _cost_map[inter_cost.name()] = c; @@ -257,9 +260,9 @@ void IterativeLQR::setCost(std::vector indices, const casadi::Function& int // codegen if required (we skip it for quadratic costs) if(_codegen_enabled) { - cost = utils::codegen(cost, _codegen_workdir); - grad = utils::codegen(grad, _codegen_workdir); - hess = utils::codegen(hess, _codegen_workdir); + cost = utils::codegen(cost, _codegen_workdir, _codegen_verbose); + grad = utils::codegen(grad, _codegen_workdir, _codegen_verbose); + hess = utils::codegen(hess, _codegen_workdir, _codegen_verbose); } c->setCost(cost, @@ -293,7 +296,7 @@ void IterativeLQR::setResidual(std::vector indices, // create cost entity auto c = std::make_shared(); -// _cost_values[residual.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); +// _cost_values[residual.name()].setConstant(_N, std::numeric_limits::quiet_NaN()); // add to map _cost_map[residual.name()] = c; @@ -311,8 +314,8 @@ void IterativeLQR::setResidual(std::vector indices, // codegen if required (we skip it for quadratic costs) if(_codegen_enabled) { - res = utils::codegen(res, _codegen_workdir); - jac = utils::codegen(jac, _codegen_workdir); + res = utils::codegen(res, _codegen_workdir, _codegen_verbose); + jac = utils::codegen(jac, _codegen_workdir, _codegen_verbose); } // local syms to evaluate residual and jacobian @@ -360,7 +363,7 @@ void IterativeLQR::setResidual(std::vector indices, c->setCost(cost_fn, grad_fn, hess_fn); - _cost_values[residual.name() + "_cost"].setConstant(_N, std::numeric_limits::quiet_NaN()); + _cost_values[residual.name() + "_cost"].setConstant(_N, std::numeric_limits::quiet_NaN()); if(_verbose) std::cout << "adding residual '" << residual << "' at k = "; @@ -386,7 +389,7 @@ void IterativeLQR::setFinalCost(const casadi::Function &final_cost) void IterativeLQR::setConstraint(std::vector indices, const casadi::Function &inter_constraint, - std::vector target_values) + std::vector target_values) { // add parameters to param_map add_param_to_map(inter_constraint); @@ -399,7 +402,7 @@ void IterativeLQR::setConstraint(std::vector indices, // initialize constraint value map int constr_dim = inter_constraint.size1_out(0); - _constr_values[inter_constraint.name()].setConstant(constr_dim, _N, std::numeric_limits::quiet_NaN()); + _constr_values[inter_constraint.name()].setConstant(constr_dim, _N, std::numeric_limits::quiet_NaN()); // set param map c->param = _param_map; @@ -412,8 +415,8 @@ void IterativeLQR::setConstraint(std::vector indices, if(_codegen_enabled) { - ic_fn = utils::codegen(ic_fn, _codegen_workdir); - ic_jac = utils::codegen(ic_jac, _codegen_workdir); + ic_fn = utils::codegen(ic_fn, _codegen_workdir, _codegen_verbose); + ic_jac = utils::codegen(ic_jac, _codegen_workdir, _codegen_verbose); } c->setConstraint(ic_fn, @@ -519,7 +522,7 @@ void IterativeLQR::updateIndices() } } -void IterativeLQR::setParameterValue(const std::string& pname, const Eigen::MatrixXd& value) +void IterativeLQR::setParameterValue(const std::string& pname, const MatrixXr& value) { auto it = _param_map->find(pname); @@ -544,7 +547,7 @@ void IterativeLQR::setParameterValue(const std::string& pname, const Eigen::Matr it->second = value; } -void IterativeLQR::setInitialState(const Eigen::VectorXd &x0) +void IterativeLQR::setInitialState(const VectorXr &x0) { if(x0.size() != _nx) { @@ -554,7 +557,7 @@ void IterativeLQR::setInitialState(const Eigen::VectorXd &x0) _xtrj.col(0) = x0; } -void IterativeLQR::setStateInitialGuess(const Eigen::MatrixXd& x0) +void IterativeLQR::setStateInitialGuess(const MatrixXr& x0) { if(x0.rows() != _xtrj.rows()) { @@ -569,7 +572,7 @@ void IterativeLQR::setStateInitialGuess(const Eigen::MatrixXd& x0) _xtrj = x0; } -void IterativeLQR::setInputInitialGuess(const Eigen::MatrixXd &u0) +void IterativeLQR::setInputInitialGuess(const MatrixXr &u0) { if(u0.rows() != _utrj.rows()) { @@ -589,12 +592,12 @@ void IterativeLQR::setIterationCallback(const CallbackType &cb) _iter_cb = cb; } -const Eigen::MatrixXd &IterativeLQR::getStateTrajectory() const +const MatrixXr &IterativeLQR::getStateTrajectory() const { return _xtrj; } -const Eigen::MatrixXd &IterativeLQR::getInputTrajectory() const +const MatrixXr &IterativeLQR::getInputTrajectory() const { return _utrj; } @@ -609,16 +612,43 @@ const std::vector& IterativeLQR::getIterationHi return _fp_res_history; } -const std::map &IterativeLQR::getConstraintsValues() const +const VectorXr &IterativeLQR::getConstrValOnNodes() const +{ + return _fp_res->constraint_values; +} + +const std::map &IterativeLQR::getConstraintsValues() const { return _constr_values; } -const std::map &IterativeLQR::getCostsValues() const +const VectorXr &IterativeLQR::getCostValOnNodes() const +{ + return _fp_res->cost_values; +} + +const std::map &IterativeLQR::getCostsValues() const { return _cost_values; } +const float IterativeLQR::getResidualNorm() const +{ + return (_constraint_to_go->C()*_bp_res[0].dx + + _constraint_to_go->h()).lpNorm<1>(); +} + +void IterativeLQR::reset() +{ + _hxx_reg = _hxx_reg_base; + + _fp_res->cost_values.setZero(); + _fp_res->constraint_values.setZero(); + _fp_res->defect_norm=0; + _fp_res->bound_violation=0; + +} + bool IterativeLQR::solve(int max_iter) { // set cost value and constraint violation *before* the forward pass @@ -633,6 +663,12 @@ bool IterativeLQR::solve(int max_iter) // reset counters _fp_accepted = 0; + // reset internal states if not rti mode + if(!_rti) + { + reset(); + } + // solve for(int i = 0; i < max_iter; i++) { @@ -675,7 +711,11 @@ bool IterativeLQR::solve(int max_iter) } } - std::cout << "max iteration reached \n"; + if (_verbose) { + + std::cout << "max iteration reached \n"; + + } return false; } @@ -797,7 +837,7 @@ void IterativeLQR::report_result(const IterativeLQR::ForwardPassResult& fpres) void IterativeLQR::set_default_cost() { - const double dfl_cost_weight = 1e-160; + const Real dfl_cost_weight = 1e-160; auto x = cs::SX::sym("x", _nx); auto u = cs::SX::sym("u", _nu); @@ -912,7 +952,7 @@ void IterativeLQR::add_param_to_map(const casadi::Function& f) // add to map (*_param_map)[f.name_in(i)].setConstant(param_size, _N+1, - std::numeric_limits::quiet_NaN() + std::numeric_limits::quiet_NaN() ); if(_verbose) std::cout << "adding parameter '" << f.name_in(i) << "', " << @@ -920,12 +960,12 @@ void IterativeLQR::add_param_to_map(const casadi::Function& f) } } -const Eigen::MatrixXd &IterativeLQR::Dynamics::A() const +const MatrixXr &IterativeLQR::Dynamics::A() const { return df.getOutput(0); } -const Eigen::MatrixXd &IterativeLQR::Dynamics::B() const +const MatrixXr &IterativeLQR::Dynamics::B() const { return df.getOutput(1); } @@ -935,7 +975,7 @@ IterativeLQR::Dynamics::Dynamics(int nx, int) d.setZero(nx); } -Eigen::Ref IterativeLQR::Dynamics::integrate(VecConstRef x, +Eigen::Ref IterativeLQR::Dynamics::integrate(VecConstRef x, VecConstRef u, int k) { @@ -964,7 +1004,7 @@ void IterativeLQR::Dynamics::computeDefect(VecConstRef x, VecConstRef u, VecConstRef xnext, int k, - Eigen::VectorXd& _d) + VectorXr& _d) { TIC(compute_defect_inner) @@ -997,16 +1037,16 @@ IterativeLQR::BoundAuglagCostEntity::BoundAuglagCostEntity(int N, _ulam.setZero(ulb.size()); } -void IterativeLQR::BoundAuglagCostEntity::setRho(double rho) +void IterativeLQR::BoundAuglagCostEntity::setRho(Real rho) { _rho = rho; } -double IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, +Real IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, VecConstRef u, int k) { - double value = 0.0; + Real value = 0.0; // positive if ub violated, negtive if lb violated _x_violation = (x - _xub).cwiseMax(0) + (x - _xlb).cwiseMin(0); @@ -1028,9 +1068,9 @@ double IterativeLQR::BoundAuglagCostEntity::evaluate(VecConstRef x, void IterativeLQR::BoundAuglagCostEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { evaluate(x, u, k); @@ -1091,17 +1131,17 @@ void IterativeLQR::IntermediateCostEntity::setCost(casadi::Function _l, ddl = _ddl; } -Eigen::Ref IterativeLQR::IntermediateCostEntity::q() const +Eigen::Ref IterativeLQR::IntermediateCostEntity::q() const { return dl.getOutput(0).col(0); } -Eigen::Ref IterativeLQR::IntermediateCostEntity::r() const +Eigen::Ref IterativeLQR::IntermediateCostEntity::r() const { return dl.getOutput(1).col(0); } -double IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, VecConstRef u, int k) { @@ -1119,9 +1159,9 @@ double IterativeLQR::IntermediateCostEntity::evaluate(VecConstRef x, void IterativeLQR::IntermediateCostEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { #ifdef HORIZON_PROFILING horizon::utils::Timer tm("quadratize_" + l.function().name() + "_inner", @@ -1139,7 +1179,7 @@ void IterativeLQR::IntermediateCostEntity::quadratize(VecConstRef x, ddl.setInput(1, u); set_param_inputs(param, k, ddl); - std::vector> out = {Q, R, P}; + std::vector> out = {Q, R, P}; ddl.call_accumulate(out); } @@ -1176,7 +1216,7 @@ void IterativeLQR::IntermediateResidualEntity::setResidual(casadi::Function _res _r.setZero(nu); } -double IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, VecConstRef u, int k) { @@ -1193,9 +1233,9 @@ double IterativeLQR::IntermediateResidualEntity::evaluate(VecConstRef x, void IterativeLQR::IntermediateResidualEntity::quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) { #ifdef HORIZON_PROFILING horizon::utils::Timer tm("quadratize_" + res.function().name() + "_inner", @@ -1241,17 +1281,17 @@ casadi::Function IterativeLQR::IntermediateResidualEntity::Jacobian(const casadi return f.factory(f.name() + "_jac", f.name_in(), {"jac:res:x", "jac:res:u"}); } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::Q() const +const MatrixXr& IterativeLQR::IntermediateCost::Q() const { return _Q; } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::R() const +const MatrixXr& IterativeLQR::IntermediateCost::R() const { return _R; } -const Eigen::MatrixXd& IterativeLQR::IntermediateCost::P() const +const MatrixXr& IterativeLQR::IntermediateCost::P() const { return _P; } @@ -1275,13 +1315,13 @@ IterativeLQR::IntermediateCost::IntermediateCost(int nx, int nu) _r.setZero(nu); } -double IterativeLQR::IntermediateCost::evaluate(VecConstRef x, +Real IterativeLQR::IntermediateCost::evaluate(VecConstRef x, VecConstRef u, int k) { TIC(evaluate_cost_inner); - double cost = 0.0; + Real cost = 0.0; for(auto& it : items) { @@ -1348,6 +1388,7 @@ IterativeLQR::ForwardPassResult::ForwardPassResult(int nx, int nu, int N): utrj.setZero(nu, N); merit = 0.0; step_length = 0.0; + cost_values.setZero(N+1); constraint_values.setZero(N+1); defect_values.setZero(nx, N); @@ -1454,7 +1495,7 @@ int IterativeLQR::ConstraintToGo::dim() const return _dim; } -Eigen::Ref IterativeLQR::ConstraintToGo::C() const +Eigen::Ref IterativeLQR::ConstraintToGo::C() const { return _C.topRows(_dim); } @@ -1464,22 +1505,22 @@ MatConstRef IterativeLQR::ConstraintToGo::D() const return _D.topRows(_dim); } -Eigen::Ref IterativeLQR::ConstraintToGo::h() const +Eigen::Ref IterativeLQR::ConstraintToGo::h() const { return _h.head(_dim); } -const Eigen::MatrixXd &IterativeLQR::ConstraintEntity::C() const +const MatrixXr &IterativeLQR::ConstraintEntity::C() const { return df.getOutput(0); } -const Eigen::MatrixXd &IterativeLQR::ConstraintEntity::D() const +const MatrixXr &IterativeLQR::ConstraintEntity::D() const { return df.getOutput(1); } -Eigen::Ref IterativeLQR::ConstraintEntity::h() const +Eigen::Ref IterativeLQR::ConstraintEntity::h() const { return _hvalue; } @@ -1546,7 +1587,7 @@ void IterativeLQR::ConstraintEntity::setConstraint(casadi::Function h, casadi::F _hdes.setZero(f.function().size1_out(0)); } -void IterativeLQR::ConstraintEntity::setTargetValue(const Eigen::VectorXd &hdes) +void IterativeLQR::ConstraintEntity::setTargetValue(const VectorXr &hdes) { if(hdes.size() != _hdes.size()) { @@ -1561,12 +1602,12 @@ casadi::Function IterativeLQR::ConstraintEntity::Jacobian(const casadi::Function return h.factory(h.name() + "_jac", h.name_in(), {"jac:h:x", "jac:h:u"}); } -const Eigen::MatrixXd &IterativeLQR::Constraint::C() const +const MatrixXr &IterativeLQR::Constraint::C() const { return _C; } -const Eigen::MatrixXd &IterativeLQR::Constraint::D() const +const MatrixXr &IterativeLQR::Constraint::D() const { return _D; } diff --git a/horizon/cpp/src/ilqr.h b/horizon/cpp/src/ilqr.h index 84b3975f..f12314e0 100644 --- a/horizon/cpp/src/ilqr.h +++ b/horizon/cpp/src/ilqr.h @@ -13,13 +13,13 @@ #include "profiling.h" #include "iterate_filter.h" +#include "typedefs.h" namespace horizon { -typedef Eigen::Ref VecConstRef; -typedef Eigen::Ref MatConstRef; - +typedef Eigen::Ref VecConstRef; +typedef Eigen::Ref MatConstRef; /** * @brief IterativeLQR implements a multiple-shooting variant of the @@ -49,7 +49,7 @@ class IterativeLQR */ typedef std::function CallbackType; - typedef std::variant OptionTypes; + typedef std::variant OptionTypes; typedef std::map OptionDict; @@ -64,9 +64,9 @@ class IterativeLQR OptionDict opt = OptionDict()); - void setStateBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub); + void setStateBounds(const MatrixXr& lb, const MatrixXr& ub); - void setInputBounds(const Eigen::MatrixXd& lb, const Eigen::MatrixXd& ub); + void setInputBounds(const MatrixXr& lb, const MatrixXr& ub); /** * @brief set an intermediate cost term for the k-th intermediate state, @@ -96,7 +96,7 @@ class IterativeLQR */ void setConstraint(std::vector indices, const casadi::Function& inter_constraint, - std::vector target_values = std::vector()); + std::vector target_values = std::vector()); void setFinalConstraint(const casadi::Function& final_constraint); @@ -105,29 +105,37 @@ class IterativeLQR void updateIndices(); - void setParameterValue(const std::string& pname, const Eigen::MatrixXd& value); + void setParameterValue(const std::string& pname, const MatrixXr& value); - void setInitialState(const Eigen::VectorXd& x0); + void setInitialState(const VectorXr& x0); - void setStateInitialGuess(const Eigen::MatrixXd& x0); + void setStateInitialGuess(const MatrixXr& x0); - void setInputInitialGuess(const Eigen::MatrixXd& u0); + void setInputInitialGuess(const MatrixXr& u0); void setIterationCallback(const CallbackType& cb); + + void reset(); bool solve(int max_iter); - const Eigen::MatrixXd& getStateTrajectory() const; + const MatrixXr& getStateTrajectory() const; - const Eigen::MatrixXd& getInputTrajectory() const; + const MatrixXr& getInputTrajectory() const; const utils::ProfilingInfo& getProfilingInfo() const; const std::vector& getIterationHistory() const; - const std::map& getConstraintsValues() const; + const VectorXr& getCostValOnNodes() const; + + const std::map& getConstraintsValues() const; + + const VectorXr& getConstrValOnNodes() const; - const std::map& getCostsValues() const; + const std::map& getCostsValues() const; + + const float getResidualNorm() const; VecConstRef state(int i) const; @@ -139,28 +147,29 @@ class IterativeLQR struct ForwardPassResult { - Eigen::MatrixXd xtrj; - Eigen::MatrixXd utrj; - double hxx_reg; - double rho; - double alpha; - double cost; - double bound_violation; - double merit; - double armijo_merit; - double mu_f; - double mu_c; - double mu_b; - double f_der; - double merit_der; - double step_length; - double constraint_violation; - double defect_norm; + MatrixXr xtrj; + MatrixXr utrj; + Real hxx_reg; + Real rho; + Real alpha; + Real cost; + Real bound_violation; + Real merit; + Real armijo_merit; + Real mu_f; + Real mu_c; + Real mu_b; + Real f_der; + Real merit_der; + Real step_length; + Real constraint_violation; + Real defect_norm; int iter; bool accepted; - Eigen::VectorXd constraint_values; - Eigen::MatrixXd defect_values; + VectorXr cost_values; + VectorXr constraint_values; + MatrixXr defect_values; ForwardPassResult(int nx, int nu, int N); @@ -168,13 +177,11 @@ class IterativeLQR }; - - protected: private: - static constexpr double inf = std::numeric_limits::infinity(); + static constexpr Real inf = std::numeric_limits::infinity(); struct ConstrainedDynamics; struct ConstrainedCost; @@ -195,7 +202,7 @@ class IterativeLQR typedef std::tuple HandleConstraintsRetType; - typedef std::shared_ptr> + typedef std::shared_ptr> ParameterMapPtr; typedef std::map> @@ -236,35 +243,35 @@ class IterativeLQR bool auglag_update(); - double compute_merit_value(double mu_f, - double mu_c, - double cost, - double defect_norm, - double constr_viol); + Real compute_merit_value(Real mu_f, + Real mu_c, + Real cost, + Real defect_norm, + Real constr_viol); - double compute_merit_slope(double cost_slope, - double mu_f, - double mu_c, - double defect_norm, - double constr_viol); + Real compute_merit_slope(Real cost_slope, + Real mu_f, + Real mu_c, + Real defect_norm, + Real constr_viol); - double compute_cost_slope(); + Real compute_cost_slope(); - std::pair compute_merit_weights(double cost_der, double defect_norm, double constr_viol); + std::pair compute_merit_weights(Real cost_der, Real defect_norm, Real constr_viol); - double compute_cost(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_cost(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_bound_penalty(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_bound_penalty(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_constr(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_constr(const MatrixXr& xtrj, + const MatrixXr& utrj); - double compute_defect(const Eigen::MatrixXd& xtrj, - const Eigen::MatrixXd& utrj); + Real compute_defect(const MatrixXr& xtrj, + const MatrixXr& utrj); - bool forward_pass(double alpha); + bool forward_pass(Real alpha); bool line_search(int iter); @@ -276,8 +283,6 @@ class IterativeLQR bool fixed_initial_state(); - - enum DecompositionType { Ldlt, Qr, Lu, Cod, Svd, ReducedHessian @@ -286,29 +291,32 @@ class IterativeLQR static DecompositionType str_to_decomp_type(const std::string& dt_str); bool _verbose; + bool _debug; + bool _log_iterations; bool _log; bool _rti; - + bool _codegen_verbose; + const int _nx; const int _nu; const int _N; - double _step_length; - double _rho_base; - double _rho; - double _rho_growth_factor; - double _hxx_reg; - double _hxx_reg_base; - double _hxx_reg_growth_factor; - double _huu_reg; - double _kkt_reg; - double _line_search_accept_ratio; - double _alpha_min; - double _svd_threshold; - double _constraint_violation_threshold; - double _defect_norm_threshold; - double _merit_der_threshold; - double _step_length_threshold; + Real _step_length; + Real _rho_base; + Real _rho; + Real _rho_growth_factor; + Real _hxx_reg; + Real _hxx_reg_base; + Real _hxx_reg_growth_factor; + Real _huu_reg; + Real _kkt_reg; + Real _line_search_accept_ratio; + Real _alpha_min; + Real _svd_threshold; + Real _constraint_violation_threshold; + Real _defect_norm_threshold; + Real _merit_der_threshold; + Real _step_length_threshold; bool _enable_line_search; bool _enable_auglag; bool _use_kkt_solver; @@ -326,8 +334,8 @@ class IterativeLQR std::vector> _auglag_cost; std::vector _cost; std::vector _constraint; - Eigen::MatrixXd _x_lb, _x_ub; - Eigen::MatrixXd _u_lb, _u_ub; + MatrixXr _x_lb, _x_ub; + MatrixXr _u_lb, _u_ub; std::vector _value; std::vector _dyn; @@ -336,22 +344,22 @@ class IterativeLQR std::unique_ptr _fp_res; int _fp_accepted; - std::vector> _kkt_triplets; - Eigen::SparseMatrix _kkt_mat; - Eigen::VectorXd _kkt_rhs; - Eigen::SparseLU, Eigen::COLAMDOrdering> _kkt_lu_solver; - Eigen::SimplicialLDLT, Eigen::Lower, Eigen::COLAMDOrdering> _kkt_ldlt_solver; + std::vector> _kkt_triplets; + Eigen::SparseMatrix _kkt_mat; + VectorXr _kkt_rhs; + Eigen::SparseLU, Eigen::COLAMDOrdering> _kkt_lu_solver; + Eigen::SimplicialLDLT, Eigen::Lower, Eigen::COLAMDOrdering> _kkt_ldlt_solver; IterateFilter _it_filt; bool _use_it_filter; - Eigen::MatrixXd _xtrj; - Eigen::MatrixXd _utrj; - std::vector _lam_g; - Eigen::MatrixXd _lam_x; + MatrixXr _xtrj; + MatrixXr _utrj; + std::vector _lam_g; + MatrixXr _lam_x; - Eigen::MatrixXd _lam_bound_x; - Eigen::MatrixXd _lam_bound_u; + MatrixXr _lam_bound_x; + MatrixXr _lam_bound_u; Eigen::MatrixXd _dx, _du; @@ -371,8 +379,8 @@ class IterativeLQR std::vector _fp_res_history; - std::map _constr_values; - std::map _cost_values; + std::map _constr_values; + std::map _cost_values; }; diff --git a/horizon/cpp/src/ilqr_backward_pass.cpp b/horizon/cpp/src/ilqr_backward_pass.cpp index 2a4b9869..0cc03102 100644 --- a/horizon/cpp/src/ilqr_backward_pass.cpp +++ b/horizon/cpp/src/ilqr_backward_pass.cpp @@ -1,4 +1,5 @@ #include "ilqr_impl.h" +#include "typedefs.h" struct HessianIndefinite : std::runtime_error { @@ -54,23 +55,26 @@ void IterativeLQR::backward_pass() // some of them could be infeasible unless the initial // satisfies them already, let's check the residual // from the computed dx[0] - Eigen::VectorXd residual; + VectorXr residual; residual = _constraint_to_go->C()*_bp_res[0].dx + _constraint_to_go->h(); // infeasible warning if(residual.lpNorm<1>() > 1e-8) { + if (_debug && _verbose) { - std::cout << "warn at k = 0: " << _constraint_to_go->dim() << - " linearized constraints not satified, residual inf-norm is " << - residual.lpNorm() << "\n"; + std::cout << "warn at k = 0: " << _constraint_to_go->dim() << + " constraints not satified, residual inf-norm is " << + residual.lpNorm() << "\n"; - if(_log) - { - std::cout << "C = \n" << _constraint_to_go->C().format(2) << "\n" << - "h = " << _constraint_to_go->h().transpose().format(2) << "\n"; + if(_log) + { + std::cout << "C = \n" << _constraint_to_go->C().format(2) << "\n" << + "h = " << _constraint_to_go->h().transpose().format(2) << "\n"; + } } + } } @@ -149,31 +153,31 @@ void IterativeLQR::backward_pass_iter(int i) // print if(_log) { - Eigen::MatrixXd H(_nu+_nx, _nu+_nx); + MatrixXr H(_nu+_nx, _nu+_nx); H << tmp.Hxx, tmp.Hux.transpose(), - tmp.Hux, tmp.Huu; - Eigen::VectorXd eigH = H.eigenvalues().real(); + tmp.Hux, tmp.Huu; + VectorXr eigH = H.eigenvalues().real(); std::cout << "eig(H[" << i << "]) in [" << eigH.minCoeff() << ", " << eigH.maxCoeff() << "] \n"; std::cout << "H symmetry error = " << (H - H.transpose()).lpNorm() << "\n"; - Eigen::MatrixXd V(_nu+_nx, _nu+_nx); + MatrixXr V(_nu+_nx, _nu+_nx); V << Q, P.transpose(), - P, R; - Eigen::VectorXd eigV = V.eigenvalues().real(); + P, R; + VectorXr eigV = V.eigenvalues().real(); std::cout << "eig(V[" << i << "]) in [" << eigV.minCoeff() << ", " << eigV.maxCoeff() << "] \n"; std::cout << "V symmetry error = " << (V - V.transpose()).lpNorm() << "\n"; - Eigen::VectorXd eigS = Snext.eigenvalues().real(); + VectorXr eigS = Snext.eigenvalues().real(); std::cout << "eig(S[" << i+1 << "]) in [" << eigS.minCoeff() << ", " << eigS.maxCoeff() << "] \n"; - Eigen::VectorXd eigHuu = tmp.Huu.eigenvalues().real(); + VectorXr eigHuu = tmp.Huu.eigenvalues().real(); std::cout << "eig(Huu[" << i << "]) in [" << eigHuu.minCoeff() << ", " << eigHuu.maxCoeff() << "] \n"; } @@ -311,8 +315,8 @@ void IterativeLQR::backward_pass_iter(int i) for(int j = 0; j < tmp.hinf.size(); j++) { // i-th infeasible constraint is in the form 0x = 0 - double hnorm = std::fabs(tmp.hinf[j]); - double Cnorm = tmp.Cinf.row(j).lpNorm(); + Real hnorm = std::fabs(tmp.hinf[j]); + Real Cnorm = tmp.Cinf.row(j).lpNorm(); if(hnorm < 1e-16 && Cnorm < 1e-16) { if(_verbose) @@ -335,8 +339,8 @@ void IterativeLQR::backward_pass_iter(int i) void IterativeLQR::optimize_initial_state() { - Eigen::VectorXd& dx = _bp_res[0].dx; - Eigen::VectorXd& lam = _bp_res[0].dx_lam; + VectorXr& dx = _bp_res[0].dx; + VectorXr& lam = _bp_res[0].dx_lam; // typical case: initial state is fixed if(fixed_initial_state()) @@ -359,14 +363,14 @@ void IterativeLQR::optimize_initial_state() auto Csvd = C.jacobiSvd(); Csvd.setThreshold(_svd_threshold); - Eigen::VectorXd svC = Csvd.singularValues(); + VectorXr svC = Csvd.singularValues(); std::cout << "sv(C[" << 0 << "]) in [" << svC.minCoeff() << ", " << svC.maxCoeff() << "], rank = " << Csvd.rank() << "\n"; } // construct kkt matrix TIC(construct_state_kkt); - Eigen::MatrixXd& K = _tmp[0].x_kkt; + MatrixXr& K = _tmp[0].x_kkt; K.resize(s.size() + h.size(), s.size() + h.size()); K.topLeftCorner(S.rows(), S.cols()) = S; K.topRightCorner(C.cols(), C.rows()) = C.transpose(); @@ -375,7 +379,7 @@ void IterativeLQR::optimize_initial_state() TOC(construct_state_kkt); // residual vector - Eigen::VectorXd k = _tmp[0].x_k0; + VectorXr k = _tmp[0].x_k0; k.resize(s.size() + h.size()); k << -s, -h; @@ -388,7 +392,7 @@ void IterativeLQR::optimize_initial_state() auto& lu = _tmp[0].x_lu; auto& qr = _tmp[0].x_qr; auto& ldlt = _tmp[0].x_ldlt; - Eigen::VectorXd& dx_lam = _tmp[0].dx_lam; + VectorXr& dx_lam = _tmp[0].dx_lam; switch(_kkt_decomp_type) { @@ -419,7 +423,7 @@ void IterativeLQR::optimize_initial_state() if(_log) { - Eigen::VectorXd eigS = S.eigenvalues().real(); + VectorXr eigS = S.eigenvalues().real(); std::cout << "eig(S[" << 0 << "]) in [" << eigS.minCoeff() << ", " << eigS.maxCoeff() << "] \n"; @@ -432,8 +436,8 @@ void IterativeLQR::optimize_initial_state() lam = dx_lam.tail(h.size()); // check constraints - Eigen::MatrixXd Cinf = C; - Eigen::VectorXd hinf = h; + MatrixXr Cinf = C; + VectorXr hinf = h; _constraint_to_go->clear(); @@ -455,7 +459,7 @@ void IterativeLQR::optimize_initial_state() void IterativeLQR::add_bound_constraint(int k) { - Eigen::RowVectorXd x_ei, u_ei; + RowVectorXr x_ei, u_ei; // state bounds u_ei.setZero(_nu); @@ -473,7 +477,7 @@ void IterativeLQR::add_bound_constraint(int k) { x_ei = x_ei.Unit(_nx, i); - Eigen::Matrix hd; + Eigen::Matrix hd; hd(0) = _xtrj(i, k) - _x_lb(i, k); _constraint_to_go->add(x_ei, u_ei, hd); @@ -501,7 +505,7 @@ void IterativeLQR::add_bound_constraint(int k) { u_ei = u_ei.Unit(_nu, i); - Eigen::Matrix hd; + Eigen::Matrix hd; hd(0) = _utrj(i, k) - _u_lb(i, k); _constraint_to_go->add(x_ei, u_ei, hd); @@ -648,9 +652,9 @@ IterativeLQR::FeasibleConstraint IterativeLQR::handle_constraints(int i) } // decompose constraint into a feasible and infeasible components - Eigen::MatrixXd Ctmp = _constraint_to_go->C(); - Eigen::MatrixXd Dtmp = _constraint_to_go->D(); - Eigen::VectorXd htmp = _constraint_to_go->h(); + MatrixXr Ctmp = _constraint_to_go->C(); + MatrixXr Dtmp = _constraint_to_go->D(); + VectorXr htmp = _constraint_to_go->h(); TOC(constraint_prepare_inner); THROW_NAN(Ctmp); THROW_NAN(Dtmp); @@ -663,14 +667,14 @@ IterativeLQR::FeasibleConstraint IterativeLQR::handle_constraints(int i) // it is rather common for D to contain exact zero rows, // we can directly consider them as unsatisfied constr _constraint_to_go->clear(); - Eigen::MatrixXd C(Ctmp.rows(), Ctmp.cols()); - Eigen::MatrixXd D(Dtmp.rows(), Dtmp.cols()); - Eigen::VectorXd h(htmp.size()); + MatrixXr C(Ctmp.rows(), Ctmp.cols()); + MatrixXr D(Dtmp.rows(), Dtmp.cols()); + VectorXr h(htmp.size()); int pruned_idx = 0; for(int j = 0; j < h.size(); j++) { - double Dnorm = Dtmp.row(j).lpNorm(); + Real Dnorm = Dtmp.row(j).lpNorm(); if(Dnorm == 0) { _constraint_to_go->add(Ctmp.row(j), diff --git a/horizon/cpp/src/ilqr_forward_pass.cpp b/horizon/cpp/src/ilqr_forward_pass.cpp index 9d8fde82..c48ff971 100644 --- a/horizon/cpp/src/ilqr_forward_pass.cpp +++ b/horizon/cpp/src/ilqr_forward_pass.cpp @@ -1,7 +1,7 @@ #include "ilqr_impl.h" +#include "typedefs.h" - -bool IterativeLQR::forward_pass(double alpha) +bool IterativeLQR::forward_pass(Real alpha) { TIC(forward_pass); @@ -23,12 +23,11 @@ bool IterativeLQR::forward_pass(double alpha) return true; } - -double IterativeLQR::compute_merit_slope(double cost_slope, - double mu_f, - double mu_c, - double defect_norm, - double constr_viol) +Real IterativeLQR::compute_merit_slope(Real cost_slope, + Real mu_f, + Real mu_c, + Real defect_norm, + Real constr_viol) { // see Nocedal and Wright, Theorem 18.2, pg. 541 // available online http://www.apmath.spbu.ru/cnsa/pdf/monograf/Numerical_Optimization2006.pdf @@ -37,11 +36,11 @@ double IterativeLQR::compute_merit_slope(double cost_slope, } -double IterativeLQR::compute_cost_slope() +Real IterativeLQR::compute_cost_slope() { TIC(compute_cost_slope); - double der = 0.; + Real der = 0.; for(int i = 0; i < _N; i++) { @@ -56,11 +55,11 @@ double IterativeLQR::compute_cost_slope() return der; } -double IterativeLQR::compute_merit_value(double mu_f, - double mu_c, - double cost, - double defect_norm, - double constr_viol) +Real IterativeLQR::compute_merit_value(Real mu_f, + Real mu_c, + Real cost, + Real defect_norm, + Real constr_viol) { // we define a merit function as follows // m(alpha) = J + mu_f * |D| + mu_c * |G| @@ -76,19 +75,18 @@ double IterativeLQR::compute_merit_value(double mu_f, return cost + mu_f*defect_norm + mu_c*constr_viol; } - -std::pair IterativeLQR::compute_merit_weights( - double cost_der, - double defect_norm, - double constr_viol) +std::pair IterativeLQR::compute_merit_weights( + Real cost_der, + Real defect_norm, + Real constr_viol) { TIC(compute_merit_weights); // note: we here assume dx = 0, since this function runs before // the forward pass - double lam_x_max = 0.0; - double lam_g_max = 0.0; + Real lam_x_max = 0.0; + Real lam_g_max = 0.0; for(int i = 0; i < _N; i++) { @@ -102,9 +100,9 @@ std::pair IterativeLQR::compute_merit_weights( } } - const double merit_safety_factor = 2.0; - double mu_f = lam_x_max * merit_safety_factor; - double mu_c = std::max(lam_g_max * merit_safety_factor, 0.0); + const Real merit_safety_factor = 2.0; + Real mu_f = lam_x_max * merit_safety_factor; + Real mu_c = std::max(lam_g_max * merit_safety_factor, static_cast(0.0)); // double g = defect_norm + mu_c/mu_f*constr_viol; // double rho = 0.5; @@ -115,30 +113,36 @@ std::pair IterativeLQR::compute_merit_weights( } -double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_cost(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_cost); - double cost = 0.0; + Real cost = 0.0; - // reset constr value to nan - for(auto& item : _cost_values) - { - item.second.setConstant(std::numeric_limits::quiet_NaN()); - } + if (_debug) { + // reset constr value to nan + for(auto& item : _cost_values) + { + item.second.setConstant(std::numeric_limits::quiet_NaN()); + } + } + // intermediate cost for(int i = 0; i < _N; i++) { - cost += _cost[i].evaluate(xtrj.col(i), utrj.col(i), i) / _N; - - // optionally (TBD) save values of single costs acting on this node - for(auto it : _cost[i].items) - { - // not updating items uninitialized (item vs item_cost) - if (_cost_values[it->getName()].size() != 0) + _fp_res->cost_values[i] = _cost[i].evaluate(xtrj.col(i), utrj.col(i), i); + cost += _fp_res->cost_values[i] / _N; + + if (_debug) { + // optionally save values of single costs acting on this node + for(auto it : _cost[i].items) { - _cost_values[it->getName()](i) = it->getCostEvaluated(); + // not updating items uninitialized (item vs item_cost) + if (_cost_values[it->getName()].size() != 0) + { + _cost_values[it->getName()](i) = it->getCostEvaluated(); + } } } @@ -147,17 +151,18 @@ double IterativeLQR::compute_cost(const Eigen::MatrixXd& xtrj, const Eigen::Matr // add final cost // note: u not used // todo: enforce this! - cost += _cost[_N].evaluate(xtrj.col(_N), utrj.col(_N-1), _N); + _fp_res->cost_values[_N] = _cost[_N].evaluate(xtrj.col(_N), utrj.col(_N-1), _N); + cost += _fp_res->cost_values[_N]; return cost; } -double IterativeLQR::compute_bound_penalty(const Eigen::MatrixXd &xtrj, - const Eigen::MatrixXd &utrj) +Real IterativeLQR::compute_bound_penalty(const MatrixXr &xtrj, + const MatrixXr &utrj) { TIC(compute_bound_penalty); - double res = 0.0; + Real res = 0.0; auto xineq = _x_lb.array() < _x_ub.array(); auto uineq = _u_lb.array() < _u_ub.array(); @@ -170,18 +175,22 @@ double IterativeLQR::compute_bound_penalty(const Eigen::MatrixXd &xtrj, return res / _N; } -double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_constr(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_constr); - double constr = 0.0; + Real constr = 0.0; - // reset constr value to nan - for(auto& item : _constr_values) - { - item.second.setConstant(std::numeric_limits::quiet_NaN()); - } + if (_debug) { + // reset constr value to nan + for(auto& item : _constr_values) + { + item.second.setConstant(std::numeric_limits::quiet_NaN()); + } + + } + // intermediate constraint violation for(int i = 0; i < _N; i++) { @@ -194,10 +203,12 @@ double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::Ma _fp_res->constraint_values[i] = _constraint[i].h().lpNorm<1>(); constr += _fp_res->constraint_values[i] / _N; - // optionally (TBD) save values of single constraints acting on this node - for(auto it : _constraint[i].items) - { - _constr_values[it->f.function().name()].col(i) = it->h(); + if (_debug) { + // optionally (TBD) save values of single constraints acting on this node + for(auto it : _constraint[i].items) + { + _constr_values[it->f.function().name()].col(i) = it->h(); + } } } @@ -222,11 +233,11 @@ double IterativeLQR::compute_constr(const Eigen::MatrixXd& xtrj, const Eigen::Ma return constr; } -double IterativeLQR::compute_defect(const Eigen::MatrixXd& xtrj, const Eigen::MatrixXd& utrj) +Real IterativeLQR::compute_defect(const MatrixXr& xtrj, const MatrixXr& utrj) { TIC(compute_defect); - double defect = 0.0; + Real defect = 0.0; // compute defects on given trajectory for(int i = 0; i < _N; i++) @@ -249,17 +260,16 @@ bool IterativeLQR::line_search(int iter) { TIC(line_search); - const double step_reduction_factor = 0.5; - const double alpha_min = _alpha_min; - double alpha = _step_length; - const double eta = _line_search_accept_ratio; + const Real step_reduction_factor = 0.5; + const Real alpha_min = _alpha_min; + Real alpha = _step_length; + const Real eta = _line_search_accept_ratio; // fill newton step length _fp_res->step_length = std::sqrt(_dx.squaredNorm() + _du.squaredNorm()); - // compute merit function weights - double cost_der = compute_cost_slope(); + Real cost_der = compute_cost_slope(); auto [mu_f, mu_c] = compute_merit_weights( cost_der, _fp_res->defect_norm, @@ -270,14 +280,14 @@ bool IterativeLQR::line_search(int iter) _fp_res->rho = _rho; // compute merit function initial value - double merit = compute_merit_value(mu_f, mu_c, + Real merit = compute_merit_value(mu_f, mu_c, _fp_res->cost, _fp_res->defect_norm, _fp_res->constraint_violation); // compute merit function directional derivative - double merit_der = compute_merit_slope(cost_der, + Real merit_der = compute_merit_slope(cost_der, mu_f, mu_c, _fp_res->defect_norm, _fp_res->constraint_violation); @@ -380,7 +390,9 @@ bool IterativeLQR::line_search(int iter) _utrj = _fp_res->utrj; // save result in history - _fp_res_history.push_back(*_fp_res); + if (_log_iterations) { + _fp_res_history.push_back(*_fp_res); + } // note: we should update the lag mult at the solution // by including the dx part @@ -392,7 +404,7 @@ void IterativeLQR::reset_iterate_filter() { _it_filt.clear(); IterateFilter::Pair test_pair; - test_pair.f = std::numeric_limits::lowest(); + test_pair.f = std::numeric_limits::lowest(); test_pair.h = _fp_res->defect_norm + _fp_res->constraint_violation; test_pair.h = std::max(1e2*test_pair.h, 1e3); } @@ -400,10 +412,10 @@ void IterativeLQR::reset_iterate_filter() bool IterativeLQR::should_stop() { - const double constraint_violation_threshold = _constraint_violation_threshold; - const double defect_norm_threshold = _defect_norm_threshold; - const double merit_der_threshold = _merit_der_threshold; - const double step_length_threshold = _step_length_threshold; + const Real constraint_violation_threshold = _constraint_violation_threshold; + const Real defect_norm_threshold = _defect_norm_threshold; + const Real merit_der_threshold = _merit_der_threshold; + const Real step_length_threshold = _step_length_threshold; TIC(should_stop); @@ -424,21 +436,24 @@ bool IterativeLQR::should_stop() return false; } - // here we're feasible // exit if merit function directional derivative (normalized) // is too close to zero if(std::fabs(_fp_res->merit_der) < merit_der_threshold*(1 + _fp_res->merit)) { - std::cout << "exiting due to small merit derivative \n"; + if (_verbose) { + std::cout << "exiting due to small merit derivative \n"; + } return true; } // exit if step size (normalized) is too short if(_fp_res->step_length < step_length_threshold*(1 + _utrj.norm())) { - std::cout << "exiting due to small control increment \n"; + if (_verbose) { + std::cout << "exiting due to small control increment \n"; + } return true; } diff --git a/horizon/cpp/src/ilqr_impl.h b/horizon/cpp/src/ilqr_impl.h index 4ff960d5..df06bf69 100644 --- a/horizon/cpp/src/ilqr_impl.h +++ b/horizon/cpp/src/ilqr_impl.h @@ -3,17 +3,21 @@ #include "ilqr.h" #include "wrapped_function.h" +#include "typedefs.h" using namespace horizon; using namespace casadi_utils; +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + namespace cs = casadi; extern utils::Timer::TocCallback on_timer_toc; struct IterativeLQR::Dynamics { - public: // dynamics function @@ -26,13 +30,13 @@ struct IterativeLQR::Dynamics ParameterMapPtr param; // df/dx - const Eigen::MatrixXd& A() const; + const MatrixXr& A() const; // df/du - const Eigen::MatrixXd& B() const; + const MatrixXr& B() const; // defect (or gap) - Eigen::VectorXd d; + VectorXr d; Dynamics(int nx, int nu); @@ -48,7 +52,7 @@ struct IterativeLQR::Dynamics VecConstRef u, VecConstRef xnext, int k, - Eigen::VectorXd& d); + VectorXr& d); void setDynamics(casadi::Function f); @@ -58,6 +62,7 @@ struct IterativeLQR::Dynamics struct IterativeLQR::ConstraintEntity { + typedef std::shared_ptr Ptr; // constraint function @@ -73,10 +78,10 @@ struct IterativeLQR::ConstraintEntity std::vector indices; // dh/dx - const Eigen::MatrixXd& C() const; + const MatrixXr& C() const; // dh/du - const Eigen::MatrixXd& D() const; + const MatrixXr& D() const; // constraint violation h(x, u) - hdes VecConstRef h() const; @@ -94,27 +99,28 @@ struct IterativeLQR::ConstraintEntity void setConstraint(casadi::Function h, casadi::Function dh); - void setTargetValue(const Eigen::VectorXd& hdes); + void setTargetValue(const VectorXr& hdes); static casadi::Function Jacobian(const casadi::Function& h); private: // desired value - Eigen::VectorXd _hdes; + VectorXr _hdes; // computed value - Eigen::VectorXd _hvalue; + VectorXr _hvalue; }; struct IterativeLQR::Constraint { + // dh/dx - const Eigen::MatrixXd& C() const; + const MatrixXr& C() const; // dh/du - const Eigen::MatrixXd& D() const; + const MatrixXr& D() const; // constraint violation f(x, u) VecConstRef h() const; @@ -139,14 +145,15 @@ struct IterativeLQR::Constraint private: - Eigen::MatrixXd _C; - Eigen::MatrixXd _D; - Eigen::VectorXd _h; + MatrixXr _C; + MatrixXr _D; + VectorXr _h; }; struct IterativeLQR::CostEntityBase { + typedef std::shared_ptr Ptr; // parameters @@ -160,20 +167,20 @@ struct IterativeLQR::CostEntityBase virtual VecConstRef r() const { return _r; } - virtual double evaluate(VecConstRef x, + virtual Real evaluate(VecConstRef x, VecConstRef u, int k) = 0; virtual void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) = 0; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) = 0; virtual std::string getName() = 0; - virtual double getCostEvaluated() const { return _cost_eval; } + virtual Real getCostEvaluated() const { return _cost_eval; } virtual ~CostEntityBase() = default; @@ -181,8 +188,8 @@ struct IterativeLQR::CostEntityBase protected: - Eigen::VectorXd _q, _r; - double _cost_eval; + VectorXr _q, _r; + Real _cost_eval; }; struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase @@ -193,16 +200,16 @@ struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase VecConstRef xlb, VecConstRef xub, VecConstRef ulb, VecConstRef uub); - void setRho(double rho); + void setRho(Real rho); - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -217,11 +224,11 @@ struct IterativeLQR::BoundAuglagCostEntity : CostEntityBase VecConstRef _xlb, _xub; VecConstRef _ulb, _uub; - Eigen::VectorXd _x_violation; - Eigen::VectorXd _u_violation; + VectorXr _x_violation; + VectorXr _u_violation; - Eigen::VectorXd _xlam, _ulam; - double _rho; + VectorXr _xlam, _ulam; + Real _rho; const int _N; }; @@ -231,8 +238,6 @@ struct IterativeLQR::IntermediateCostEntity : CostEntityBase { typedef std::shared_ptr Ptr; - - // set cost void setCost(casadi::Function l, casadi::Function dl, @@ -241,14 +246,14 @@ struct IterativeLQR::IntermediateCostEntity : CostEntityBase VecConstRef q() const override; VecConstRef r() const override; - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -274,14 +279,14 @@ struct IterativeLQR::IntermediateResidualEntity : CostEntityBase void setResidual(casadi::Function res, casadi::Function dres); - double evaluate(VecConstRef x, VecConstRef u, int k) override; + Real evaluate(VecConstRef x, VecConstRef u, int k) override; void quadratize(VecConstRef x, VecConstRef u, int k, - Eigen::MatrixXd& Q, - Eigen::MatrixXd& R, - Eigen::MatrixXd& P) override; + MatrixXr& Q, + MatrixXr& R, + MatrixXr& P) override; std::string getName(); @@ -302,17 +307,17 @@ struct IterativeLQR::IntermediateCost { /* Quadratized cost */ - const Eigen::MatrixXd& Q() const; + const MatrixXr& Q() const; VecConstRef q() const; - const Eigen::MatrixXd& R() const; + const MatrixXr& R() const; VecConstRef r() const; - const Eigen::MatrixXd& P() const; + const MatrixXr& P() const; IntermediateCost(int nx, int nu); void addCost(CostEntityBase::Ptr cost); - double evaluate(VecConstRef x, VecConstRef u, int k); + Real evaluate(VecConstRef x, VecConstRef u, int k); void quadratize(VecConstRef x, VecConstRef u, int k); void clear(); @@ -321,8 +326,8 @@ struct IterativeLQR::IntermediateCost private: - Eigen::MatrixXd _Q, _R, _P; - Eigen::VectorXd _q, _r; + MatrixXr _Q, _R, _P; + VectorXr _q, _r; }; struct IterativeLQR::Temporaries @@ -330,59 +335,59 @@ struct IterativeLQR::Temporaries /* Backward pass */ // temporary for s + S*d - Eigen::MatrixXd s_plus_S_d; + MatrixXr s_plus_S_d; // temporary for S*A - Eigen::MatrixXd S_A; + MatrixXr S_A; // feasible constraint - Eigen::MatrixXd Cf, Df; - Eigen::VectorXd hf; + MatrixXr Cf, Df; + VectorXr hf; // cod of constraint - Eigen::CompleteOrthogonalDecomposition ccod; - Eigen::ColPivHouseholderQR cqr; - Eigen::BDCSVD csvd; - Eigen::MatrixXd codQ; + Eigen::CompleteOrthogonalDecomposition ccod; + Eigen::ColPivHouseholderQR cqr; + Eigen::BDCSVD csvd; + MatrixXr codQ; Eigen::PermutationMatrix codP; // quadratized value function - Eigen::MatrixXd Huu; - Eigen::MatrixXd Hux; - Eigen::MatrixXd Hxx; - Eigen::VectorXd hx; - Eigen::VectorXd hu; + MatrixXr Huu; + MatrixXr Hux; + MatrixXr Hxx; + VectorXr hx; + VectorXr hu; // temporary for kkt rhs - Eigen::MatrixXd kkt; - Eigen::MatrixXd kx0; + MatrixXr kkt; + MatrixXr kx0; // lu for kkt matrix - Eigen::PartialPivLU lu; - Eigen::ColPivHouseholderQR qr; - Eigen::LDLT ldlt; + Eigen::PartialPivLU lu; + Eigen::ColPivHouseholderQR qr; + Eigen::LDLT ldlt; // kkt solution - Eigen::MatrixXd u_lam; + MatrixXr u_lam; // infeasible component of constraint - Eigen::MatrixXd Cinf; - Eigen::MatrixXd Dinf; - Eigen::VectorXd hinf; + MatrixXr Cinf; + MatrixXr Dinf; + VectorXr hinf; // optimal state computation // (note: only for initial state x[0]) - Eigen::FullPivLU x_lu; - Eigen::ColPivHouseholderQR x_qr; - Eigen::LDLT x_ldlt; - Eigen::MatrixXd x_kkt; - Eigen::VectorXd x_k0; - Eigen::VectorXd dx_lam; + Eigen::FullPivLU x_lu; + Eigen::ColPivHouseholderQR x_qr; + Eigen::LDLT x_ldlt; + MatrixXr x_kkt; + VectorXr x_k0; + VectorXr dx_lam; /* Forward pass */ - Eigen::VectorXd dx; - Eigen::VectorXd du; - Eigen::VectorXd defect; + VectorXr dx; + VectorXr du; + VectorXr defect; }; @@ -415,16 +420,16 @@ struct IterativeLQR::ConstraintToGo private: - Eigen::Matrix _C; - Eigen::Matrix _D; - Eigen::VectorXd _h; + Eigen::Matrix _C; + Eigen::Matrix _D; + VectorXr _h; int _dim; }; struct IterativeLQR::ValueFunction { - Eigen::MatrixXd S; - Eigen::VectorXd s; + MatrixXr S; + VectorXr s; ValueFunction(int nx); }; @@ -433,26 +438,26 @@ struct IterativeLQR::BackwardPassResult { // real input as function of state // (u = Lu*x + lu) - Eigen::MatrixXd Lu; - Eigen::VectorXd lu; + MatrixXr Lu; + VectorXr lu; // auxiliary input as function of state // (z = Lz*x + lz, where u = lc + Lc*x + Bz*z) - Eigen::MatrixXd Lz; - Eigen::VectorXd lz; + MatrixXr Lz; + VectorXr lz; // constraint-to-go size int nc; // lagrange multipliers - Eigen::MatrixXd Gu; - Eigen::MatrixXd Gx; - Eigen::VectorXd glam; + MatrixXr Gu; + MatrixXr Gx; + VectorXr glam; // optimal state // (this is only filled at i = 0) - Eigen::VectorXd dx; - Eigen::VectorXd dx_lam; + VectorXr dx; + VectorXr dx_lam; BackwardPassResult(int nx, int nu); }; @@ -464,7 +469,7 @@ struct IterativeLQR::FeasibleConstraint VecConstRef h; }; -static void set_param_inputs(std::shared_ptr> params, int k, +static void set_param_inputs(std::shared_ptr> params, int k, casadi_utils::WrappedFunction& f); #define THROW_NAN(mat) \ diff --git a/horizon/cpp/src/ilqr_test.cpp b/horizon/cpp/src/ilqr_test.cpp index b0161e4a..533529df 100644 --- a/horizon/cpp/src/ilqr_test.cpp +++ b/horizon/cpp/src/ilqr_test.cpp @@ -1,9 +1,14 @@ #include "ilqr.h" #include #include "wrapped_function.h" +#include "typedefs.h" int main() { + using Real=horizon::Real; + using MatrixXr=horizon::MatrixXr; + using VectorXr=horizon::VectorXr; + bool stop = false; auto f = casadi::external("zero_velocity_l_foot_l_sole_vel_task_jac", @@ -18,9 +23,9 @@ int main() while(!stop) { - auto x = Eigen::VectorXd::Random(f.size1_in(0)).eval(); - auto u = Eigen::VectorXd::Random(f.size1_in(1), 1).eval(); - auto tgt = Eigen::VectorXd::Random(f.size1_in(2), 1).eval(); + auto x = VectorXr::Random(f.size1_in(0)).eval(); + auto u = VectorXr::Random(f.size1_in(1), 1).eval(); + auto tgt = VectorXr::Random(f.size1_in(2), 1).eval(); fw.setInput(0, x); fw.setInput(1, u); @@ -61,6 +66,10 @@ int main() int not_a_main() { + using Real=horizon::Real; + using MatrixXr=horizon::MatrixXr; + using VectorXr=horizon::VectorXr; + auto x = casadi::SX::sym("x", 1); auto u = casadi::SX::sym("u", 1); auto p = casadi::SX::sym("p", 1); @@ -73,21 +82,21 @@ int not_a_main() int N = 3; horizon::IterativeLQR ilqr(f, N); - Eigen::MatrixXd xlb, xub; + MatrixXr xlb, xub; xlb.setConstant(1, N+1, -INFINITY); xub.setConstant(1, N+1, INFINITY); xlb(N) = 1.0; xub(N) = 1.0; ilqr.setStateBounds(xlb, xub); - Eigen::MatrixXd ulb, uub; + MatrixXr ulb, uub; ulb.setConstant(1, N, -INFINITY); uub.setConstant(1, N, INFINITY); ulb(0) = 11.0; uub(0) = 11.0; ilqr.setInputBounds(ulb, uub); - Eigen::VectorXd x0(1); + VectorXr x0(1); x0 << 0.0; ilqr.setInitialState(x0); xlb(0) = x0(0); @@ -96,11 +105,11 @@ int not_a_main() ilqr.setCost({0, 1}, l); ilqr.setConstraint({2}, cf); - Eigen::MatrixXd myparam_values; + MatrixXr myparam_values; myparam_values.setConstant(1, N+1, -1.0); ilqr.setParameterValue("myparam", myparam_values); - Eigen::MatrixXd dt_values; + MatrixXr dt_values; dt_values.setConstant(1, N+1, 0.1); ilqr.setParameterValue("dt", dt_values); diff --git a/horizon/cpp/src/iterate_filter.cpp b/horizon/cpp/src/iterate_filter.cpp index 499e7c04..fbb2dc8d 100644 --- a/horizon/cpp/src/iterate_filter.cpp +++ b/horizon/cpp/src/iterate_filter.cpp @@ -2,11 +2,11 @@ IterateFilter::Pair::Pair(): - f(std::numeric_limits::max()), - h(std::numeric_limits::max()) + f(std::numeric_limits::max()), + h(std::numeric_limits::max()) {} -bool IterateFilter::Pair::dominates(const IterateFilter::Pair &other, double beta, double gamma) const +bool IterateFilter::Pair::dominates(const IterateFilter::Pair &other, Real beta, Real gamma) const { return f < other.f + gamma*other.h && beta*h < other.h; } diff --git a/horizon/cpp/src/iterate_filter.h b/horizon/cpp/src/iterate_filter.h index 09f94b3b..8f864183 100644 --- a/horizon/cpp/src/iterate_filter.h +++ b/horizon/cpp/src/iterate_filter.h @@ -4,22 +4,26 @@ #include #include #include +#include +#include "typedefs.h" class IterateFilter { +using Real=horizon::Real; + public: struct Pair { - double f; - double h; + Real f; + Real h; Pair(); bool dominates(const Pair& other, - double beta = 1.0, - double gamma = 0.0) const; + Real beta = 1.0, + Real gamma = 0.0) const; }; IterateFilter() = default; @@ -32,9 +36,9 @@ class IterateFilter void print(); - double beta = 1.0; - double gamma = 0.0; - double constr_tol = 1e-6; + Real beta = 1.0; + Real gamma = 0.0; + Real constr_tol = 1e-6; private: diff --git a/horizon/cpp/src/profiling.cpp b/horizon/cpp/src/profiling.cpp index d7b68507..7f34a5db 100644 --- a/horizon/cpp/src/profiling.cpp +++ b/horizon/cpp/src/profiling.cpp @@ -23,7 +23,7 @@ void Timer::toc() { if(_done) return; - double usec = (hrc::now() - _t0).count() * 1e-3; + Real usec = (hrc::now() - _t0).count() * 1e-3; _on_toc(_name, usec); _done = true; } diff --git a/horizon/cpp/src/profiling.h b/horizon/cpp/src/profiling.h index eab38435..b8ea5076 100644 --- a/horizon/cpp/src/profiling.h +++ b/horizon/cpp/src/profiling.h @@ -5,15 +5,20 @@ #include #include #include +#include + +#include "typedefs.h" namespace horizon { namespace utils { struct Timer { + using Real=horizon::Real; + typedef std::chrono::high_resolution_clock hrc; - typedef std::function TocCallback; + typedef std::function TocCallback; Timer(const char* name, TocCallback& cb); @@ -35,7 +40,7 @@ struct Timer struct ProfilingInfo { - std::map> timings; + std::map> timings; }; diff --git a/horizon/cpp/src/sqp.cpp b/horizon/cpp/src/sqp.cpp index 992062d7..feb72c26 100644 --- a/horizon/cpp/src/sqp.cpp +++ b/horizon/cpp/src/sqp.cpp @@ -2,7 +2,6 @@ using namespace horizon; - template const casadi::DMDict& SQPGaussNewton::solve( const casadi::DM& initial_guess_x, @@ -24,8 +23,8 @@ const casadi::DMDict& SQPGaussNewton::solve( _iteration_to_solve = 0; // set parameters as second input of f and df - _f.setInput(1, Eigen::VectorXd::Map(p->data(), p.size1())); - _df.setInput(1, Eigen::VectorXd::Map(p->data(), p.size1())); + _f.setInput(1, VectorXr::Map(p->data(), p.size1())); + _df.setInput(1, VectorXr::Map(p->data(), p.size1())); // do the same on g and A (i.e., dg) _g_dict.input[_g.name_in(1)] = p; @@ -66,7 +65,7 @@ const casadi::DMDict& SQPGaussNewton::solve( casadi_utils::toCasadiMatrix(_grad, grad_); if(!H_.is_init()) - H_ = casadi_utils::WrappedSparseMatrix(_H); + H_ = casadi_utils::WrappedSparseMatrix(_H); else H_.update_values(_H); @@ -147,7 +146,7 @@ const casadi::DMDict& SQPGaussNewton::solve( _solution["x"] = x0_; - double norm_f = _f.getOutput(0).norm(); + Real norm_f = _f.getOutput(0).norm(); _solution["f"] = 0.5*norm_f*norm_f; _solution["g"] = casadi::norm_2(_g_dict.output[_g.name_out(0)].get_elements()); @@ -156,10 +155,10 @@ const casadi::DMDict& SQPGaussNewton::solve( template bool SQPGaussNewton::lineSearch( - Eigen::VectorXd &x, - const Eigen::VectorXd &dx, - const Eigen::VectorXd &lam_x, - const Eigen::VectorXd &lam_a, + VectorXr &x, + const VectorXr &dx, + const VectorXr &lam_x, + const VectorXr &lam_a, const casadi::DM &lbg, const casadi::DM &ubg, const casadi::DM &lbx, @@ -173,20 +172,20 @@ bool SQPGaussNewton::lineSearch( _x0_ = x; - const double merit_safety_factor = 2.0; - double norminf_lam_x = lam_x.lpNorm(); - double norminf_lam_a = lam_a.lpNorm(); - double norminf_lam = merit_safety_factor*std::max(norminf_lam_x, norminf_lam_a); + const Real merit_safety_factor = 2.0; + Real norminf_lam_x = lam_x.lpNorm(); + Real norminf_lam_a = lam_a.lpNorm(); + Real norminf_lam = merit_safety_factor*std::max(norminf_lam_x, norminf_lam_a); - double initial_cost = computeCost(_f); + Real initial_cost = computeCost(_f); - double cost_derr = computeCostDerivative(dx, _grad); + Real cost_derr = computeCostDerivative(dx, _grad); casadi_utils::toEigen(_g_dict.output[_g.name_out(0)], _g_); - double constraint_violation = computeConstraintViolation(_g_, _x0_, _lbg_, _ubg_, _lbx_, _ubx_); + Real constraint_violation = computeConstraintViolation(_g_, _x0_, _lbg_, _ubg_, _lbx_, _ubx_); - double merit_der = cost_derr - norminf_lam * constraint_violation; - double initial_merit = initial_cost + norminf_lam*constraint_violation; + Real merit_der = cost_derr - norminf_lam * constraint_violation; + Real initial_merit = initial_cost + norminf_lam*constraint_violation; // report initial value (only if iter == 0) if(iter == 0) @@ -215,15 +214,15 @@ bool SQPGaussNewton::lineSearch( { x = _x0_ + _alpha*dx; eval(_f, 0, x, false); - double candidate_cost = computeCost(_f); + Real candidate_cost = computeCost(_f); casadi_utils::toCasadiMatrix(x, _x_); _g_dict.input[_g.name_in(0)] = _x_; eval(_g, _g_dict); casadi_utils::toEigen(_g_dict.output[_g.name_out(0)], _g_); - double candidate_constraint_violation = computeConstraintViolation(_g_, x, _lbg_, _ubg_, _lbx_, _ubx_); + Real candidate_constraint_violation = computeConstraintViolation(_g_, x, _lbg_, _ubg_, _lbx_, _ubx_); - double candidate_merit = candidate_cost + norminf_lam*candidate_constraint_violation; + Real candidate_merit = candidate_cost + norminf_lam*candidate_constraint_violation; // evaluate Armijo's condition accepted = candidate_merit < (initial_merit + _beta*_alpha*merit_der); diff --git a/horizon/cpp/src/sqp.h b/horizon/cpp/src/sqp.h index 162c5292..9bd0ece5 100644 --- a/horizon/cpp/src/sqp.h +++ b/horizon/cpp/src/sqp.h @@ -11,18 +11,23 @@ #include "profiling.h" #include "ilqr.h" +#include "typedefs.h" #define GR 1.61803398875 namespace horizon{ -typedef Eigen::Ref VecConstRef; -typedef Eigen::Ref MatConstRef; +typedef Eigen::Ref VecConstRef; +typedef Eigen::Ref MatConstRef; template ///casadi::SX or casadi::MX class SQPGaussNewton { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + public: static void setQPOasesOptionsMPC(casadi::Dict& opts) @@ -31,13 +36,13 @@ class SQPGaussNewton opts["initialStatusBounds"] = "inactive"; opts["numRefinementSteps"] = 0; opts["enableDriftCorrection"] = 0; - opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); + opts["terminationTolerance"] = 10e9 * std::numeric_limits::epsilon(); opts["enableFlippingBounds"] = false; opts["enableNZCTests"] = false; opts["enableRamping"] = false; opts["enableRegularisation"] = true; opts["numRegularisationSteps"] = 2; - opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); + opts["epsRegularisation"] = 5. * 10e3 * std::numeric_limits::epsilon(); } static void setQPOasesOptionsReliable(casadi::Dict& opts) @@ -45,7 +50,7 @@ class SQPGaussNewton opts["enableEqualities"] = false; opts["numRefinementSteps"] = 2; opts["enableFullLITest"] = true; - opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); + opts["epsLITests"] = 10e5 * std::numeric_limits::epsilon(); opts["maxDualJump"] = 10e8; opts["enableCholeskyRefactorisation"] = 1; } @@ -223,12 +228,12 @@ class SQPGaussNewton * @brief setAlphaMin set the minumi allowed alpha during linesearch * @param alpha min in Newton's method step */ - void setAlphaMin(const double alpha_min) + void setAlphaMin(const Real alpha_min) { _alpha_min = alpha_min; } - const double& getAlpha() const + const Real& getAlpha() const { return _alpha; } @@ -248,26 +253,26 @@ class SQPGaussNewton const casadi::DM& lbx, const casadi::DM& ubx, const casadi::DM& lbg, const casadi::DM& ubg); - double computeCost(const casadi_utils::WrappedFunction& f) + Real computeCost(const casadi_utils::WrappedFunction& f) { return f.getOutput(0).squaredNorm(); } - double computeCostDerivative(const Eigen::VectorXd& dx, const Eigen::VectorXd& grad) + Real computeCostDerivative(const VectorXr& dx, const VectorXr& grad) { return dx.dot(grad); } - double computeConstraintViolation(const Eigen::VectorXd& g, const Eigen::VectorXd& x, - const Eigen::VectorXd& lbg, const Eigen::VectorXd& ubg, - const Eigen::VectorXd& lbx, const Eigen::VectorXd& ubx) + Real computeConstraintViolation(const VectorXr& g, const VectorXr& x, + const VectorXr& lbg, const VectorXr& ubg, + const VectorXr& lbx, const VectorXr& ubx) { return (lbg-g).cwiseMax(0.).lpNorm<1>() + (ubg-g).cwiseMin(0.).lpNorm<1>() + (lbx-x).cwiseMax(0.).lpNorm<1>() + (ubx-x).cwiseMin(0.).lpNorm<1>(); } - bool lineSearch(Eigen::VectorXd& x, const Eigen::VectorXd& dx, const Eigen::VectorXd& lam_x, const Eigen::VectorXd& lam_a, + bool lineSearch(VectorXr& x, const VectorXr& dx, const VectorXr& lam_x, const VectorXr& lam_a, const casadi::DM& lbg, const casadi::DM& ubg, const casadi::DM& lbx, const casadi::DM& ubx, int iter); @@ -339,9 +344,9 @@ class SQPGaussNewton * @brief getObjectiveIterations * @return 0.5*norm2 of objective (one per iteration) */ - const std::vector& getObjectiveIterations() + const std::vector& getObjectiveIterations() { - Eigen::VectorXd tmp; + VectorXr tmp; _objective.clear(); _objective.reserve(_iteration_to_solve); for(unsigned int k = 0; k < _iteration_to_solve; ++k) @@ -349,7 +354,7 @@ class SQPGaussNewton casadi_utils::toEigen(_variable_trj[k], tmp); _f.setInput(0, tmp); // cost function _f.call(); - double norm = _f.getOutput(0).norm(); + Real norm = _f.getOutput(0).norm(); _objective.push_back(0.5*norm*norm); } return _objective; @@ -359,7 +364,7 @@ class SQPGaussNewton * @brief getConstraintNormIterations * @return norm2 of the constraint vector (one per iteration) */ - const std::vector& getConstraintNormIterations() + const std::vector& getConstraintNormIterations() { _constraints_norm.clear(); _constraints_norm.reserve(_iteration_to_solve); @@ -376,7 +381,7 @@ class SQPGaussNewton * @brief getHessianComputationTime * @return vector of times needed to compute hessian (one value per iteration) */ - const std::vector& getHessianComputationTime() const + const std::vector& getHessianComputationTime() const { return _hessian_computation_time; } @@ -385,22 +390,22 @@ class SQPGaussNewton * @brief getQPComputationTime * @return vector of times needed to solve qp (one value per iteration) */ - const std::vector& getQPComputationTime() const + const std::vector& getQPComputationTime() const { return _qp_computation_time; } - const std::vector& getLineSearchComputationTime() const + const std::vector& getLineSearchComputationTime() const { return _line_search_time; } - void setBeta(const double beta) + void setBeta(const Real beta) { _beta = beta; } - double getBeta() + Real getBeta() { return _beta; } @@ -422,7 +427,7 @@ class SQPGaussNewton * @param x point * @param sparse if result will be sparse */ - void eval(casadi_utils::WrappedFunction& wf, const int i, const Eigen::VectorXd& x, const bool sparse) + void eval(casadi_utils::WrappedFunction& wf, const int i, const VectorXr& x, const bool sparse) { wf.setInput(i, x); // cost function wf.call(sparse); @@ -440,7 +445,7 @@ class SQPGaussNewton } - bool checkIsStationary(const Eigen::VectorXd& grad, const double tol) + bool checkIsStationary(const VectorXr& grad, const Real tol) { for(unsigned int i = 0; i < grad.size(); ++i) { @@ -478,27 +483,27 @@ class SQPGaussNewton casadi::Dict _qp_opts; casadi::DMVector _variable_trj; - std::vector _objective, _constraints_norm; + std::vector _objective, _constraints_norm; - Eigen::SparseMatrix _J; - Eigen::SparseMatrix _H; - Eigen::SparseMatrix _I; - Eigen::VectorXd _grad; + Eigen::SparseMatrix _J; + Eigen::SparseMatrix _H; + Eigen::SparseMatrix _I; + VectorXr _grad; casadi::DM grad_; casadi::DM g_; casadi::DM A_; - casadi_utils::WrappedSparseMatrix H_; + casadi_utils::WrappedSparseMatrix H_; casadi::DM x0_; - Eigen::VectorXd _sol, _dx, _lam_a, _lam_x; + VectorXr _sol, _dx, _lam_a, _lam_x; IODMDict _g_dict; IODMDict _A_dict; - double _alpha, _alpha_min; + Real _alpha, _alpha_min; - std::vector _hessian_computation_time; - std::vector _qp_computation_time; - std::vector _line_search_time; + std::vector _hessian_computation_time; + std::vector _qp_computation_time; + std::vector _line_search_time; unsigned int _iteration_to_solve; @@ -506,23 +511,23 @@ class SQPGaussNewton IterativeLQR::ForwardPassResult _fpr; CallbackType _iter_cb; - double _beta; + Real _beta; - double _solution_convergence; - double _constraint_violation_tolerance; - double _merit_derivative_tolerance; + Real _solution_convergence; + Real _constraint_violation_tolerance; + Real _merit_derivative_tolerance; - double _eps_regularization = 0.0; - double _eps_regularization_base = 0.0; + Real _eps_regularization = 0.0; + Real _eps_regularization_base = 0.0; bool _use_gr; //line search - Eigen::VectorXd _lbg_, _ubg_, _lbx_, _ubx_; - Eigen::VectorXd _x0_; + VectorXr _lbg_, _ubg_, _lbx_, _ubx_; + VectorXr _x0_; casadi::DM _x_; - Eigen::VectorXd _g_; - double _merit_eps; + VectorXr _g_; + Real _merit_eps; }; diff --git a/horizon/cpp/src/sqp_test.cpp b/horizon/cpp/src/sqp_test.cpp index 021d0f02..5d8449d9 100644 --- a/horizon/cpp/src/sqp_test.cpp +++ b/horizon/cpp/src/sqp_test.cpp @@ -1,5 +1,6 @@ #include "sqp.h" #include +#include "typedefs.h" int main() { @@ -38,8 +39,8 @@ int main() // std::cout<<"solution: "< objs = sqp.getObjectiveIterations(); -// std::vector cons = sqp.getConstraintNormIterations(); +// std::vector objs = sqp.getObjectiveIterations(); +// std::vector cons = sqp.getConstraintNormIterations(); // for(unsigned int i = 0; i < sqp.getNumberOfIterations(); ++i) // std::cout<<"iter "< sol: "< objs2 = sqp2.getObjectiveIterations(); -// std::vector cons2 = sqp2.getConstraintNormIterations(); +// std::vector objs2 = sqp2.getObjectiveIterations(); +// std::vector cons2 = sqp2.getConstraintNormIterations(); // for(unsigned int i = 0; i < sqp2.getNumberOfIterations(); ++i) // std::cout<<"iter "< sol2: "< + +namespace horizon +{ + +#ifdef ILQR_FLOAT +typedef float Real; +#define casadi_real float +#else +typedef double Real; +#define casadi_real double +#endif + +typedef Eigen::Matrix VectorXr; +typedef Eigen::Matrix MatrixXr; + +typedef Eigen::Matrix Matrix2r; +typedef Eigen::Matrix Vector2r; + +typedef Eigen::Matrix RowVectorXr; + +const Real RInfinity = std::numeric_limits::infinity(); + +#ifdef ILQR_FLOAT +#define EXPECT_Real_EQ EXPECT_FLOAT_EQ +#else +#define EXPECT_Real_EQ EXPECT_DOUBLE_EQ +#endif + +} + +#endif // TYPDEFS_H \ No newline at end of file diff --git a/horizon/cpp/src/wrapped_function.cpp b/horizon/cpp/src/wrapped_function.cpp index e5c10040..934fc6bc 100644 --- a/horizon/cpp/src/wrapped_function.cpp +++ b/horizon/cpp/src/wrapped_function.cpp @@ -5,7 +5,6 @@ using namespace casadi_utils; extern horizon::utils::Timer::TocCallback on_timer_toc; - WrappedFunction::WrappedFunction(casadi::Function f) { *this = f; @@ -19,6 +18,9 @@ WrappedFunction &WrappedFunction::operator=(casadi::Function f) } _f = f; + // const std::string dtype =""; + // const GenericType = ; + // _f.change_option(); // resize work vectors _iw.assign(_f.sz_iw(), 0); @@ -39,10 +41,10 @@ WrappedFunction &WrappedFunction::operator=(casadi::Function f) _out_buf.push_back(_out_data.back().data()); // allocate a zero dense matrix to store the output - _out_matrix.emplace_back(Eigen::MatrixXd::Zero(sp.size1(), sp.size2())); + _out_matrix.emplace_back(MatrixXr::Zero(sp.size1(), sp.size2())); //allocate a zero sparse matrix to store the output - _out_matrix_sparse.emplace_back(Eigen::SparseMatrix(sp.size1(), sp.size2())); + _out_matrix_sparse.emplace_back(Eigen::SparseMatrix(sp.size1(), sp.size2())); // save sparsity pattern for i-th output std::vector rows, cols; @@ -64,7 +66,7 @@ WrappedFunction::WrappedFunction(const WrappedFunction & other) *this = other._f; } -void WrappedFunction::setInput(int i, Eigen::Ref xi) +void WrappedFunction::setInput(int i, Eigen::Ref xi) { if(xi.size() != _f.size1_in(i)) { @@ -127,7 +129,7 @@ void WrappedFunction::call(bool sparse) oss << _out_matrix[i].format(3) << "\n"; for(int j = 0; j < _f.n_in(); j++) { - auto u = Eigen::VectorXd::Map(_in_buf[j], + auto u = VectorXr::Map(_in_buf[j], _f.size1_in(j)); oss << _f.name() << " input " << j << " = " << u.transpose().format(3) << "\n"; @@ -142,7 +144,7 @@ void WrappedFunction::call(bool sparse) _f.release(mem); } -void WrappedFunction::call_accumulate(std::vector> &out) +void WrappedFunction::call_accumulate(std::vector> &out) { // call function (allocation-free) casadi_int mem = _f.checkout(); @@ -175,7 +177,7 @@ void WrappedFunction::call_accumulate(std::vector> & _f.release(mem); } -const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const +const MatrixXr& WrappedFunction::getOutput(int i) const { if(_out_matrix[i].hasNaN() || !_out_matrix[i].allFinite()) { @@ -183,7 +185,7 @@ const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const std::cout << "output #" << i << ": \n" << _out_matrix[i].format(3) << "\n"; for(int j = 0; j < _f.n_in(); j++) { - auto in = Eigen::VectorXd::Map(_in_buf[j], _f.size1_in(j)); + auto in = VectorXr::Map(_in_buf[j], _f.size1_in(j)); std::cout << "input #" << j << ": \n" << in.transpose().format(3) << "\n"; } } @@ -191,12 +193,12 @@ const Eigen::MatrixXd& WrappedFunction::getOutput(int i) const return _out_matrix[i]; } -const Eigen::SparseMatrix& WrappedFunction::getSparseOutput(int i) const +const Eigen::SparseMatrix& WrappedFunction::getSparseOutput(int i) const { return _out_matrix_sparse[i]; } -Eigen::MatrixXd& WrappedFunction::out(int i) +MatrixXr& WrappedFunction::out(int i) { return _out_matrix[i]; } @@ -219,13 +221,13 @@ bool WrappedFunction::is_valid() const void WrappedFunction::csc_to_sparse_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::SparseMatrix& matrix) + const std::vector& data, + Eigen::SparseMatrix& matrix) { - std::vector> triplet_list; + std::vector> triplet_list; triplet_list.reserve(data.size()); for(unsigned int i = 0; i < data.size(); ++i) - triplet_list.push_back(Eigen::Triplet(sp_rows[i], sp_cols[i], data[i])); + triplet_list.push_back(Eigen::Triplet(sp_rows[i], sp_cols[i], data[i])); matrix.setFromTriplets(triplet_list.begin(), triplet_list.end()); } @@ -233,14 +235,14 @@ void WrappedFunction::csc_to_sparse_matrix(const casadi::Sparsity& sp, void WrappedFunction::csc_to_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::MatrixXd& matrix) + const std::vector& data, + MatrixXr& matrix) { // if dense output, do copy assignment which should be // faster if(sp.is_dense()) { - matrix = Eigen::MatrixXd::Map(data.data(), + matrix = MatrixXr::Map(data.data(), matrix.rows(), matrix.cols()); @@ -262,14 +264,14 @@ void WrappedFunction::csc_to_matrix(const casadi::Sparsity& sp, void WrappedFunction::csc_to_matrix_accu(const casadi::Sparsity &sp, const std::vector &sp_rows, const std::vector &sp_cols, - const std::vector &data, - Eigen::Ref matrix) + const std::vector &data, + Eigen::Ref matrix) { // if dense output, do copy assignment which should be // faster if(sp.is_dense()) { - matrix += Eigen::MatrixXd::Map(data.data(), + matrix += MatrixXr::Map(data.data(), matrix.rows(), matrix.cols()); diff --git a/horizon/cpp/src/wrapped_function.h b/horizon/cpp/src/wrapped_function.h index 071ebc7e..ca7b2519 100644 --- a/horizon/cpp/src/wrapped_function.h +++ b/horizon/cpp/src/wrapped_function.h @@ -5,9 +5,15 @@ #include #include +#include "typedefs.h" + namespace casadi_utils { +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + template void toCasadiMatrix(const Eigen::Matrix& E, casadi::Matrix& C) { @@ -35,7 +41,7 @@ class WrappedSparseMatrix to_triplets(E, _r, _c); _s = casadi::Sparsity::triplet(E.rows(), E.cols(), _r, _c); _values = casadi::DM::zeros(E.nonZeros()); - std::memcpy(_values.ptr(), E.valuePtr(), sizeof(double)*E.nonZeros()); + std::memcpy(_values.ptr(), E.valuePtr(), sizeof(Real)*E.nonZeros()); _C = casadi::Matrix(_s, _values); } @@ -106,14 +112,14 @@ class WrappedFunction WrappedFunction(const WrappedFunction&); WrappedFunction& operator=(const WrappedFunction&); - void setInput(int i, Eigen::Ref xi); + void setInput(int i, Eigen::Ref xi); void call(bool sparse = false); - void call_accumulate(std::vector>& out); - const Eigen::MatrixXd& getOutput(int i) const; - const Eigen::SparseMatrix& getSparseOutput(int i) const; + void call_accumulate(std::vector>& out); + const MatrixXr& getOutput(int i) const; + const Eigen::SparseMatrix& getSparseOutput(int i) const; casadi::Function& functionRef(); const casadi::Function& function() const; - Eigen::MatrixXd& out(int i); + MatrixXr& out(int i); bool is_valid() const; @@ -122,28 +128,28 @@ class WrappedFunction void csc_to_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::MatrixXd& matrix); + const std::vector& data, + MatrixXr& matrix); void csc_to_matrix_accu(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::Ref matrix); + const std::vector& data, + Eigen::Ref matrix); void csc_to_sparse_matrix(const casadi::Sparsity& sp, const std::vector& sp_rows, const std::vector& sp_cols, - const std::vector& data, - Eigen::SparseMatrix& matrix); - - std::vector _in_buf; - std::vector> _out_data; - std::vector _out_matrix; - std::vector > _out_matrix_sparse; - std::vector _out_buf; + const std::vector& data, + Eigen::SparseMatrix& matrix); + + std::vector _in_buf; + std::vector> _out_data; + std::vector _out_matrix; + std::vector > _out_matrix_sparse; + std::vector _out_buf; std::vector _iw; - std::vector _dw; + std::vector _dw; std::vector> _rows; std::vector> _cols; diff --git a/horizon/cpp/tests/CMakeLists.txt b/horizon/cpp/tests/CMakeLists.txt index b4ba9b30..c5cd28e2 100644 --- a/horizon/cpp/tests/CMakeLists.txt +++ b/horizon/cpp/tests/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.11) +cmake_minimum_required(VERSION 3.10) include(ExternalProject) set(PROJECTNAME tests) diff --git a/horizon/cpp/tests/testCasadiUtils.cpp b/horizon/cpp/tests/testCasadiUtils.cpp index cf2d0403..fc696d65 100644 --- a/horizon/cpp/tests/testCasadiUtils.cpp +++ b/horizon/cpp/tests/testCasadiUtils.cpp @@ -1,8 +1,13 @@ #include #include "../src/wrapped_function.h" +#include "../src/typedefs.h" namespace{ +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; + class testCasadiUtils: public ::testing::Test { protected: @@ -27,14 +32,14 @@ class testCasadiUtils: public ::testing::Test TEST_F(testCasadiUtils, testSparseHessian) { // // This compile - Eigen::MatrixXd A, B; + MatrixXr A, B; A.resize(8,8); A.setRandom(8,8); B.resize(8, 8); B.triangularView() = A.transpose()*A; //This does not compile - Eigen::SparseMatrix J, H; + Eigen::SparseMatrix J, H; J.resize(8, 8); J.setIdentity(); H.resize(8, 8); @@ -46,19 +51,19 @@ TEST_F(testCasadiUtils, testSparseHessian) -void EXPECT_EQUAL(const Eigen::SparseMatrix& E, const casadi::DM& C) +void EXPECT_EQUAL(const Eigen::SparseMatrix& E, const casadi::DM& C) { EXPECT_EQ(E.rows(), C.rows()); EXPECT_EQ(E.cols(), C.columns()); EXPECT_EQ(E.nonZeros(), C.nnz()); - std::vector e; + std::vector e; e.assign(E.valuePtr(), E.valuePtr() + E.nonZeros()); - std::vector c; + std::vector c; c.assign(C->data(), C->data() + C.nnz()); for(unsigned int i = 0; i < e.size(); ++i) - EXPECT_DOUBLE_EQ(e[i], c[i]); + EXPECT_EQ(e[i], c[i]); } @@ -66,28 +71,28 @@ TEST_F(testCasadiUtils, toCasadiSparse) { std::default_random_engine gen; - std::uniform_real_distribution dist(0.0,1.0); + std::uniform_real_distribution dist(0.0,1.0); int rows=100; int cols=100; - std::vector > tripletList; + std::vector > tripletList; for(int i=0;i(i,j,v_ij)); //if larger than treshold, insert it + tripletList.push_back(Eigen::Triplet(i,j,v_ij)); //if larger than treshold, insert it } } - Eigen::SparseMatrix E(rows,cols); + Eigen::SparseMatrix E(rows,cols); E.setFromTriplets(tripletList.begin(), tripletList.end()); //std::cout<<"E: "< C(E); + casadi_utils::WrappedSparseMatrix C(E); auto toc = std::chrono::high_resolution_clock::now(); std::cout<<"Constructor: "<<(toc-tic).count()*1E-9<<" [s]"< #include "../src/ilqr.h" +#include "../src/typedefs.h" +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using Matrix2r=horizon::Matrix2r; +using Vector2r=horizon::Vector2r; class testIlqr : public ::testing::Test { + protected: testIlqr(){ @@ -70,7 +77,7 @@ TEST_F(testIlqr, checkResiduals) { ilqr->setConstraint({10}, fc); - Eigen::Vector2d x0(1, 1); + Vector2r x0(1, 1); ilqr->setInitialState(x0); diff --git a/horizon/cpp/tests/testQr.cpp b/horizon/cpp/tests/testQr.cpp index f6533653..59b62a43 100644 --- a/horizon/cpp/tests/testQr.cpp +++ b/horizon/cpp/tests/testQr.cpp @@ -4,22 +4,29 @@ #include #include +#include "../src/typedefs.h" + +using Real=horizon::Real; +using MatrixXr=horizon::MatrixXr; +using VectorXr=horizon::VectorXr; +using Matrix2r=horizon::Matrix2r; +using Vector2r=horizon::Vector2r; TEST(testQr, rank) { - Eigen::MatrixXd A, R; - Eigen::HouseholderQR qr; + MatrixXr A, R; + Eigen::HouseholderQR qr; for(int i = 0; i < 100000; i++) { - A = Eigen::MatrixXd::Random(10, 3)*Eigen::MatrixXd::Random(3, 5); + A = MatrixXr::Random(10, 3)*MatrixXr::Random(3, 5); qr.compute(A); R = qr.matrixQR().triangularView(); EXPECT_LT(std::fabs(R(3, 3)), 1e-9); EXPECT_LT(std::fabs(R(4, 4)), 1e-9); - A = Eigen::MatrixXd::Random(5, 3)*Eigen::MatrixXd::Random(3, 10); + A = MatrixXr::Random(5, 3)*MatrixXr::Random(3, 10); qr.compute(A); R = qr.matrixQR().triangularView(); @@ -33,7 +40,7 @@ TEST(testDecomp, compare) int nu = 18 + 12; int nc = 6 + 12; - Eigen::MatrixXd K, Huu, D; + MatrixXr K, Huu, D; K.setZero(nu+nc, nu+nc); Huu.setIdentity(nu, nu); @@ -42,14 +49,14 @@ TEST(testDecomp, compare) K.bottomLeftCorner(nc, nu) = D; K.topRightCorner(nu, nc) = D.transpose(); - Eigen::ColPivHouseholderQR qr(K); - Eigen::BDCSVD bdcsvd(K); - Eigen::JacobiSVD jsvd(K); - Eigen::CompleteOrthogonalDecomposition cod(K); - Eigen::FullPivLU lu(K); + Eigen::ColPivHouseholderQR qr(K); + Eigen::BDCSVD bdcsvd(K); + Eigen::JacobiSVD jsvd(K); + Eigen::CompleteOrthogonalDecomposition cod(K); + Eigen::FullPivLU lu(K); int n_trials = 1000; - std::map> times; + std::map> times; using hrc = std::chrono::high_resolution_clock; hrc::time_point tic, toc; for(int i = 0; i < n_trials; i++) @@ -65,7 +72,7 @@ TEST(testDecomp, compare) tic = hrc::now(); qr.compute(K); - Eigen::MatrixXd q = qr.householderQ(); + MatrixXr q = qr.householderQ(); toc = hrc::now(); times["qr"].push_back((toc - tic).count()*1e-3); @@ -102,21 +109,21 @@ TEST(testDecomp, compare) TEST(testLdlt, basic) { #if false - Eigen::MatrixXd L; + MatrixXr L; L.setZero(3, 3); L.triangularView() = L.Random(3, 3); L.diagonal().setConstant(1); - Eigen::VectorXd d = d.Random(3); + VectorXr d = d.Random(3); - Eigen::MatrixXd K = L*d.asDiagonal()*L.transpose(); + MatrixXr K = L*d.asDiagonal()*L.transpose(); std::cout << "L=\n" << L << std::endl; std::cout << "d = " << d.transpose() << std::endl; int n = K.rows(); - Eigen::VectorXd AP = AP.Zero(n*(n+1)/2); + VectorXr AP = AP.Zero(n*(n+1)/2); Eigen::VectorXi ipiv(n); diff --git a/horizon/functions.py b/horizon/functions.py index abbc7883..39780d84 100644 --- a/horizon/functions.py +++ b/horizon/functions.py @@ -100,7 +100,10 @@ def getNodes(self) -> list: a list of the nodes where the function is active """ - return misc.getNodesFromBinary(self._active_nodes_array) + # we only want to get node indexes which are both feasible and active + active_and_feasible=np.logical_and(self._active_nodes_array,self._feas_nodes_array) + + return misc.getNodesFromBinary(active_and_feasible) def setNodes(self, nodes, erasing=True): """ diff --git a/horizon/problem.py b/horizon/problem.py index 9b63f431..1e219cc4 100644 --- a/horizon/problem.py +++ b/horizon/problem.py @@ -329,9 +329,22 @@ def getDt(self): raise ValueError('dt not defined, have you called setDt?') return self.dt - def setInitialState(self, x0: Iterable): + def setInitialState(self, x0: np.ndarray): self.getState().setBounds(lb=x0, ub=x0, nodes=0) + def setInitialStateSoft(self, x0_meas: np.ndarray, + x0_internal: np.ndarray): + + # set initial state with a "soft approach", which is useful when running a controller + # in closed loop to avoid issues + x_tilde=(x0_internal+x0_meas)/2 + x_sigma=np.absolute((x0_internal-x0_meas)/2) + + # relax state bound on first node depending on the mismatch between the measured + # and internal MPC state. Works also if x0_meas already contains some data + # from MPC + self.getState().setBounds(lb=x_tilde-x_sigma, ub=x_tilde+x_sigma, nodes=0) + def getInitialState(self) -> np.array: lb, ub = self.getState().getBounds(node=0) if np.any(lb != ub): diff --git a/horizon/rhc/RecedingHorizon.py b/horizon/rhc/RecedingHorizon.py index 30160d46..b7871a07 100644 --- a/horizon/rhc/RecedingHorizon.py +++ b/horizon/rhc/RecedingHorizon.py @@ -6,8 +6,6 @@ from horizon.rhc.action_manager.ActionManager import ActionManager import numpy as np import horizon.utils.kin_dyn as kd -import matplotlib.pyplot as plt -from horizon.ros import replay_trajectory from typing import Union from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint @@ -299,6 +297,8 @@ def __init_replayer(self): raise Exception("ROS required for replayer.") contact_list_repl = list(self.model.cmap.keys()) + from horizon.ros import replay_trajectory + self.repl = replay_trajectory.replay_trajectory(self.dt, self.model.kd.joint_names(), np.array([]), {k: None for k in self.model.fmap.keys()}, self.model.kd_frame, self.model.kd, diff --git a/horizon/rhc/model_description.py b/horizon/rhc/model_description.py index 8b52b636..73a86c83 100644 --- a/horizon/rhc/model_description.py +++ b/horizon/rhc/model_description.py @@ -9,8 +9,6 @@ import urdf_parser_py.urdf as upp from collections import OrderedDict -np.set_printoptions(precision=3, suppress=True) - class FullModelInverseDynamics: def __init__(self, problem, kd, q_init, base_init=None, floating_base=True, fixed_joint_map=None, sys_order_degree=2, **kwargs): @@ -630,6 +628,8 @@ def getContacts(self): if __name__ == '__main__': + np.set_printoptions(precision=3, suppress=True) + import rospkg import casadi_kin_dyn.py3casadi_kin_dyn as casadi_kin_dyn diff --git a/horizon/rhc/taskInterface.py b/horizon/rhc/taskInterface.py index 269a136b..9c463a01 100644 --- a/horizon/rhc/taskInterface.py +++ b/horizon/rhc/taskInterface.py @@ -1,6 +1,5 @@ from horizon.utils import kin_dyn, mat_storer, resampler_trajectory -from casadi_kin_dyn import pycasadi_kin_dyn from horizon.rhc.tasks.cartesianTask import CartesianTask from horizon.rhc.tasks.contactTask import ContactTask from horizon.rhc.tasks.interactionTask import InteractionTask, SurfaceContact, VertexContact @@ -16,14 +15,31 @@ from horizon.rhc import task_factory, plugin_handler, solver_interface from horizon.rhc.yaml_handler import YamlParser from horizon.solvers.solver import Solver -from horizon.ros.replay_trajectory import replay_trajectory + +import copy + +# from horizon.ros.replay_trajectory import replay_trajectory + import time class ProblemInterface: def __init__(self, - prb, - model): + prb, + model, + max_solver_iter: int = 1, + debug: bool = False, + verbose: bool = False, + codegen_workdir: str = "/tmp/tyhio"): + + self._debug = debug + self._verbose = verbose + + self._codegen_workdir = codegen_workdir + + self.max_solver_iter = max_solver_iter + + self.rt_solve_time = -1.0 # get the model self.prb = prb @@ -32,6 +48,11 @@ def __init__(self, self.solver_bs = None self.solver_rti = None + self.solution = {} + self.bootstrap_sol = {} + + self.bootstrap_solved = False + def finalize(self, rti=True): """ to be called after all variables have been created @@ -40,29 +61,121 @@ def finalize(self, rti=True): self._create_solver(rti) def bootstrap(self): + + # this is called sporadically: we don't really care + # about printing overheads here t = time.time() self.solver_bs.solve() elapsed = time.time() - t print(f'bootstrap solved in {elapsed} s') + try: self.solver_rti.print_timings() + except: pass + self.solution = self.solver_bs.getSolutionDict() - def rti(self): + # we backup a copy (needs to be deep to work properly) + # of the bootstrap, which can be used to reset the controller + # if needed + + self.update_bootstrap_from_sol() + + self.bootstrap_solved = True + + def update_bootstrap_from_sol(self): + + # updates bootstrap backup with latest available solution + + self.bootstrap_sol = copy.deepcopy(self.solution) + def reset(self): + + # copies latest bootstrap into solution + + self.solver_rti.reset() # resets solver internal state (useful in case of failure) + + self.solution = copy.deepcopy(self.bootstrap_sol) + # resets the controller with the latest solution + + self.load_initial_guess() + + def rti(self): + + if self._verbose: + self._rti_db() + else: + self._rti_min() + + def _rti_db(self): + t = time.time() check = self.solver_rti.solve() - elapsed = time.time() - t - print(f'rti solved in {elapsed} s') - + self.rt_solve_time = time.time() - t + print(f'rti solved in {self.rt_solve_time} s') self.solution = self.solver_rti.getSolutionDict() + return check + + def _rti_min(self): + check = self.solver_rti.solve() + self.solution = self.solver_rti.getSolutionDict() return check - def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): + def init_inv_dyn_for_res(self): + + # we create the inv dynamics for resampling here + # to avoid runtime overhead + + if (self.bootstrap_solved): # we need to have the + # force map (in particular the keys) from the solution, so we wait for the bootstrap, + # since it's solved during the initialization phase and not + # at runtime + + self.fmap = dict() + for frame, wrench in self.model.fmap.items(): + self.fmap[frame] = self.solution[f'{wrench.getName()}'] + + self.res_id = kin_dyn.InverseDynamics(self.model.kd, + self.fmap.keys(), + self.model.kd_frame) + + self.tau_eval = np.zeros([self.model.tau.shape[0], + self.prb.getNNodes() - 1]) # evaluated tau on nodes + + self.fmap_0 = dict() # we initialize also the force map with the wrenches on the + # first noe + + else: + raise Exception("The method init_inv_dyn_for_res from " + __class__.__name__ + + " can only be called after bootstrap() has returned!") + + def eval_efforts_on_node(self, node_idx: int =0): + + for frame, wrench in self.model.fmap.items(): + + # we update the force maps from the latest solution + + self.fmap_0[frame] = self.solution[f'{wrench.getName()}'][:, node_idx] # it's an input + # we get it from node 0 + + # compute torque with inverse dynamics (states from node 1, inputs from + # node 0) + tau_i = self.res_id.call(self.solution['q'][:, node_idx], + self.solution['v'][:, node_idx], + self.solution['a'][:, node_idx], + self.fmap_0) + + tau_array=tau_i.toarray() + tau_array[:, :]=np.nan_to_num(tau_array, copy=True, nan=300.0, + posinf=300, neginf=-300) # handle not finite vals + return np.clip(tau_array, a_min=-300, a_max=300) + + def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): + if nodes is None: nodes = list(range(self.prb.getNNodes() + 1)) @@ -101,20 +214,11 @@ def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): # new fmap with resampled forces if self.model.fmap: - fmap = dict() - for frame, wrench in self.model.fmap.items(): - fmap[frame] = self.solution[f'{wrench.getName()}'] - - fmap_res = dict() - for frame, wrench in self.model.fmap.items(): - fmap_res[frame] = self.solution[f'{wrench.getName()}_res'] - # get tau resampled if resample_tau: - tau = np.zeros([self.model.tau.shape[0], self.prb.getNNodes() - 1]) - tau_res = np.zeros([self.model.tau.shape[0], u_res.shape[1]]) - id = kin_dyn.InverseDynamics(self.model.kd, fmap_res.keys(), self.model.kd_frame) + tau_res = np.zeros([self.model.tau.shape[0], u_res.shape[1]]) # we create this at runtime + # (can be improved) # id_fn = kin_dyn.InverseDynamics(self.kd, self.fmap.keys(), self.kd_frame) # self.tau = id_fn.call(self.q, self.v, self.a, self.fmap) @@ -122,23 +226,33 @@ def resample(self, dt_res, dae=None, nodes=None, resample_tau=True): # todo: this is horrible. id.call should take matrices, I should not iter over each node - for i in range(tau.shape[1]): + for i in range(self.tau_eval.shape[1]): + fmap_i = dict() - for frame, wrench in fmap.items(): + for frame, wrench in self.fmap.items(): fmap_i[frame] = wrench[:, i] - tau_i = id.call(self.solution['q'][:, i], self.solution['v'][:, i], self.solution['a'][:, i], + + tau_i = self.res_id.call(self.solution['q'][:, i], + self.solution['v'][:, i], + self.solution['a'][:, i], fmap_i) - tau[:, i] = tau_i.toarray().flatten() + + self.tau_eval[:, i] = tau_i.toarray().flatten() for i in range(tau_res.shape[1]): + fmap_res_i = dict() - for frame, wrench in fmap_res.items(): + for frame, wrench in self.fmap.items(): fmap_res_i[frame] = wrench[:, i] - tau_res_i = id.call(self.solution['q_res'][:, i], self.solution['v_res'][:, i], - self.solution['a_res'][:, i], fmap_res_i) + + tau_res_i = self.res_id.call(self.solution['q_res'][:, i], + self.solution['v_res'][:, i], + self.solution['a_res'][:, i], + fmap_res_i) + tau_res[:, i] = tau_res_i.toarray().flatten() - self.solution['tau'] = tau + self.solution['tau'] = self.tau_eval self.solution['tau_res'] = tau_res def save_solution(self, filename): @@ -176,32 +290,32 @@ def load_initial_guess(self, from_dict=None): self.prb.getState().setInitialGuess(x_opt) self.prb.getInput().setInitialGuess(u_opt) - self.prb.setInitialState(x0=x_opt[:, 0]) + # self.prb.setInitialState(x0=x_opt[:, 0]) - def replay_trajectory(self, trajectory_markers=[], trajectory_markers_opts={}): + # def replay_trajectory(self, trajectory_markers=[], trajectory_markers_opts={}): - # single replay - joint_names = self.model.kd.joint_names() - q_sol = self.solution['q'] - q_sol_minimal = np.zeros([q_sol.shape[0], self.prb.getNNodes()]) + # # single replay + # joint_names = self.model.kd.joint_names() + # q_sol = self.solution['q'] + # q_sol_minimal = np.zeros([q_sol.shape[0], self.prb.getNNodes()]) - # if q is not minimal (continuous joints are present) make it minimal - for col in range(q_sol.shape[1]): - q_sol_minimal[:, col] = self.model.kd.getMinimalQ(q_sol[:, col]) + # # if q is not minimal (continuous joints are present) make it minimal + # for col in range(q_sol.shape[1]): + # q_sol_minimal[:, col] = self.model.kd.getMinimalQ(q_sol[:, col]) - frame_force_mapping = {cname: self.solution[f.getName()] for cname, f in self.model.fmap.items()} + # frame_force_mapping = {cname: self.solution[f.getName()] for cname, f in self.model.fmap.items()} - repl = replay_trajectory(self.prb.getDt(), - joint_names, - q_sol_minimal, - frame_force_mapping, - self.model.kd_frame, - self.model.kd, - fixed_joint_map=self.model.fixed_joint_map, - trajectory_markers=trajectory_markers, - trajectory_markers_opts=trajectory_markers_opts) - repl.sleep(1.) - repl.replay(is_floating_base=True, base_link='pelvis') + # repl = replay_trajectory(self.prb.getDt(), + # joint_names, + # q_sol_minimal, + # frame_force_mapping, + # self.model.kd_frame, + # self.model.kd, + # fixed_joint_map=self.model.fixed_joint_map, + # trajectory_markers=trajectory_markers, + # trajectory_markers_opts=trajectory_markers_opts) + # repl.sleep(1.) + # repl.replay(is_floating_base=True, base_link='pelvis') def setSolverOptions(self, solver_options): solver_type = solver_options.pop('type') @@ -216,7 +330,14 @@ def _create_solver(self, rti=True): th = Transcriptor.make_method('multiple_shooting', self.prb) # todo if receding is true .... - self.solver_bs = Solver.make_solver(self.si.type, self.prb, self.si.opts) + scoped_opts_bs = self.si.opts.copy() + scoped_opts_bs['ilqr.debug'] = self._debug + scoped_opts_bs['ilqr.verbose'] = self._verbose + scoped_opts_bs['ilqr.codegen_verbose'] = self._verbose + scoped_opts_bs['ilqr.log_iterations'] = False + scoped_opts_bs['ilqr.codegen_workdir'] = self._codegen_workdir + + self.solver_bs = Solver.make_solver(self.si.type, self.prb, scoped_opts_bs) try: self.solver_bs.set_iteration_callback() @@ -224,9 +345,21 @@ def _create_solver(self, rti=True): pass if rti: + scoped_opts_rti = self.si.opts.copy() - scoped_opts_rti['ilqr.enable_line_search'] = False - scoped_opts_rti['ilqr.max_iter'] = 1 + + scoped_opts_rti['ilqr.max_iter'] = self.max_solver_iter + scoped_opts_rti['ilqr.debug'] = self._debug # enables debugging in iLQR (basically + # allows to retrieve costs and constraints values at runtime) + scoped_opts_rti['ilqr.verbose'] = self._verbose + scoped_opts_rti['ilqr.codegen_verbose'] = self._debug + scoped_opts_rti['ilqr.rti'] = True + scoped_opts_rti['ilqr.log_iterations'] = False # debugging iLQR logs + scoped_opts_rti['ilqr.codegen_workdir'] = self._codegen_workdir + if self.max_solver_iter == 1: + # real-time iteration -> no line-search necessary + scoped_opts_rti['ilqr.enable_line_search'] = False + self.solver_rti = Solver.make_solver(self.si.type, self.prb, scoped_opts_rti) return self.solver_bs, self.solver_rti @@ -236,10 +369,18 @@ def getProblem(self): class TaskInterface(ProblemInterface): def __init__(self, - prb, - model): - - super().__init__(prb, model) + prb, + model, + max_solver_iter: int = 1, + debug = False, + verbose = False, + codegen_workdir: str = "/tmp/tyhio"): + + super().__init__(prb, model, + max_solver_iter, + debug, + verbose, + codegen_workdir) # here I register the the default tasks # todo: should I do it here? diff --git a/horizon/rhc/tasks/contactTask.py b/horizon/rhc/tasks/contactTask.py index 10fa8030..928bb5be 100644 --- a/horizon/rhc/tasks/contactTask.py +++ b/horizon/rhc/tasks/contactTask.py @@ -12,11 +12,9 @@ def __init__(self, subtask, establish/break contact """ - # todo : default interaction or cartesian task ? - # todo : make tasks discoverable by name? subtask: {'interaction': force_contact_1} - self.dynamics_task: InteractionTask = Task.subtask_by_class(subtask, InteractionTask) + self.dynamics_tasks: InteractionTask = Task.subtask_by_class(subtask, InteractionTask) # allowed tasks are cartesian and rolling - self.kinematics_task: CartesianTask = Task.subtask_by_class(subtask, (CartesianTask, RollingTask)) # CartesianTask RollingTask + self.kinematics_tasks: CartesianTask = Task.subtask_by_class(subtask, (CartesianTask, RollingTask)) # CartesianTask RollingTask # initialize data class super().__init__(*args, **kwargs) @@ -28,6 +26,8 @@ def __initialize(self): self.setNodes(self.nodes) def setNodes(self, nodes, erasing=True): - - self.dynamics_task.setContact(nodes, erasing=erasing) # this is from taskInterface - self.kinematics_task.setNodes(nodes, erasing=erasing) # state + starting from node 1 # this is from taskInterface + + for task in self.dynamics_tasks: + task.setContact(nodes, erasing=erasing) # this is from taskInterface + for task in self.kinematics_tasks: + task.setNodes(nodes, erasing=erasing) # state + starting from node 1 # this is from taskInterface diff --git a/horizon/rhc/tasks/interactionTask.py b/horizon/rhc/tasks/interactionTask.py index 11595b53..4fc18a76 100644 --- a/horizon/rhc/tasks/interactionTask.py +++ b/horizon/rhc/tasks/interactionTask.py @@ -181,6 +181,8 @@ def __init__(self, frame, vertex_frames, *args, **kwargs): # init base super().__init__(frame, *args, **kwargs) + self.vertex_frames=vertex_frames + # ask model to create vertex forces self.forces = self.model.setContactFrame(frame, 'vertex', @@ -206,13 +208,17 @@ def __initialize(self): def make_fn_barrier(self): - fn_barrier_cost = [] - for f in self.forces: - fn_barrier_cost.append(barrier_fun(f[2] - self.fn_min)) - fn_barrier_cost = cs.vertcat(*fn_barrier_cost) - fn_barrier = self.prb.createResidual(f'{self.frame}_unil_barrier', 1e1 * fn_barrier_cost, self.all_nodes) - return fn_barrier + if not self.fn_min < -1e3: + fn_barrier_cost = [] + for f in self.forces: + fn_barrier_cost.append(barrier_fun(f[2] - self.fn_min)) + fn_barrier_cost = cs.vertcat(*fn_barrier_cost) + fn_barrier = self.prb.createResidual(f'{self.frame}_unil_barrier', 1e1 * fn_barrier_cost, self.all_nodes) + return fn_barrier + else: + return None + def make_friction_cone(self): fcost = [] for f in self.forces: @@ -249,7 +255,8 @@ def setContact(self, nodes, erasing=True): # start_time3 = time.time() # add normal force constraint - self.fn_barrier.setNodes(good_nodes, erasing=erasing) + if self.fn_barrier is not None: + self.fn_barrier.setNodes(good_nodes, erasing=erasing) # end_time3 = time.time() - start_time3 # start_time4 = time.time() diff --git a/horizon/rhc/tasks/regularizationTask.py b/horizon/rhc/tasks/regularizationTask.py index 2bfec1f9..826d473e 100644 --- a/horizon/rhc/tasks/regularizationTask.py +++ b/horizon/rhc/tasks/regularizationTask.py @@ -4,21 +4,60 @@ from horizon.problem import Problem import numpy as np - # todo: better to do an aggregate class RegularizationTask(Task): - def __init__(self, variable_name, *args, **kwargs): + @classmethod + def from_dict(cls, task_description): + opt_variable = [] if 'variable' not in task_description else task_description['variable'] + + if 'weight' in task_description and isinstance(task_description['weight'], dict): + opt_variable = list(task_description['weight'].keys()) + task_description['weight'] = list(task_description['weight'].values()) + + task = cls(opt_variable, **task_description) + return task + + def __init__(self, opt_variable_names, *args, **kwargs): super().__init__(*args, **kwargs) self._createWeightParam() - self.opt_reference = dict() - self.indices_dict = dict() - - try: - self.opt_variable = self.prb.getVariables(variable_name) - except: - raise Exception("variable not found.") + self.opt_varariable_dim = [] + self.opt_variable_list = [] + if not isinstance(opt_variable_names, list): + var=self.prb.getVariables(opt_variable_names) + if var is not None: + self.opt_variable_list.append(var) + self.opt_varariable_dim.append(var.getDim()) + else: + self.opt_varariable_dim.append(None) + else: + for name in opt_variable_names: + var=self.prb.getVariables(name) + var_dim=None + if var is not None: + var_dim=var.getDim() + self.opt_variable_list.append(var) + self.opt_varariable_dim.append(var_dim) + if None in self.opt_variable_list: + raise ValueError(f'variable inserted is not in the problem.') + + if None in self.opt_variable_list: + raise ValueError(f'variable inserted is not in the problem.') + + var_dim_match=all(x == self.opt_varariable_dim[0] for x in self.opt_varariable_dim) + if not var_dim_match: + incorrect_dims=list(map(str, self.opt_varariable_dim)) + raise ValueError(f'Dimensions of variables do not match! -> {", ".join(incorrect_dims)}') + + self.indices = np.array(list(range(self.opt_varariable_dim[0]))).astype(int) if self.indices is None else np.array(self.indices).astype(int) + + indices_within_bounds=all(x for _, v in subtask.items(): if isinstance(v, classname): ret.append(v) - return ret[0] if len(ret) == 1 else ret + return ret def __post_init__(self): # todo: this is for simplicity diff --git a/horizon/ros/trajectory_viewer.py b/horizon/ros/trajectory_viewer.py index 6e8a4c11..cb6b3219 100644 --- a/horizon/ros/trajectory_viewer.py +++ b/horizon/ros/trajectory_viewer.py @@ -8,7 +8,6 @@ from sensor_msgs.msg import JointState import subprocess import time -from numpy_ros import to_numpy, to_message class TrajectoryViewer: @@ -109,7 +108,10 @@ def publish_line(self, points): for col in range(points.shape[1]): - point = to_message(Point, points[:3, col]) + point.x = points[0, col] + point.y = points[1, col] + point.z = points[2, col] + marker.points.append(point) self.line_array.markers.append(marker) @@ -141,4 +143,4 @@ def publish_line(self, points): rate.sleep() # # rospy.sleep(0.5) - # rospy.spin() \ No newline at end of file + # rospy.spin() diff --git a/horizon/solvers/ilqr.py b/horizon/solvers/ilqr.py index 5f73efd9..d216894e 100644 --- a/horizon/solvers/ilqr.py +++ b/horizon/solvers/ilqr.py @@ -12,7 +12,6 @@ from horizon.transcriptions import integrators import casadi as cs import numpy as np -from matplotlib import pyplot as plt class SolverILQR(Solver): @@ -32,7 +31,7 @@ def __init__(self, # save max iter if any self.max_iter = self.opts.get('ilqr.max_iter', 100) - + # num shooting interval self.N = prb.getNNodes() - 1 @@ -72,7 +71,8 @@ def __init__(self, # create ilqr solver self.ilqr = IterativeLQR(self.prb.getIntegrator(), self.N, self.opts) - + self.ilqr.reset() + # should we use GN approx for residuals? self.use_gn = self.opts.get('ilqr.enable_gn', False) @@ -83,6 +83,7 @@ def __init__(self, # set a default iteration callback self.plot_iter = False + self.xax = None self.uax = None self.dax = None @@ -91,9 +92,20 @@ def __init__(self, # empty solution dict self.solution_dict = dict() + self.current_iteration = 0 + self.iteration_costs = [] + # print iteration statistics self.set_iteration_callback() + self.set_iteration_callback(self._sol_info_callback) + + self._mem_usage = 0.0 + + def reset(self): + + self.ilqr.reset() + def save(self): data = self.prb.save() data['solver'] = dict() @@ -104,25 +116,32 @@ def save(self): data['dynamics'] = self.dyn.serialize() return data - def set_iteration_callback(self, cb=None): if cb is None: self.ilqr.setIterationCallback(self._iter_callback) else: - print('setting custom iteration callback') + # print('setting custom iteration callback') self.ilqr.setIterationCallback(cb) + def _sol_info_callback(self, fpres): + + self.current_iteration = self.current_iteration + 1 + + self.iteration_costs.append(fpres.cost) def configure_rti(self) -> bool: self.opts['max_iter'] = 1 def solve(self): - + + self.iteration_costs = [] # resets costs data + self.current_iteration = 0 # resets iteration counter + # set initial state x0 = self.prb.getInitialState() xinit = self.prb.getState().getInitialGuess() uinit = self.prb.getInput().getInitialGuess() - + # update initial guess self.ilqr.setStateInitialGuess(xinit) self.ilqr.setInputInitialGuess(uinit) @@ -159,6 +178,11 @@ def solve(self): self.solution_dict['x_opt'] = self.x_opt self.solution_dict['u_opt'] = self.u_opt + self.solution_dict['opt_cost'] = self.iteration_costs[-1] if len(self.iteration_costs) else -1.0 + self.solution_dict['iter_costs'] = np.array(self.iteration_costs) + self.solution_dict['n_iter2sol'] = self.current_iteration + self.solution_dict['residual_norm'] = self.getResidualNorm() + return ret def getSolutionDict(self): @@ -187,6 +211,15 @@ def getConstraintsValues(self): def getCostsValues(self): return self.ilqr.getCostsValues() + def getResidualNorm(self): + return self.ilqr.getResidualNorm() + + def getConstrValOnNodes(self): + return self.ilqr.getConstrValOnNodes() + + def getCostValOnNodes(self): + return self.ilqr.getCostValOnNodes() + def print_timings(self): prof_info = self.ilqr.getProfilingInfo() @@ -339,6 +372,7 @@ def _iter_callback(self, fpres): # print(f'il male รจ {fpres.constraint_values[0]} + {np.linalg.norm(fpres.defect_values[:, 0])}') if self.plot_iter: + from matplotlib import pyplot as plt if self.dax is None: _, (self.dax, self.hax) = plt.subplots(2) diff --git a/horizon/solvers/nlpsol.py b/horizon/solvers/nlpsol.py index 8d1473cf..7105006f 100644 --- a/horizon/solvers/nlpsol.py +++ b/horizon/solvers/nlpsol.py @@ -6,7 +6,6 @@ import numpy as np import pprint - class NlpsolSolver(Solver): def __init__(self, prb: Problem, opts: Dict, solver_plugin: str) -> None: @@ -41,9 +40,70 @@ def __init__(self, prb: Problem, opts: Dict, solver_plugin: str) -> None: self.prob_dict = {'f': j, 'x': w, 'g': g, 'p': p} + # callback to get the iteration count + self.iter_counter_callback = self.IterCountCallback('iter_counter_callback', w.shape[0], g.shape[0], p.shape[0], + opts = self.opts) + self.opts['iteration_callback'] = self.iter_counter_callback + # create solver from prob self.solver = cs.nlpsol('solver', solver_plugin, self.prob_dict, self.opts) + class IterCountCallback(cs.Callback): + def __init__(self, name, nx, ng, np, opts={}): + + cs.Callback.__init__(self) + + self.solv_opts = opts + self.constr_viol = self.solv_opts["ipopt.constr_viol_tol"] + + self.nx = nx + self.ng = ng + self.np = np + + self.iter_counter = -1 + self.cost_values = [] + # self.is_feasible = [] + + # Initialize internal objects + self.construct(name, {}) + + def get_n_in(self): return cs.nlpsol_n_out() + def get_n_out(self): return 1 + def get_name_in(self, i): return cs.nlpsol_out(i) + def get_name_out(self, i): return "ret" + + def get_sparsity_in(self, i): + n = cs.nlpsol_out(i) + if n=='f': + return cs.Sparsity. scalar() + elif n in ('x', 'lam_x'): + return cs.Sparsity.dense(self.nx) + elif n in ('g', 'lam_g'): + return cs.Sparsity.dense(self.ng) + else: + return cs.Sparsity(0,0) + + def eval(self, arg): + + # add here any info to be retrieved from the solver + + self.cost_values.append(arg[1]) + self.iter_counter = len(self.cost_values) + + constraint_violations = arg[2] + n_cnstrnts = constraint_violations.shape[0] + + within_feasibility = np.zeros((n_cnstrnts, 1), dtype=bool) + for i in range(0, n_cnstrnts): + # DM types not iterable by default + + within_feasibility[i] = constraint_violations[i] <= self.solv_opts["ipopt.constr_viol_tol"] and \ + constraint_violations[i] >= - self.solv_opts["ipopt.constr_viol_tol"] + + # self.is_feasible.append(np.all(within_feasibility)) + + return [0] + def build(self): """ fill the dictionary "state_var_impl" @@ -95,6 +155,8 @@ def build(self): def solve(self) -> bool: + self.iter_counter_callback.iter_counter = 0 # resetting iteration number at each solve + # update lower/upper bounds of variables lbw = self._getVarList('lb') ubw = self._getVarList('ub') @@ -129,11 +191,21 @@ def solve(self) -> bool: self.cnstr_solution = self._createCnsrtSolDict(sol) + self.lambd_solution = self._createCnsrtLambDict(sol) + #adding lagrange multipliers list of g to dict + # self.cnstr_solution['lam_x'] = np.array(sol['lam_x']) + self.lambd_solution['lam_g'] = np.array(sol['lam_g']) + # retrieve state and input trajector # get solution dict self.var_solution = self._createVarSolDict(sol) + # adding additional info + self.var_solution["opt_cost"] = float(sol['f']) + self.var_solution["n_iter2sol"] = self.iter_counter_callback.iter_counter + self.var_solution["cost_values"] = self.iter_counter_callback.cost_values + # get solution as state/input self._createVarSolAsInOut(sol) self.var_solution['x_opt'] = self.x_opt @@ -150,6 +222,9 @@ def getSolutionDict(self): def getConstraintSolutionDict(self): return self.cnstr_solution + def getCnstrLmbdSolDict(self): + return self.lambd_solution + def getDt(self): return self.dt_solution diff --git a/horizon/solvers/solver.py b/horizon/solvers/solver.py index d6cdef5d..159284e4 100644 --- a/horizon/solvers/solver.py +++ b/horizon/solvers/solver.py @@ -56,7 +56,6 @@ def make_solver(cls, ret.type = type return ret - def __init__(self, prb: Problem, opts: Dict = None) -> None: @@ -99,7 +98,6 @@ def __init__(self, self.configure_rti() del self.opts['realtime_iteration'] - def _getVarList(self, type): var_list = list() for var in self.prb.var_container.getVarList(offset=False): @@ -141,6 +139,22 @@ def _getFunList(self, type): f = cs.vertcat(*fun_list) return f + def _createCnsrtLambDict(self, solution): + + lambd_cnsrt_dict = dict() + pos = 0 + for name, fun in self.prb.function_container.getCnstr().items(): # iterate through each symbolic constraint + + #extracting and reshaping the constraint values associated to name + lam_g_vals = solution['lam_g'][pos:pos + fun.getDim() * len(fun.getNodes())] + lam_g_vals_mat = np.reshape(lam_g_vals, (fun.getDim(), len(fun.getNodes())), order='F') + + lambd_cnsrt_dict[name + "_lambd"] = lam_g_vals_mat + + pos = pos + fun.getDim() * len(fun.getNodes()) + + return lambd_cnsrt_dict + def _createCnsrtSolDict(self, solution): fun_sol_dict = dict() @@ -276,6 +290,13 @@ def solve(self) -> bool: bool: success flag """ pass + + def reset(self): + """ + Resets solver (to be overridden by child class) + """ + + pass def getSolutionDict(self): """ diff --git a/horizon/utils/trajectoryGenerator.py b/horizon/utils/trajectoryGenerator.py index e47c0375..56719bdc 100644 --- a/horizon/utils/trajectoryGenerator.py +++ b/horizon/utils/trajectoryGenerator.py @@ -1,15 +1,9 @@ -import numpy as np -import matplotlib.pyplot as plt -from scipy.optimize import curve_fit -from scipy.interpolate import splprep - - -from numpy import linspace, sin, pi from scipy.interpolate import BPoly, CubicSpline - +import numpy as np class TrajectoryGenerator: + def __init__(self): pass @@ -20,22 +14,18 @@ def sin_trj(self, tau): return np.sin(tau * np.pi) def bezier_trj(self, tau): - P0 = 1 P1 = 3 P2 = 2 fun = (1 - tau) ** 2 * P0 + 2 * (1 - tau) * tau * P1 + tau * 2 * P2 - return fun + def compute_polynomial_trajectory(self, k_start, nodes, nodes_duration, p_start, p_goal, clearance, dim=None): if dim is None: dim = [0, 1, 2] - # todo check dimension of parameter before assigning it - traj_array = np.zeros(len(nodes)) - start = p_start[dim] goal = p_goal[dim] @@ -49,11 +39,13 @@ def compute_polynomial_trajectory(self, k_start, nodes, nodes_duration, p_start, return np.array(traj_array) - def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): - + def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): if derivatives is None: derivatives = [None] * nodes - + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes cxi = [0, 0.5, 1] @@ -62,25 +54,34 @@ def from_derivatives(self, nodes, p_start, p_goal, clearance, derivatives=None, else: cyi = [p_start, p_goal + clearance, p_goal] - xcurve = linspace(0, 1, nodes) + xcurve = np.linspace(0, 1, nodes) yder = [] - for i, val in enumerate(derivatives): - yder.append([cyi[i], val] if val is not None else [cyi[i]]) + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + if d3 is not None: + constraints.append(d3) + yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) y_bpoly = bpoly(xcurve) return y_bpoly - def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None): + def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): if derivatives is None: derivatives = [None] * nodes - if second_der is None: second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes cxi = [0, 0.5, 1] + if p_start >= p_goal: cyi = [p_start, p_start + clearance, p_goal] else: @@ -89,16 +90,17 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative xcurve = np.linspace(0, 1, nodes) yder = [] - for i, (d1, d2) in enumerate(zip(derivatives, second_der)): + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): constraints = [cyi[i]] if d1 is not None: constraints.append(d1) if d2 is not None: constraints.append(d2) + if d3 is not None: + constraints.append(d3) yder.append(constraints) bpoly = BPoly.from_derivatives(cxi, yder) - # Compute the first derivative of the BPoly object bpoly_derivative = bpoly.derivative() @@ -107,6 +109,114 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative return y_bpoly_derivative + def second_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): + """ + Compute the second derivative of the trajectory. + + Parameters: + - nodes: int, number of points in the trajectory + - p_start: float, start position + - p_goal: float, goal position + - clearance: float, distance above or below the straight line + - derivatives: list of first derivative constraints or `None` + - second_der: list of second derivative constraints or `None` + - third_der: list of third derivative constraints or `None` + + Returns: + - y_bpoly_second_derivative: np.ndarray, second derivative values + """ + if derivatives is None: + derivatives = [None] * nodes + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes + + cxi = [0, 0.5, 1] + + # Adjust clearance for the middle point + if p_start >= p_goal: + cyi = [p_start, p_start + clearance, p_goal] + else: + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + if d3 is not None: + constraints.append(d3) + yder.append(constraints) + + # Create the BPoly object from derivatives + bpoly = BPoly.from_derivatives(cxi, yder) + # Compute the second derivative of the BPoly object + bpoly_second_derivative = bpoly.derivative().derivative() + + # Evaluate the second derivative at the given points + y_bpoly_second_derivative = bpoly_second_derivative(xcurve) + + return y_bpoly_second_derivative + + def third_derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivatives=None, second_der=None, third_der=None): + """ + Compute the third derivative of the trajectory. + + Parameters: + - nodes: int, number of points in the trajectory + - p_start: float, start position + - p_goal: float, goal position + - clearance: float, distance above or below the straight line + - derivatives: list of first derivative constraints or `None` + - second_der: list of second derivative constraints or `None` + - third_der: list of third derivative constraints or `None` + + Returns: + - y_bpoly_third_derivative: np.ndarray, third derivative values + """ + if derivatives is None: + derivatives = [None] * nodes + if second_der is None: + second_der = [None] * nodes + if third_der is None: + third_der = [None] * nodes + + cxi = [0, 0.5, 1] + + # Adjust clearance for the middle point + if p_start >= p_goal: + cyi = [p_start, p_start + clearance, p_goal] + else: + cyi = [p_start, p_goal + clearance, p_goal] + + xcurve = np.linspace(0, 1, nodes) + + yder = [] + for i, (d1, d2, d3) in enumerate(zip(derivatives, second_der, third_der)): + constraints = [cyi[i]] + if d1 is not None: + constraints.append(d1) + if d2 is not None: + constraints.append(d2) + if d3 is not None: + constraints.append(d3) + yder.append(constraints) + + # Create the BPoly object from derivatives + bpoly = BPoly.from_derivatives(cxi, yder) + # Compute the third derivative of the BPoly object + bpoly_third_derivative = bpoly.derivative().derivative().derivative() + + # Evaluate the third derivative at the given points + y_bpoly_third_derivative = bpoly_third_derivative(xcurve) + + return y_bpoly_third_derivative + if __name__ == '__main__': import matplotlib.pyplot as plt @@ -115,45 +225,61 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative n_samples = 100 - der = [None, 0, 0] - second_der = [None, 0, 0] + # Specify derivatives for testing + der = [None, 0, 0] # First derivatives at key points + second_der = [None, None, 0] # Second derivatives at key points + third_der = [None, None, 0] # Third derivatives at key points - # Original trajectory with first and second derivative constraints + # Compute the trajectory z_trj = tg.from_derivatives( n_samples, - -1, - -1, - 1, + 0, + 0, + 0.12, derivatives=der, - second_der=second_der + second_der=second_der, + third_der=third_der ) - # First derivative of the trajectory with first and second derivative constraints + # First derivative of the trajectory z_trj_derivative = tg.derivative_of_trajectory( n_samples, - -1, - -1, - 1, + 0, + 0, + 0.12, + derivatives=der, + second_der=second_der, + third_der=third_der + ) + + # Compute the second derivative of the trajectory + z_trj_second_derivative = tg.second_derivative_of_trajectory( + n_samples, + 0, + 0, + 0.12, derivatives=der, - second_der=second_der + second_der=second_der, + third_der=third_der ) - # # Second derivative of the trajectory with first and second derivative constraints - # z_trj_second_derivative = tg.second_derivative_of_trajectory( - # n_samples, - # -1, - # -1, - # 1, - # derivatives=der, - # second_der=second_der - # ) + # Compute the third derivative of the trajectory + z_trj_third_derivative = tg.third_derivative_of_trajectory( + n_samples, + 0, + 0, + 0.12, + derivatives=der, + second_der=second_der, + third_der=third_der + ) axis = np.linspace(0, 1, num=z_trj.shape[0]) - # Plotting the trajectory, its first derivative, and its second derivative - plt.figure(figsize=(18, 6)) + # Plot the trajectory, its first, second, and third derivatives + plt.figure(figsize=(18, 8)) - plt.subplot(1, 3, 1) + plt.subplot(2, 2, 1) plt.plot(axis, z_trj, label="Trajectory") plt.title("Trajectory") plt.xlabel("Time (normalized)") @@ -161,21 +287,29 @@ def derivative_of_trajectory(self, nodes, p_start, p_goal, clearance, derivative plt.grid() plt.legend() - plt.subplot(1, 3, 2) - plt.plot(axis, z_trj_derivative, label="Trajectory Derivative", linestyle="--") - plt.title("Derivative of Trajectory") + plt.subplot(2, 2, 2) + plt.plot(axis, z_trj_derivative, label="First Derivative", linestyle="--") + plt.title("First Derivative of Trajectory") plt.xlabel("Time (normalized)") plt.ylabel("Velocity") plt.grid() plt.legend() - # plt.subplot(1, 3, 3) - # plt.plot(axis, z_trj_second_derivative, label="Trajectory Second Derivative", linestyle="-.") - # plt.title("Second Derivative of Trajectory") - # plt.xlabel("Time (normalized)") - # plt.ylabel("Acceleration") - # plt.grid() - # plt.legend() + plt.subplot(2, 2, 3) + plt.plot(axis, z_trj_second_derivative, label="Second Derivative", linestyle="-.") + plt.title("Second Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Acceleration") + plt.grid() + plt.legend() + + plt.subplot(2, 2, 4) + plt.plot(axis, z_trj_third_derivative, label="Third Derivative", linestyle=":") + plt.title("Third Derivative of Trajectory") + plt.xlabel("Time (normalized)") + plt.ylabel("Jerk") + plt.grid() + plt.legend() plt.tight_layout() - plt.show() \ No newline at end of file + plt.show() diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..5c564fa6 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,11 @@ +[build-system] +requires = ["flit_core >=2,<4"] +build-backend = "flit_core.buildapi" + +[project] +name = "horizon" +version = "0.4.5" +description = "" +authors = [{name = "Francesco Ruscelli", email = "francesco.ruscelli@iit.it"}] +readme = "README.md" +license = {file = "LICENSE.txt"} \ No newline at end of file diff --git a/setup.cfg b/setup.cfg.old similarity index 100% rename from setup.cfg rename to setup.cfg.old diff --git a/setup.py b/setup.py.old similarity index 56% rename from setup.py rename to setup.py.old index 83a17dbb..4595b8f3 100644 --- a/setup.py +++ b/setup.py.old @@ -1,8 +1,6 @@ import setuptools - from setuptools.command.develop import develop from setuptools.command.build_py import build_py - import os import codecs import subprocess @@ -21,40 +19,56 @@ def get_version(rel_path): raise RuntimeError("Unable to find version string.") def _pre_build(dirname): - # create a build dir and run 'make generate_python_package' current_dir = os.getcwd() print('building from dir: ', current_dir) os.makedirs(dirname, exist_ok=True) build_dir = current_dir + '/' + dirname try: - p = subprocess.run(["cmake", "-DCMAKE_BUILD_TYPE=Release", "../horizon/cpp"], cwd=build_dir) - except subprocess.CalledProcessError: - raise + subprocess.run(["cmake", "-DCMAKE_BUILD_TYPE=Release", "../horizon/cpp"], cwd=build_dir, check=True) + subprocess.run(["make", "-j8"], cwd=build_dir, check=True) + subprocess.run(["make", "generate_python_package", "-j8"], cwd=build_dir, check=True) + except subprocess.CalledProcessError as e: + raise RuntimeError("Build failed") from e - try: - p = subprocess.run(["make", "-j8"], cwd=build_dir) - except subprocess.CalledProcessError: - raise +class CustomBuild(build_py): + user_options = build_py.user_options + [ + ('skip-build', None, 'Skip custom build steps') + ] - try: - p = subprocess.run(["make", "generate_python_package", "-j8"], cwd=build_dir) - except subprocess.CalledProcessError: - raise + def initialize_options(self): + build_py.initialize_options(self) + self.skip_build = False + + def finalize_options(self): + build_py.finalize_options(self) -class CustomBuild(build_py): - # called by pip install and by python setup.py build and python setup.py install - # build_py is not called by pip install -e def run(self): - dir_name = 'temp_build' - _pre_build(dir_name) + if not self.skip_build: + dir_name = 'temp_build' + _pre_build(dir_name) + else: + print("Skipping custom build steps") build_py.run(self) class CustomDevelop(develop): - # called by pip install -e + user_options = develop.user_options + [ + ('skip-build', None, 'Skip custom build steps') + ] + + def initialize_options(self): + develop.initialize_options(self) + self.skip_build = False + + def finalize_options(self): + develop.finalize_options(self) + def run(self): - dir_name = 'temp_build' - _pre_build(dir_name) + if not self.skip_build: + dir_name = 'temp_build' + _pre_build(dir_name) + else: + print("Skipping custom build steps") develop.run(self) setuptools.setup( @@ -68,8 +82,7 @@ def run(self): packages=['horizon', 'horizon.utils', 'horizon.solvers', 'horizon.transcriptions', 'horizon.examples', 'horizon.ros', 'horizon.rhc', 'horizon.rhc.ros', 'horizon.rhc.tasks'], install_requires=['numpy', 'matplotlib', 'scipy', 'casadi-kin-dyn', 'rospkg'], python_requires=">=3.6", - cmdclass={'build_py': CustomBuild, - 'develop': CustomDevelop}, + cmdclass={'build_py': CustomBuild, 'develop': CustomDevelop}, ext_modules=[ setuptools.Extension( name="ilqrext", sources=[] @@ -79,3 +92,4 @@ def run(self): ) ] ) +