From 60608b41f8934a8f8c4ef768ee53f5baeea25f3f Mon Sep 17 00:00:00 2001 From: "Joris Gillis; u0052373" Date: Wed, 26 Oct 2016 15:28:31 +0200 Subject: [PATCH] issue #1865: hpmpc interface --- .gitmodules | 6 + .travis.yml | 2 +- CMakeLists.txt | 34 + casadi/CMakeLists.txt | 3 + casadi/core/CMakeLists.txt | 2 - casadi/core/casadi_types.hpp | 2 + casadi/core/function/conic.cpp | 42 + casadi/core/function/conic_impl.hpp | 10 + casadi/core/function/function_internal.cpp | 69 +- casadi/core/function/function_internal.hpp | 6 + casadi/core/function/nlpsol.cpp | 5 - casadi/core/function/nlpsol_impl.hpp | 3 - casadi/core/function/oracle_function.cpp | 58 -- casadi/core/function/plugin_interface.hpp | 70 +- casadi/core/runtime/runtime.hpp | 61 ++ casadi/interfaces/CMakeLists.txt | 4 + casadi/interfaces/hpmpc/CMakeLists.txt | 19 + casadi/interfaces/hpmpc/hpmpc_interface.cpp | 755 ++++++++++++++++++ casadi/interfaces/hpmpc/hpmpc_interface.hpp | 216 +++++ .../interfaces/hpmpc/hpmpc_interface_meta.cpp | 30 + cmake/FindBLASFEO.cmake | 31 + cmake/FindHPMPC.cmake | 34 + external_packages/CMakeLists.txt | 8 + external_packages/blasfeo/CMakeLists.txt | 143 ++++ external_packages/blasfeo/make2cmake.py | 59 ++ external_packages/blasfeo/src | 1 + external_packages/hpmpc/CMakeLists.txt | 246 ++++++ .../hpmpc/dummy_include/blasfeo_target.h | 1 + .../hpmpc/dummy_include/foo/bar/dummy.txt | 1 + .../hpmpc/dummy_include/include/target.h | 1 + external_packages/hpmpc/make2cmake.py | 79 ++ external_packages/hpmpc/src | 1 + test/python/conic.py | 220 +++++ 33 files changed, 2134 insertions(+), 88 deletions(-) create mode 100644 .gitmodules create mode 100644 casadi/interfaces/hpmpc/CMakeLists.txt create mode 100644 casadi/interfaces/hpmpc/hpmpc_interface.cpp create mode 100644 casadi/interfaces/hpmpc/hpmpc_interface.hpp create mode 100644 casadi/interfaces/hpmpc/hpmpc_interface_meta.cpp create mode 100644 cmake/FindBLASFEO.cmake create mode 100644 cmake/FindHPMPC.cmake create mode 100644 external_packages/blasfeo/CMakeLists.txt create mode 100644 external_packages/blasfeo/make2cmake.py create mode 160000 external_packages/blasfeo/src create mode 100644 external_packages/hpmpc/CMakeLists.txt create mode 100644 external_packages/hpmpc/dummy_include/blasfeo_target.h create mode 100644 external_packages/hpmpc/dummy_include/foo/bar/dummy.txt create mode 100644 external_packages/hpmpc/dummy_include/include/target.h create mode 100644 external_packages/hpmpc/make2cmake.py create mode 160000 external_packages/hpmpc/src diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000..196a7d1d47 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "external_packages/hpmpc/src"] + path = external_packages/hpmpc/src + url = https://github.com/jgillis/hpmpc.git +[submodule "external_packages/blasfeo/src"] + path = external_packages/blasfeo/src + url = https://github.com/giaf/blasfeo.git diff --git a/.travis.yml b/.travis.yml index 68c9a4f11d..8af929ce22 100644 --- a/.travis.yml +++ b/.travis.yml @@ -240,7 +240,7 @@ matrix: - mkdir build - pushd build - cmake -DOLD_LLVM=ON -DWITH_ECOS=ON -DWITH_MOSEK=ON -DWITH_WORHP=ON -DWITH_SLICOT=OFF -DWITH_PYTHON=ON -DWITH_JSON=ON .. - - make + - make VERBOSE=1 - sudo make install - popd - python -c "from casadi.tools import *;loadAllCompiledPlugins()" diff --git a/CMakeLists.txt b/CMakeLists.txt index 0265b19964..0a9bd204c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,10 @@ option(WITH_NO_QPOASES_BANNER "Add -D__NO_COPYRIGHT__ to qpOASES definitions" OF option(WITH_BLOCKSQP "Compile the interface to blockSQP (the source code for blockSQP is included)" ON) option(WITH_BUILD_DSDP "Compile the the included source code for DSDP" ON) option(WITH_DSDP "Compile the interface to DSDP" ON) +option(WITH_BUILD_HPMPC "Compile the the included source code for HPMPC" OFF) +option(WITH_HPMPC "Compile the interface to HPMPC" OFF) +option(WITH_BUILD_BLASFEO "Compile the the included source code for BLASFEO" OFF) +option(WITH_BLASFEO "Compile the interface to BLASFEO" OFF) option(WITH_MOSEK "Compile the interface to MOSEK" OFF) option(WITH_ECOS "Compile the interface to ECOS" OFF) option(WITH_BUILD_CSPARSE "Compile the included source code for CSparse" ON) @@ -421,6 +425,36 @@ if(WITH_CSPARSE) endif() add_feature_info(csparse-interface CSPARSE_FOUND "Interface to the sparse direct linear solver CSparse.") +# BLASFEO +if(WITH_BLASFEO) + if(WITH_BUILD_BLASFEO) + set(BLASFEO_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/external_packages/blasfeo/src/include) + set(BLASFEO_LIBRARIES) + set(BLASFEO_DLOPEN ON) + set(BLASFEO_FOUND ON) + else() + # try to find system blasfeo + find_package(BLASFEO) + set(BLASFEO_DLOPEN OFF) + endif() +endif() +add_feature_info(blasfeo-interface BLASFEO_FOUND "Interface to blasfeo.") + +# HPMPC +if(WITH_HPMPC AND BLASFEO_FOUND) + if(WITH_BUILD_HPMPC) + set(HPMPC_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/external_packages/hpmpc/src/include) + set(HPMPC_LIBRARIES) + set(HPMPC_DLOPEN ON) + set(HPMPC_FOUND ON) + else() + # try to find system hpmpc + find_package(HPMPC) + set(HPMPC_DLOPEN OFF) + endif() +endif() +add_feature_info(hpmpc-interface HPMPC_FOUND "Interface to hpmpc.") + # TinyXML if(WITH_TINYXML) if(WITH_BUILD_TINYXML) diff --git a/casadi/CMakeLists.txt b/casadi/CMakeLists.txt index e67837a471..c5edd8ced4 100644 --- a/casadi/CMakeLists.txt +++ b/casadi/CMakeLists.txt @@ -86,6 +86,9 @@ macro(casadi_library_or_plugin name is_plugin) set_property(GLOBAL APPEND PROPERTY CASADI_MODULES ${name}) endmacro() +add_definitions(-DSHARED_LIBRARY_PREFIX="\\"${CMAKE_SHARED_LIBRARY_PREFIX}\\"") +add_definitions(-DSHARED_LIBRARY_SUFFIX="\\"${CMAKE_SHARED_LIBRARY_SUFFIX}\\"") + add_subdirectory(core) # needed by all except external_packages add_subdirectory(interfaces) # needs external_packages add_subdirectory(solvers) # needs interfaces diff --git a/casadi/core/CMakeLists.txt b/casadi/core/CMakeLists.txt index 48044d2e23..118afbf580 100644 --- a/casadi/core/CMakeLists.txt +++ b/casadi/core/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 2.8.6) # Make the shared library prefix and suffix available in C++ -add_definitions(-DSHARED_LIBRARY_PREFIX="${CMAKE_SHARED_LIBRARY_PREFIX}") -add_definitions(-DSHARED_LIBRARY_SUFFIX="${CMAKE_SHARED_LIBRARY_SUFFIX}") add_subdirectory(runtime) diff --git a/casadi/core/casadi_types.hpp b/casadi/core/casadi_types.hpp index 357c77e938..9eca90d63b 100644 --- a/casadi/core/casadi_types.hpp +++ b/casadi/core/casadi_types.hpp @@ -388,6 +388,8 @@ namespace casadi { CONIC_X0, /// dense CONIC_LAM_X0, + /// dense + CONIC_LAM_A0, CONIC_NUM_IN}; /// Output arguments of an QP Solver diff --git a/casadi/core/function/conic.cpp b/casadi/core/function/conic.cpp index 4099d55288..fe0171d5da 100644 --- a/casadi/core/function/conic.cpp +++ b/casadi/core/function/conic.cpp @@ -86,6 +86,7 @@ namespace casadi { case CONIC_UBX: return "ubx"; case CONIC_X0: return "x0"; case CONIC_LAM_X0: return "lam_x0"; + case CONIC_LAM_A0: return "lam_a0"; case CONIC_NUM_IN: break; } return string(); @@ -183,6 +184,7 @@ namespace casadi { w[CONIC_UBA] = ret_in[NLPSOL_UBG] - v.at(3); w[CONIC_X0] = ret_in[NLPSOL_X0]; w[CONIC_LAM_X0] = ret_in[NLPSOL_LAM_X0]; + w[CONIC_LAM_A0] = ret_in[NLPSOL_LAM_G0]; w = conic_f(w); // Get expressions for the solution @@ -256,6 +258,7 @@ namespace casadi { return get_sparsity_out(CONIC_X); case CONIC_LBA: case CONIC_UBA: + case CONIC_LAM_A0: return get_sparsity_out(CONIC_LAM_A); case CONIC_A: return A_; @@ -352,4 +355,43 @@ namespace casadi { } } + void Conic::print_fstats(const ConicMemory* m) const { + + size_t maxNameLen=0; + + // Retrieve all qp keys + std::vector keys; + for (auto &&s : m->fstats) { + maxNameLen = max(s.first.size(), maxNameLen); + keys.push_back(s.first); + } + + // Print header + std::stringstream s; + std::string blankName(maxNameLen, ' '); + s + << blankName + << " proc wall num mean mean" + << endl << blankName + << " time time evals proc time wall time"; + userOut() << s.str() << endl; + + std::sort(keys.begin(), keys.end()); + for (auto k : keys) { + const FStats& fs = m->fstats.at(k); + print_stats_line(maxNameLen, k, fs.n_call, fs.t_proc, fs.t_wall); + } + + // Sum the previously printed stats + double t_wall_all_previous = 0; + double t_proc_all_previous = 0; + for (auto k : keys) { + const FStats& fs = m->fstats.at(k); + t_proc_all_previous += fs.t_proc; + t_wall_all_previous += fs.t_wall; + } + print_stats_line(maxNameLen, "all previous", -1, t_proc_all_previous, t_wall_all_previous); + + } + } // namespace casadi diff --git a/casadi/core/function/conic_impl.hpp b/casadi/core/function/conic_impl.hpp index 0c490230e4..8fce517668 100644 --- a/casadi/core/function/conic_impl.hpp +++ b/casadi/core/function/conic_impl.hpp @@ -29,9 +29,16 @@ #include "conic.hpp" #include "function_internal.hpp" #include "plugin_interface.hpp" +#include "../timing.hpp" /// \cond INTERNAL namespace casadi { + + struct CASADI_EXPORT ConicMemory { + // Function specific statistics + std::map fstats; + }; + /// Internal class class CASADI_EXPORT Conic : public FunctionInternal, public PluginInterface { public: @@ -98,6 +105,9 @@ namespace casadi { /// Can discrete variables be treated virtual bool integer_support() const { return false;} + /// Print statistics + void print_fstats(const ConicMemory* m) const; + protected: /// Options std::vector discrete_; diff --git a/casadi/core/function/function_internal.cpp b/casadi/core/function/function_internal.cpp index 531453b227..612fd066dd 100644 --- a/casadi/core/function/function_internal.cpp +++ b/casadi/core/function/function_internal.cpp @@ -35,6 +35,7 @@ #include #include #endif // WITH_DL +#include using namespace std; @@ -179,11 +180,16 @@ namespace casadi { {"max_num_dir", {OT_INT, "Specify the maximum number of directions for derivative functions." - " Overrules the builtin optimized_num_dir."}} + " Overrules the builtin optimized_num_dir."}}, + {"print_time", + {OT_BOOL, + "print information about execution time"}} } }; void FunctionInternal::init(const Dict& opts) { + + print_time_ = true; // Read options for (auto&& op : opts) { if (op.first=="verbose") { @@ -216,6 +222,8 @@ namespace casadi { ad_weight_sp_ = op.second; } else if (op.first=="max_num_dir") { max_num_dir_ = op.second; + } else if (op.first=="print_time") { + print_time_ = op.second; } } @@ -3175,4 +3183,63 @@ namespace casadi { return singleton; } + // Convert a float to a string of an exact length. + // First it tries fixed precision, then falls back to exponential notation. + // + // todo(jaeandersson,jgillis): needs either review or unit tests + // because it throws exceptions if it fail. + std::string formatFloat(double x, int totalWidth, int maxPrecision, int fallbackPrecision) { + std::ostringstream out0; + out0 << fixed << setw(totalWidth) << setprecision(maxPrecision) << x; + std::string ret0 = out0.str(); + if (ret0.length() == totalWidth) { + return ret0; + } else if (ret0.length() > totalWidth) { + std::ostringstream out1; + out1 << setw(totalWidth) << setprecision(fallbackPrecision) << x; + std::string ret1 = out1.str(); + if (ret1.length() != totalWidth) + casadi_error( + "ipopt timing formatting fallback is bugged, sorry about that." + << "expected " << totalWidth << " digits, but got " << ret1.length() + << ", string: \"" << ret1 << "\", number: " << x); + return ret1; + } else { + casadi_error("ipopt timing formatting is bugged, sorry about that."); + } + } + + void FunctionInternal::print_stats_line(int maxNameLen, std::string label, + double n_call, double t_proc, double t_wall) { + // Skip when not called + if (n_call == 0) return; + + std::stringstream s; + + s + << setw(maxNameLen) << label << " " + << formatFloat(t_proc, 9, 3, 3) << " [s] " + << formatFloat(t_wall, 9, 3, 3) << " [s]"; + if (n_call == -1) { + // things like main loop don't have # evals + s << endl; + } else { + s + << " " + << setw(5) << n_call; + if (n_call < 2) { + s << endl; + } else { + // only print averages if there is more than 1 eval + s + << " " + << formatFloat(1000.0*t_proc/n_call, 10, 2, 3) << " [ms] " + << formatFloat(1000.0*t_wall/n_call, 10, 2, 3) << " [ms]" + << endl; + } + } + userOut() << s.str(); + } + + } // namespace casadi diff --git a/casadi/core/function/function_internal.hpp b/casadi/core/function/function_internal.hpp index af1523fea2..e0c90cc837 100644 --- a/casadi/core/function/function_internal.hpp +++ b/casadi/core/function/function_internal.hpp @@ -844,6 +844,9 @@ namespace casadi { /// Errors are thrown if numerical values of inputs look bad bool inputs_check_; + // Print timing statistics + bool print_time_; + /** \brief Get type name */ virtual std::string type_name() const; @@ -862,6 +865,9 @@ namespace casadi { template std::vector > symbolicAdjSeed(int nadj, const std::vector& v); + protected: + static void print_stats_line(int maxNameLen, std::string label, double n_call, + double t_proc, double t_wall); private: /// Memory objects std::vector mem_; diff --git a/casadi/core/function/nlpsol.cpp b/casadi/core/function/nlpsol.cpp index 5ac5dab6e8..10525fc71c 100644 --- a/casadi/core/function/nlpsol.cpp +++ b/casadi/core/function/nlpsol.cpp @@ -228,9 +228,6 @@ namespace casadi { {OT_BOOL, "When errors occur during evaluation of f,g,...," "stop the iterations"}}, - {"print_time", - {OT_BOOL, - "print information about execution time"}}, {"verbose_init", {OT_BOOL, "Print out timing information about " @@ -262,8 +259,6 @@ namespace casadi { warn_initial_bounds_ = op.second; } else if (op.first=="iteration_callback_ignore_errors") { iteration_callback_ignore_errors_ = op.second; - } else if (op.first=="print_time") { - print_time_ = op.second; } else if (op.first=="discrete") { discrete_ = op.second; } diff --git a/casadi/core/function/nlpsol_impl.hpp b/casadi/core/function/nlpsol_impl.hpp index 6296aae2b4..d12a991e23 100644 --- a/casadi/core/function/nlpsol_impl.hpp +++ b/casadi/core/function/nlpsol_impl.hpp @@ -79,9 +79,6 @@ namespace casadi { // Ignore errors in the iteration callbacks bool iteration_callback_ignore_errors_; - // Print timing statistics - bool print_time_; - /// Which variables are discrete? std::vector discrete_; diff --git a/casadi/core/function/oracle_function.cpp b/casadi/core/function/oracle_function.cpp index fe8d020690..1311d07be6 100644 --- a/casadi/core/function/oracle_function.cpp +++ b/casadi/core/function/oracle_function.cpp @@ -262,64 +262,6 @@ namespace casadi { oracle_ = oracle_.expand(); } - // Convert a float to a string of an exact length. - // First it tries fixed precision, then falls back to exponential notation. - // - // todo(jaeandersson,jgillis): needs either review or unit tests - // because it throws exceptions if it fail. - std::string formatFloat(double x, int totalWidth, int maxPrecision, int fallbackPrecision) { - std::ostringstream out0; - out0 << fixed << setw(totalWidth) << setprecision(maxPrecision) << x; - std::string ret0 = out0.str(); - if (ret0.length() == totalWidth) { - return ret0; - } else if (ret0.length() > totalWidth) { - std::ostringstream out1; - out1 << setw(totalWidth) << setprecision(fallbackPrecision) << x; - std::string ret1 = out1.str(); - if (ret1.length() != totalWidth) - casadi_error( - "ipopt timing formatting fallback is bugged, sorry about that." - << "expected " << totalWidth << " digits, but got " << ret1.length() - << ", string: \"" << ret1 << "\", number: " << x); - return ret1; - } else { - casadi_error("ipopt timing formatting is bugged, sorry about that."); - } - } - - void print_stats_line(int maxNameLen, std::string label, - double n_call, double t_proc, double t_wall) { - // Skip when not called - if (n_call == 0) return; - - std::stringstream s; - - s - << setw(maxNameLen) << label << " " - << formatFloat(t_proc, 9, 3, 3) << " [s] " - << formatFloat(t_wall, 9, 3, 3) << " [s]"; - if (n_call == -1) { - // things like main loop don't have # evals - s << endl; - } else { - s - << " " - << setw(5) << n_call; - if (n_call < 2) { - s << endl; - } else { - // only print averages if there is more than 1 eval - s - << " " - << formatFloat(1000.0*t_proc/n_call, 10, 2, 3) << " [ms] " - << formatFloat(1000.0*t_wall/n_call, 10, 2, 3) << " [ms]" - << endl; - } - } - userOut() << s.str(); - } - void OracleFunction::print_fstats(const OracleMemory* m) const { size_t maxNameLen=0; diff --git a/casadi/core/function/plugin_interface.hpp b/casadi/core/function/plugin_interface.hpp index 9545e345af..4e39148757 100644 --- a/casadi/core/function/plugin_interface.hpp +++ b/casadi/core/function/plugin_interface.hpp @@ -54,6 +54,12 @@ #define SHARED_LIBRARY_SUFFIX ".so" #endif // SHARED_LIBRARY_SUFFIX +#ifdef _WIN32 + #define DL_HANDLE_TYPE HINSTANCE +#else // _WIN32 + #define DL_HANDLE_TYPE void * +#endif + #endif // WITH_DL namespace casadi { @@ -93,6 +99,10 @@ namespace casadi { /// Load a plugin dynamically static Plugin load_plugin(const std::string& pname, bool register_plugin=true); + /// Load a library dynamically + static DL_HANDLE_TYPE load_library(const std::string& libname, std::string& resultpath, + bool global); + /// Register an integrator in the factory static void registerPlugin(const Plugin& plugin); @@ -143,28 +153,17 @@ namespace casadi { return plugin; } - template - typename PluginInterface::Plugin - PluginInterface::load_plugin(const std::string& pname, bool register_plugin) { - // Issue warning and quick return if already loaded - if (Derived::solvers_.find(pname) != Derived::solvers_.end()) { - casadi_warning("PluginInterface: Solver " + pname + " is already in use. Ignored."); - return Plugin(); - } + template + DL_HANDLE_TYPE PluginInterface::load_library(const std::string& libname, + std::string& resultpath, bool global) { #ifndef WITH_DL casadi_error("WITH_DL option needed for dynamic loading"); #else // WITH_DL - // Retrieve the registration function - RegFcn reg; // Get the name of the shared library - std::string lib = SHARED_LIBRARY_PREFIX "casadi_" - + Derived::infix_ + "_" + pname + SHARED_LIBRARY_SUFFIX; - - // Load the dll - std::string regName = "casadi_register_" + Derived::infix_ + "_" + pname; + std::string lib = SHARED_LIBRARY_PREFIX + libname + SHARED_LIBRARY_SUFFIX; // Build up search paths; std::vector search_paths; @@ -232,7 +231,12 @@ namespace casadi { // Alocate a handle pointer #ifndef _WIN32 - int flag = RTLD_LAZY | RTLD_LOCAL; + int flag; + if (global) { + flag = RTLD_NOW | RTLD_GLOBAL; + } else { + flag = RTLD_LAZY | RTLD_LOCAL; + } #ifdef WITH_DEEPBIND flag |= RTLD_DEEPBIND; #endif @@ -248,8 +252,8 @@ namespace casadi { handle = LoadLibrary(TEXT(lib.c_str())); SetDllDirectory(NULL); #else // _WIN32 - std::string pname = searchpath.size()==0 ? lib : searchpath + filesep + lib; - handle = dlopen(pname.c_str(), flag); + std::string libname = searchpath.size()==0 ? lib : searchpath + filesep + lib; + handle = dlopen(libname.c_str(), flag); #endif // _WIN32 if (handle) { break; @@ -263,8 +267,38 @@ namespace casadi { } } + resultpath = searchpath; + casadi_assert_message(handle!=0, errors.str()); + return handle; + +#endif // WITH_DL + } + + template + typename PluginInterface::Plugin + PluginInterface::load_plugin(const std::string& pname, bool register_plugin) { + // Issue warning and quick return if already loaded + if (Derived::solvers_.find(pname) != Derived::solvers_.end()) { + casadi_warning("PluginInterface: Solver " + pname + " is already in use. Ignored."); + return Plugin(); + } + + +#ifndef WITH_DL + casadi_error("WITH_DL option needed for dynamic loading"); +#else // WITH_DL + // Retrieve the registration function + RegFcn reg; + + // Load the dll + std::string regName = "casadi_register_" + Derived::infix_ + "_" + pname; + + std::string searchpath; + DL_HANDLE_TYPE handle = load_library("casadi_" + Derived::infix_ + "_" + pname, searchpath, + false); + #ifdef _WIN32 reg = (RegFcn)GetProcAddress(handle, TEXT(regName.c_str())); #else // _WIN32 diff --git a/casadi/core/runtime/runtime.hpp b/casadi/core/runtime/runtime.hpp index 8617ec7a57..8603a8e05a 100644 --- a/casadi/core/runtime/runtime.hpp +++ b/casadi/core/runtime/runtime.hpp @@ -44,6 +44,18 @@ namespace casadi { template void CASADI_PREFIX(project)(const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w); + /// Sparse copy: y <- x, w work vector (length >= number of rows) + template + void CASADI_PREFIX(mproject)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w); + + /// Sparse copy: y <- x, w work vector (length >= number of rows) + template + void CASADI_PREFIX(maddproject)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w); + + /// Dense transfer: y(y_sp).nonzeros() <- x(x_sp).nonzeros() (length >= max(number of rows, nnz)) + template + void CASADI_PREFIX(dense_transfer)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w); + /// Convert sparse to dense template void CASADI_PREFIX(densify)(const real1_t* x, const int* sp_x, real2_t* y, int tr); @@ -217,6 +229,55 @@ namespace casadi { } } + template + void CASADI_PREFIX(mproject)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w) { + int ncol_x = sp_x[1]; + const int *colind_x = sp_x+2, *row_x = sp_x + 2 + ncol_x+1; + int ncol_y = sp_y[1]; + const int *colind_y = sp_y+2, *row_y = sp_y + 2 + ncol_y+1; + /* Loop over columns of x and y */ + int i, el; + for (i=0; i + void CASADI_PREFIX(maddproject)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w) { + int ncol_x = sp_x[1]; + const int *colind_x = sp_x+2, *row_x = sp_x + 2 + ncol_x+1; + int ncol_y = sp_y[1]; + const int *colind_y = sp_y+2, *row_y = sp_y + 2 + ncol_y+1; + /* Loop over columns of x and y */ + int i, el; + for (i=0; i + void CASADI_PREFIX(dense_transfer)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w) { + CASADI_PREFIX(sparsify)(x, w, sp_x, false); + int nrow_y = sp_y[0]; + int ncol_y = sp_y[1]; + const int *colind_y = sp_y+2, *row_y = sp_y + 2 + ncol_y+1; + /* Loop over columns of y */ + int i, el; + for (i=0; i void CASADI_PREFIX(densify)(const real1_t* x, const int* sp_x, real2_t* y, int tr) { /* Quick return - output ignored */ diff --git a/casadi/interfaces/CMakeLists.txt b/casadi/interfaces/CMakeLists.txt index 324f79031f..0e77c2f67d 100644 --- a/casadi/interfaces/CMakeLists.txt +++ b/casadi/interfaces/CMakeLists.txt @@ -70,6 +70,10 @@ if(BLOCKSQP_FOUND) add_subdirectory(blocksqp) endif() +if(HPMPC_FOUND) + add_subdirectory(hpmpc) +endif() + set(LINT_TARGETS ${LINT_TARGETS} PARENT_SCOPE) set(SPELL_TARGETS ${SPELL_TARGETS} PARENT_SCOPE) set(CASADI_MODULES ${CASADI_MODULES} PARENT_SCOPE) diff --git a/casadi/interfaces/hpmpc/CMakeLists.txt b/casadi/interfaces/hpmpc/CMakeLists.txt new file mode 100644 index 0000000000..865c50b452 --- /dev/null +++ b/casadi/interfaces/hpmpc/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.6) +include_directories(${HPMPC_INCLUDE_DIR}) + +casadi_plugin(Conic hpmpc + hpmpc_interface.hpp + hpmpc_interface.cpp + hpmpc_interface_meta.cpp) + +casadi_plugin_link_libraries(Conic hpmpc ${HPMPC_LIBRARIES} ${BLASFEO_LIBRARIES}) + +if(HPMPC_DLOPEN) +add_definitions(-DHPMPC_DLOPEN) +endif() + +if(BLASFEO_DLOPEN) +add_definitions(-DBLASFEO_DLOPEN) +endif() + +casadi_plugin_link_libraries(Conic hpmpc ${HPMPC_LIBRARIES} ${BLASFEO_LIBRARIES}) diff --git a/casadi/interfaces/hpmpc/hpmpc_interface.cpp b/casadi/interfaces/hpmpc/hpmpc_interface.cpp new file mode 100644 index 0000000000..144e25a7da --- /dev/null +++ b/casadi/interfaces/hpmpc/hpmpc_interface.cpp @@ -0,0 +1,755 @@ +/* + * This file is part of CasADi. + * + * CasADi -- A symbolic framework for dynamic optimization. + * Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, + * K.U. Leuven. All rights reserved. + * Copyright (C) 2011-2014 Greg Horn + * + * CasADi is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * CasADi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with CasADi; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include "hpmpc_interface.hpp" +#include + +using namespace std; +namespace casadi { + + extern "C" + int CASADI_CONIC_HPMPC_EXPORT + casadi_register_conic_hpmpc(Conic::Plugin* plugin) { + plugin->creator = HpmpcInterface::creator; + plugin->name = "hpmpc"; + plugin->doc = HpmpcInterface::meta_doc.c_str(); + plugin->version = 30; + return 0; + } + + extern "C" + void CASADI_CONIC_HPMPC_EXPORT casadi_load_conic_hpmpc() { + Conic::registerPlugin(casadi_register_conic_hpmpc); + } + + HpmpcInterface::HpmpcInterface(const std::string& name, + const std::map& st) + : Conic(name, st) { + } + + HpmpcInterface::~HpmpcInterface() { + clear_memory(); + } + + Options HpmpcInterface::options_ + = {{&Conic::options_}, + {{"N", + {OT_INT, + "OCP horizon"}}, + {"nx", + {OT_INTVECTOR, + "Number of states, length N+1"}}, + {"nu", + {OT_INTVECTOR, + "Number of controls, length N"}}, + {"ng", + {OT_INTVECTOR, + "Number of non-dynamic constraints, length N+1"}}, + {"mu0", + {OT_DOUBLE, + "Max element in cost function as estimate of max multiplier"}}, + {"max_iter", + {OT_INT, + "Max number of iterations"}}, + {"tol", + {OT_DOUBLE, + "Tolerance in the duality measure"}}, + {"warm_start", + {OT_BOOL, + "Use warm-starting"}}, + {"inf", + {OT_DOUBLE, + "HPMPC cannot handle infinities. Infinities will be replaced by this option's value."}}, + {"target", + {OT_STRING, + "hpmpc target"}}, + {"blasfeo_target", + {OT_STRING, + "hpmpc target"}}} + }; + + void HpmpcInterface::init(const Dict& opts) { + Conic::init(opts); + + // Default options + mu0_ = 1; + max_iter_ = 1000; + print_level_ = 1; + tol_ = 1e-8; + warm_start_ = false; + inf_ = 1e6; + target_ = "X64_AVX"; + blasfeo_target_ = "X64_INTEL_SANDY_BRIDGE"; + int struct_cnt=0; + // Read options + for (auto&& op : opts) { + if (op.first=="N") { + N_ = op.second; + struct_cnt++; + } else if (op.first=="nx") { + nxs_ = op.second; + struct_cnt++; + } else if (op.first=="nu") { + nus_ = op.second; + struct_cnt++; + } else if (op.first=="ng") { + ngs_ = op.second; + struct_cnt++; + } else if (op.first=="mu0") { + mu0_ = op.second; + } else if (op.first=="max_iter") { + max_iter_ = op.second; + } else if (op.first=="tol") { + tol_ = op.second; + } else if (op.first=="warm_start") { + warm_start_ = op.second; + } else if (op.first=="target") { + target_ = static_cast(op.second); + } else if (op.first=="blasfeo_target") { + blasfeo_target_ = static_cast(op.second); + } else if (op.first=="print_level") { + print_level_ = op.second; + } else if (op.first=="inf") { + inf_ = op.second; + } + } + + bool detect_structure = struct_cnt==0; + casadi_assert_message(struct_cnt==0 || struct_cnt==4, + "You must either set all of N, nx, nu, ng; " + "or set none at all (automatic detection)."); + + const std::vector& nx = nxs_; + const std::vector& ng = ngs_; + const std::vector& nu = nus_; + + if (detect_structure) { + /* General strategy: look for the xk+1 diagonal part in A + */ + + // Find the right-most column for each row in A -> A_skyline + // Find the second-to-right-most column -> A_skyline2 + // Find the left-most column -> A_bottomline + Sparsity AT = sparsity_in(CONIC_A).T(); + std::vector A_skyline; + std::vector A_skyline2; + std::vector A_bottomline; + for (int i=0;iAT.colind()[i]) { + A_skyline.push_back(AT.row()[pivot-1]); + if (pivot>AT.colind()[i]+1) { + A_skyline2.push_back(AT.row()[pivot-2]); + } else { + A_skyline2.push_back(-1); + } + } else { + A_skyline.push_back(-1); + A_skyline2.push_back(-1); + } + } + + /* + Loop over the right-most columns of A: + they form the diagonal part due to xk+1 in gap constraints. + detect when the diagonal pattern is broken -> new stage + */ + int pivot = 0; // Current right-most element + int start_pivot = pivot; // First right-most element that started the stage + int cg = 0; // Counter for non-gap-closing constraints + for (int i=0;ipivot+1) { // Jump to a diagonal in the future + nus_.push_back(A_skyline[i]-pivot-1); // Size of jump equals number of states + commit = true; + } else if (A_skyline[i]==pivot+1) { // Walking the diagonal + if (A_skyline2[i]=0;--i) { + if (A_bottomline[i] theirs_u_blocks, theirs_x_blocks; + offset = 0; + + for (int k=0;k lamg_gap_blocks; + for (int k=0;k(mem); + + m->nx = nxs_; + m->nu = nus_; m->nu.push_back(0); + m->ng = ngs_; + + const std::vector& nx = m->nx; + const std::vector& nu = m->nu; + const std::vector& ng = m->ng; + const std::vector& nb = m->nb; + + m->nb.resize(N_+1); + int offset = 0; + for (int k=0;knb[k] = nx[k]+nu[k]; + m->nb[N_] = nx[N_]; + + m->A.resize(Asp_.nnz()); + m->B.resize(Bsp_.nnz()); + m->C.resize(Csp_.nnz()); + m->D.resize(Dsp_.nnz()); + m->R.resize(Rsp_.nnz()); + m->I.resize(Isp_.nnz()); + m->S.resize(Ssp_.nnz()); + m->Q.resize(Qsp_.nnz()); + m->b.resize(bsp_.nnz()); + m->b2.resize(bsp_.nnz()); + m->x.resize(xsp_.nnz());m->q.resize(xsp_.nnz()); + m->u.resize(usp_.nnz());m->r.resize(usp_.nnz()); + m->lg.resize(lugsp_.nnz()); + m->ug.resize(lugsp_.nnz()); + m->lb.resize(nx_);m->ub.resize(nx_); + m->pi.resize(pisp_.nnz()); + + offset = 0; + for (int k=0;knx[k]+nu[k]; + m->hidxb.resize(offset); + + offset = 0; + for (int k=0;klam.resize(2*offset); + + // Allocate double* work vectors + blockptr(m->As, m->A, A_blocks); + blockptr(m->Bs, m->B, B_blocks); + blockptr(m->Cs, m->C, C_blocks); + blockptr(m->Ds, m->D, D_blocks); + blockptr(m->Is, m->I, I_blocks, true); + blockptr(m->Rs, m->R, R_blocks); + blockptr(m->Qs, m->Q, Q_blocks); + blockptr(m->Ss, m->S, S_blocks); + blockptr(m->us, m->u, u_blocks); + blockptr(m->xs, m->x, x_blocks); + blockptr(m->rs, m->r, u_blocks); + blockptr(m->qs, m->q, x_blocks); + blockptr(m->lgs, m->lg, lug_blocks); + blockptr(m->ugs, m->ug, lug_blocks); + blockptr(m->bs, m->b, b_blocks); + + m->pis.resize(N_); + offset = 0; + for (int k=0;kpis[k] = get_ptr(m->pi)+offset; + offset+=nx[k+1]; + } + + m->lbs.resize(N_+1); + m->ubs.resize(N_+1); + offset = 0; + for (int k=0;klbs[k] = get_ptr(m->lb)+offset; + m->ubs[k] = get_ptr(m->ub)+offset; + offset+=nu[k]+nx[k]; + } + + m->lams.resize(N_+1); + offset = 0; + for (int k=0;klams[k] = get_ptr(m->lam)+offset; + offset+=2*(ng[k]+nb[k]); + } + + m->hidxbs.resize(N_+1); + offset = 0; + for (int k=0;khidxbs[k] = get_ptr(m->hidxb)+offset; + for (int i=0;inb[k];++i) m->hidxbs[k][i] = i; + offset+=nb[k]; + } + + // Allocate extra workspace as per HPMPC request + int workspace_size = hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes( + N_, get_ptr(m->nx), get_ptr(m->nu), get_ptr(m->nb), get_ptr(m->hidxbs), get_ptr(m->ng), N_); + m->workspace.resize(workspace_size); + m->stats.resize(max_iter_*5); + + m->pv.resize(2*(nx_+na_)); + + m->res.resize(4); + + m->fstats["preprocessing"] = FStats(); + m->fstats["solver"] = FStats(); + m->fstats["postprocessing"] = FStats(); + } + + void HpmpcInterface:: + eval(void* mem, const double** arg, double** res, int* iw, double* w) const { + auto m = static_cast(mem); + // Statistics + for (auto&& s : m->fstats) s.second.reset(); + m->fstats.at("preprocessing").tic(); + + double* pv = get_ptr(m->pv); + + // Dissect A matrix + casadi_project(arg[CONIC_A], sparsity_in(CONIC_A), get_ptr(m->A), Asp_, pv); + casadi_project(arg[CONIC_A], sparsity_in(CONIC_A), get_ptr(m->B), Bsp_, pv); + casadi_project(arg[CONIC_A], sparsity_in(CONIC_A), get_ptr(m->C), Csp_, pv); + casadi_project(arg[CONIC_A], sparsity_in(CONIC_A), get_ptr(m->D), Dsp_, pv); + casadi_project(arg[CONIC_A], sparsity_in(CONIC_A), get_ptr(m->I), Isp_, pv); + + // Dissect H matrix; definition of HPMPC lacks a factor 2 + casadi_mproject(0.5, arg[CONIC_H], sparsity_in(CONIC_H), get_ptr(m->R), Rsp_, pv); + casadi_mproject(0.5, arg[CONIC_H], sparsity_in(CONIC_H), get_ptr(m->S), Ssp_, pv); + casadi_mproject(0.5, arg[CONIC_H], sparsity_in(CONIC_H), get_ptr(m->Q), Qsp_, pv); + + // Dissect LBA/UBA + casadi_mproject(-1.0, arg[CONIC_LBA], sparsity_in(CONIC_LBA), get_ptr(m->b), bsp_, pv); + casadi_mproject(-1.0, arg[CONIC_UBA], sparsity_in(CONIC_UBA), get_ptr(m->b2), bsp_, pv); + casadi_assert(std::equal(m->b.begin(), m->b.end(), m->b2.begin())); + casadi_project(arg[CONIC_LBA], sparsity_in(CONIC_LBA), get_ptr(m->lg), lugsp_, pv); + casadi_project(arg[CONIC_UBA], sparsity_in(CONIC_UBA), get_ptr(m->ug), lugsp_, pv); + + // Dissect LBX/UBX input + std::fill(m->lb.begin(), m->lb.end(), 0); + std::fill(m->ub.begin(), m->ub.end(), 0); + + casadi_dense_transfer(1.0, arg[CONIC_LBX], xsp_, get_ptr(m->lb), theirs_xsp_, pv); + casadi_dense_transfer(1.0, arg[CONIC_UBX], xsp_, get_ptr(m->ub), theirs_xsp_, pv); + casadi_dense_transfer(1.0, arg[CONIC_LBX], usp_, get_ptr(m->lb), theirs_usp_, pv); + casadi_dense_transfer(1.0, arg[CONIC_UBX], usp_, get_ptr(m->ub), theirs_usp_, pv); + + // Dissect G + casadi_mproject(0.5, arg[CONIC_G], sparsity_in(CONIC_G), get_ptr(m->r), usp_, pv); + casadi_mproject(0.5, arg[CONIC_G], sparsity_in(CONIC_G), get_ptr(m->q), xsp_, pv); + + // Dissect X0 + casadi_project(arg[CONIC_X0], sparsity_in(CONIC_X0), get_ptr(m->u), usp_, pv); + casadi_project(arg[CONIC_X0], sparsity_in(CONIC_X0), get_ptr(m->x), xsp_, pv); + + m->iter_count = -1; + + int N = N_; + + // Deal with non-unity I block + for (int k=0;knx[k+1]; + for (int i=0;iIs[k][i]; + m->bs[k][i]*=f; + for (int j=0;jnx[k];++j) m->As[k][i+j*n_row]*=f; + for (int j=0;jnu[k];++j) m->Bs[k][i+j*n_row]*=f; + } + } + + // replace infinities + for (int i=0;ilb.size();++i) { + if (m->lb[i]==-std::numeric_limits::infinity()) m->lb[i] = -inf_; + } + for (int i=0;iub.size();++i) { + if (m->ub[i]==std::numeric_limits::infinity()) m->ub[i] = inf_; + } + for (int i=0;ilg.size();++i) { + if (m->lg[i]==-std::numeric_limits::infinity()) m->lg[i] = -inf_; + } + for (int i=0;iug.size();++i) { + if (m->ug[i]==std::numeric_limits::infinity()) m->ug[i] = inf_; + } + + m->fstats.at("preprocessing").toc(); + m->fstats.at("solver").tic(); + + casadi_dense_transfer(0.5, arg[CONIC_LAM_A0], lamg_gapsp_, get_ptr(m->pi), pisp_, pv); + // Deal with non-unity I block + for (int k=0;knx[k+1]; + for (int i=0;iIs[k][i]; + m->pis[k][i]*=f; + } + } + + casadi_dense_transfer(0.5, arg[CONIC_LAM_A0], lamg_csp_, get_ptr(m->lam), lam_cusp_, pv); + casadi_dense_transfer(0.5, arg[CONIC_LAM_X0], usp_, get_ptr(m->lam), lam_uusp_, pv); + casadi_dense_transfer(0.5, arg[CONIC_LAM_X0], xsp_, get_ptr(m->lam), lam_xusp_, pv); + + m->return_status = + fortran_order_d_ip_ocp_hard_tv(&m->iter_count, max_iter_, mu0_, tol_, N_, get_ptr(m->nx), + get_ptr(m->nu), get_ptr(m->nb), get_ptr(m->hidxbs), get_ptr(m->ng), N_, warm_start_, + get_ptr(m->As), get_ptr(m->Bs), get_ptr(m->bs), get_ptr(m->Qs), get_ptr(m->Ss), + get_ptr(m->Rs), get_ptr(m->qs), get_ptr(m->rs), get_ptr(m->lbs), get_ptr(m->ubs), + get_ptr(m->Cs), get_ptr(m->Ds), get_ptr(m->lgs), get_ptr(m->ugs), get_ptr(m->xs), + get_ptr(m->us), get_ptr(m->pis), get_ptr(m->lams), get_ptr(m->res), + get_ptr(m->workspace), get_ptr(m->stats)); + + m->fstats.at("solver").toc(); + m->fstats.at("postprocessing").tic(); + if (print_level_>0) { + userOut() << "HPMPC finished after " << m->iter_count << " iterations." << std::endl; + userOut() << "return status: " << m->return_status << std::endl; + userOut() << "residuals: " << m->res << std::endl; + } + + std::fill(res[CONIC_X], res[CONIC_X]+nx_, 0); + casadi_dense_transfer(1.0, get_ptr(m->x), theirs_Xsp_, res[CONIC_X], xsp_, pv); + casadi_dense_transfer(1.0, get_ptr(m->u), theirs_Usp_, res[CONIC_X], usp_, pv); + + std::fill(res[CONIC_LAM_X], res[CONIC_LAM_X]+nx_, 0); + std::fill(res[CONIC_LAM_A], res[CONIC_LAM_A]+na_, 0); + + // Deal with non-unity I block + for (int k=0;knx[k+1]; + for (int i=0;iIs[k][i]; + m->pis[k][i]*=f; + } + } + + casadi_dense_transfer(2.0, get_ptr(m->pi), pisp_, res[CONIC_LAM_A], lamg_gapsp_, pv); + casadi_dense_transfer(2.0, get_ptr(m->lam), lam_cusp_, res[CONIC_LAM_A], lamg_csp_, pv); + casadi_dense_transfer(-2.0, get_ptr(m->lam), lam_clsp_, res[CONIC_LAM_A], lamg_csp_, pv); + + casadi_dense_transfer(-2.0, get_ptr(m->lam), lam_ulsp_, res[CONIC_LAM_X], usp_, pv); + casadi_dense_transfer(2.0, get_ptr(m->lam), lam_uusp_, res[CONIC_LAM_X], usp_, pv); + casadi_dense_transfer(-2.0, get_ptr(m->lam), lam_xlsp_, res[CONIC_LAM_X], xsp_, pv); + casadi_dense_transfer(2.0, get_ptr(m->lam), lam_xusp_, res[CONIC_LAM_X], xsp_, pv); + + // Construct f + double f = casadi_dot(nx_, arg[CONIC_G], res[CONIC_X]); + f += 0.5*casadi_bilin(arg[CONIC_H], sparsity_in(CONIC_H), res[CONIC_X], res[CONIC_X]); + res[CONIC_COST][0] = f; + + m->fstats.at("postprocessing").toc(); + + // Show statistics + if (print_time_) print_fstats(static_cast(mem)); + } + + Dict HpmpcInterface::get_stats(void* mem) const { + Dict stats = Conic::get_stats(mem); + auto m = static_cast(mem); + stats["return_status"] = m->return_status; + stats["iter_count"] = m->iter_count; + stats["res"] = m->res; + return stats; + } + + HpmpcMemory::HpmpcMemory() { + } + + HpmpcMemory::~HpmpcMemory() { + + } + + Sparsity HpmpcInterface::blocksparsity(int rows, int cols, const std::vector& blocks, + bool eye) { + DM r(rows, cols); + for (auto && b : blocks) { + if (eye) { + r(range(b.offset_r, b.offset_r+b.rows), + range(b.offset_c, b.offset_c+b.cols)) = DM::eye(b.rows); + casadi_assert(b.rows==b.cols); + } else { + r(range(b.offset_r, b.offset_r+b.rows), + range(b.offset_c, b.offset_c+b.cols)) = DM::zeros(b.rows, b.cols); + } + } + return r.sparsity(); + } + void HpmpcInterface::blockptr(std::vector& vs, std::vector& v, + const std::vector& blocks, bool eye) { + int N = blocks.size(); + vs.resize(N); + int offset=0; + for (int k=0;k + +/** \defgroup plugin_Conic_hpmpc +Interface to HMPC Solver + + +In order to use this interface, you must: + + - Decision variables must only by state and control, + and the variable ordering must be [x0 u0 x1 u1 ...] + - The constraints must be in order: [ gap0 lincon0 gap1 lincon1 ] + - The gap constraints must be diagonal sparse + - Supply nx, ng, nu options + + +*/ + +#ifndef HPMPC_DLOPEN +#include +extern "C" { +#include +} +#endif + +/** \pluginsection{Conic,hpmpc} */ + +/// \cond INTERNAL +namespace casadi { + + // Forward declaration + class HpmpcInterface; + + struct CASADI_CONIC_HPMPC_EXPORT HpmpcMemory : public ConicMemory { + + std::vector A, B, b, b2, Q, S, R, q, r, lb, ub, C, D, lg, ug; + std::vector As, Bs, bs, Qs, Ss, Rs, qs, rs, lbs, ubs, Cs, Ds, lgs, ugs; + + std::vector I; + std::vector Is; + std::vector x, u, pi, lam; + std::vector xs, us, pis, lams; + + std::vector hidxb; + std::vector hidxbs; + + std::vector nx; + std::vector nu; + std::vector ng; + std::vector nb; + std::vector stats; + + std::vector workspace; + + std::vector pv; + + int iter_count; + int return_status; + std::vector res; + + /// Constructor + HpmpcMemory(); + + /// Destructor + ~HpmpcMemory(); + }; + + /** \brief \pluginbrief{Conic,hpmpc} + * + * @copydoc QPSolver_doc + * @copydoc plugin_Conic_hpmpc + * + * \author Joris Gillis + * \date 2016 + * + * */ + class CASADI_CONIC_HPMPC_EXPORT HpmpcInterface : public Conic { + public: + /** \brief Constructor */ + explicit HpmpcInterface(); + + /** \brief Create a new QP Solver */ + static Conic* creator(const std::string& name, + const std::map& st) { + return new HpmpcInterface(name, st); + } + + /** \brief Create a new Solver */ + explicit HpmpcInterface(const std::string& name, + const std::map& st); + + /// Get all statistics + virtual Dict get_stats(void* mem) const; + + /** \brief Destructor */ + virtual ~HpmpcInterface(); + + // Get name of the plugin + virtual const char* plugin_name() const { return "hpmpc";} + + ///@{ + /** \brief Options */ + static Options options_; + virtual const Options& get_options() const { return options_;} + ///@} + + /** \brief Initialize */ + virtual void init(const Dict& opts); + + /** \brief Create memory block */ + virtual void* alloc_memory() const { return new HpmpcMemory();} + + /** \brief Free memory block */ + virtual void free_memory(void *mem) const { delete static_cast(mem);} + + /** \brief Initalize memory block */ + virtual void init_memory(void* mem) const; + + /** \brief Evaluate numerically */ + virtual void eval(void* mem, const double** arg, double** res, int* iw, double* w) const; + + /// A documentation string + static const std::string meta_doc; + +#ifdef HPMPC_DLOPEN + /// hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes function + typedef int (*Work_size)(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, int N2); + /// fortran_order_d_ip_ocp_hard_tv function + typedef int (*Ocp_solve)(int *kk, int k_max, double mu0, double mu_tol, + int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, int N2, int warm_start, + double **A, double **B, double **b, double **Q, double **S, double **R, double **q, + double **r, double **lb, double **ub, double **C, double **D, double **lg, double **ug, + double **x, double **u, double **pi, double **lam, /*double **t,*/ + double *inf_norm_res, void *work0, double *stat); +#endif + + protected: + + struct Block { + int offset_r; + int offset_c; + int rows; + int cols; + }; + + static Sparsity blocksparsity(int rows, int cols, const std::vector& b, bool eye=false); + static void blockptr(std::vector& vs, std::vector& v, + const std::vector& blocks, bool eye=false); + Sparsity Asp_, Bsp_, Csp_, Dsp_, Isp_, Rsp_, Ssp_, Qsp_, bsp_, lugsp_, usp_, xsp_; + Sparsity theirs_xsp_, theirs_usp_, theirs_Xsp_, theirs_Usp_; + Sparsity lamg_gapsp_, lamg_csp_, lam_ulsp_, lam_uusp_, lam_xlsp_, lam_xusp_, lam_clsp_; + Sparsity lam_cusp_, pisp_; + + std::vector< Block > R_blocks, S_blocks, Q_blocks; + std::vector< Block > b_blocks, lug_blocks; + std::vector< Block > u_blocks, x_blocks; + std::vector< Block > lam_ul_blocks, lam_xl_blocks, lam_uu_blocks, lam_xu_blocks, lam_cl_blocks; + std::vector< Block > lam_cu_blocks, A_blocks, B_blocks, C_blocks, D_blocks, I_blocks; + + std::vector nxs_; + std::vector nus_; + std::vector ngs_; + int N_; + int print_level_; + + bool warm_start_; + double inf_; + + double mu0_; // max element in cost function as estimate of max multiplier + int max_iter_; // maximum number of iterations + double tol_; // tolerance in the duality measure + + std::string blasfeo_target_; + std::string target_; + +#ifdef HPMPC_DLOPEN + Work_size hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes; + Ocp_solve fortran_order_d_ip_ocp_hard_tv; +#endif + + + }; + +} // namespace casadi + +/// \endcond +#endif // CASADI_HPMPC_INTERFACE_HPP diff --git a/casadi/interfaces/hpmpc/hpmpc_interface_meta.cpp b/casadi/interfaces/hpmpc/hpmpc_interface_meta.cpp new file mode 100644 index 0000000000..63e1a72db5 --- /dev/null +++ b/casadi/interfaces/hpmpc/hpmpc_interface_meta.cpp @@ -0,0 +1,30 @@ +/* + * This file is part of CasADi. + * + * CasADi -- A symbolic framework for dynamic optimization. + * Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl, + * K.U. Leuven. All rights reserved. + * Copyright (C) 2011-2014 Greg Horn + * + * CasADi is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * CasADi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with CasADi; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + + #include "hpmpc_interface.hpp" + #include + + const std::string casadi::HpmpcInterface::meta_doc= + "\n"; diff --git a/cmake/FindBLASFEO.cmake b/cmake/FindBLASFEO.cmake new file mode 100644 index 0000000000..5b1673ee93 --- /dev/null +++ b/cmake/FindBLASFEO.cmake @@ -0,0 +1,31 @@ +message(STATUS "Looking for blasfeo") + +find_path(BLASFEO_INCLUDE_DIR + blasfeo_common.h + HINTS $ENV{BLASFEO}/include /opt/blasfeo/include + ) + +if(BLASFEO_INCLUDE_DIR) + message(STATUS "Found blasfeo include directory: ${BLASFEO_INCLUDE_DIR}") +else() + message(STATUS "Could not find blasfeo include dir") +endif() + +find_library(BLASFEO_LIB + NAMES blasfeo + HINTS $ENV{BLASFEO}/lib /opt/blasfeo/lib +) + +if(BLASFEO_LIB) + set(BLASFEO_LIBRARIES ${BLASFEO_LIB}) + message(STATUS "Found blasfeo libraries ${BLASFEO_LIBRARIES}") + set(BLASFEO_FOUND_LIBS TRUE) +else() + message(STATUS "Could not find blasfeo libraries") +endif() + +if(BLASFEO_INCLUDE_DIR AND BLASFEO_FOUND_LIBS) + set(BLASFEO_FOUND TRUE) +else() + message(STATUS "BLASFEO: Cound not find blasfeo. Try setting BLASFEO env var.") +endif() diff --git a/cmake/FindHPMPC.cmake b/cmake/FindHPMPC.cmake new file mode 100644 index 0000000000..6f48a16396 --- /dev/null +++ b/cmake/FindHPMPC.cmake @@ -0,0 +1,34 @@ +message(STATUS "Looking for hpmpc") + +find_path(HPMPC_INCLUDE_DIR + mpc_solvers.h + HINTS $ENV{HPMPC}/include /opt/hpmpc/include + ) + +if(HPMPC_INCLUDE_DIR) + #set(HPMPC_INCLUDE_DIR ${HPMPC_INCLUDE_DIR} ${B_INCLUDE_DIR}) + message(STATUS "Found hpmpc include directory: ${HPMPC_INCLUDE_DIR}") +else() + message(STATUS "Could not find hpmpc include dir") +endif() + +find_library(HPMPC_LIB + NAMES hpmpc + HINTS $ENV{CSPARSE}/lib /opt/hpmpc/lib +) + +if(HPMPC_LIB) + set(HPMPC_LIBRARIES ${HPMPC_LIB} ${BLASFEO_LIBRARIES}) + message(STATUS "Found hpmpc libraries ${HPMPC_LIBRARIES}") + set(HPMPC_FOUND_LIBS TRUE) +else() + message(STATUS "Could not find hpmpc libraries") +endif() + +if(HPMPC_INCLUDE_DIR AND HPMPC_FOUND_LIBS) + set(HPMPC_FOUND TRUE) +else() + message(STATUS "HPMPC: Cound not find hpmpc. Try setting HPMPC env var.") +endif() + + diff --git a/external_packages/CMakeLists.txt b/external_packages/CMakeLists.txt index d9af8c873f..41ce1d22b6 100644 --- a/external_packages/CMakeLists.txt +++ b/external_packages/CMakeLists.txt @@ -64,3 +64,11 @@ endif() if(WITH_BUILD_DSDP AND DSDP_FOUND) add_subdirectory(DSDP) endif() + +if(WITH_BUILD_HPMPC) + add_subdirectory(hpmpc) +endif() + +if(WITH_BUILD_BLASFEO) + add_subdirectory(blasfeo) +endif() diff --git a/external_packages/blasfeo/CMakeLists.txt b/external_packages/blasfeo/CMakeLists.txt new file mode 100644 index 0000000000..0e9b80f3e5 --- /dev/null +++ b/external_packages/blasfeo/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 2.8.6) + + if(WIN32) + set(BLASFEO_FLAGS -DOS_WINDOWS) + elseif(APPLE) + set(BLASFEO_FLAGS -DOS_MAC) + else() + set(BLASFEO_FLAGS -DOS_LINUX) + endif() + casadi_external_library(casadi_blasfeo_X64_INTEL_HASWELL src/aux/d_aux_lib4.c +src/aux/d_aux_extern_depend_lib4.c +src/aux/s_aux_lib4.c +src/aux/s_aux_extern_depend_lib4.c +src/aux/i_aux_extern_depend_lib4.c +src/aux/avx2/kernel_dgetr_lib4.c +src/aux/avx/kernel_dgecp_lib4.c +src/kernel/avx2/kernel_dgemm_4x4_lib4.S +src/kernel/avx2/kernel_dgemm_8x4_lib4.S +src/kernel/avx2/kernel_dgemm_12x4_lib4.S +src/kernel/avx2/kernel_dgemv_8_lib4.S +src/kernel/avx2/kernel_dgetrf_pivot_4_lib4.c +src/kernel/avx2/kernel_dsymv_6_lib4.S +src/kernel/avx/kernel_dgemv_4_lib4.S +src/kernel/c99/kernel_dsymv_4_lib4.c +src/kernel/c99/kernel_sgemm_4x4_lib4.c +src/kernel/c99/kernel_sgemv_4_lib4.c +src/blas/d_blas3_lib4.c +src/blas/d_lapack_lib4.c +src/blas/d_blas2_lib4.c +src/blas/d_blas1_lib4.c +src/blas/s_blas3_lib4.c +src/blas/s_lapack_lib4.c +src/blas/s_blas2_lib4.c) +set_target_properties(casadi_blasfeo_X64_INTEL_HASWELL PROPERTIES COMPILE_FLAGS "-O2 -fPIC -m64 -mavx2 -mfma -DTARGET_X64_INTEL_HASWELL -DLA_BLASFEO -DREF_BLAS_OPENBLAS ${BLASFEO_FLAGS}") + +set_property(SOURCE src/kernel/avx2/kernel_dgemm_4x4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx2/kernel_dgemm_8x4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx2/kernel_dgemm_12x4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx2/kernel_dgemv_8_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx2/kernel_dsymv_6_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dgemv_4_lib4.S PROPERTY LANGUAGE C) +casadi_external_library(casadi_blasfeo_X64_INTEL_SANDY_BRIDGE src/aux/d_aux_lib4.c +src/aux/d_aux_extern_depend_lib4.c +src/aux/s_aux_lib4.c +src/aux/s_aux_extern_depend_lib4.c +src/aux/i_aux_extern_depend_lib4.c +src/aux/avx/kernel_dgecp_lib4.c +src/aux/avx/kernel_dgetr_lib4.c +src/kernel/avx/kernel_dgemm_4x4_lib4.S +src/kernel/avx/kernel_dgemm_8x4_lib4.S +src/kernel/avx/kernel_dgemv_12_lib4.S +src/kernel/avx/kernel_dgemv_8_lib4.S +src/kernel/avx/kernel_dgemv_4_lib4.S +src/kernel/avx/kernel_dgetrf_pivot_4_lib4.c +src/kernel/avx/kernel_dsymv_6_lib4.S +src/kernel/c99/kernel_dsymv_4_lib4.c +src/kernel/c99/kernel_sgemm_4x4_lib4.c +src/kernel/c99/kernel_sgemv_4_lib4.c +src/blas/d_blas3_lib4.c +src/blas/d_lapack_lib4.c +src/blas/d_blas2_lib4.c +src/blas/d_blas1_lib4.c +src/blas/s_blas3_lib4.c +src/blas/s_lapack_lib4.c +src/blas/s_blas2_lib4.c) +set_target_properties(casadi_blasfeo_X64_INTEL_SANDY_BRIDGE PROPERTIES COMPILE_FLAGS "-O2 -fPIC -m64 -mavx -DTARGET_X64_INTEL_SANDY_BRIDGE -DLA_BLASFEO -DREF_BLAS_OPENBLAS ${BLASFEO_FLAGS}") + +set_property(SOURCE src/kernel/avx/kernel_dgemm_4x4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dgemm_8x4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dgemv_12_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dgemv_8_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dgemv_4_lib4.S PROPERTY LANGUAGE C) +set_property(SOURCE src/kernel/avx/kernel_dsymv_6_lib4.S PROPERTY LANGUAGE C) +casadi_external_library(casadi_blasfeo_X64_INTEL_CORE src/aux/d_aux_lib4.c +src/aux/d_aux_extern_depend_lib4.c +src/aux/s_aux_lib4.c +src/aux/s_aux_extern_depend_lib4.c +src/aux/i_aux_extern_depend_lib4.c +src/aux/c99/kernel_dgecp_lib4.c +src/aux/c99/kernel_dgetr_lib4.c +src/kernel/sse3/kernel_dgemm_4x4_lib4.S +src/kernel/c99/kernel_dgemm_4x4_lib4.c +src/kernel/c99/kernel_dgemv_4_lib4.c +src/kernel/c99/kernel_dgetrf_pivot_4_lib4.c +src/kernel/c99/kernel_dsymv_4_lib4.c +src/kernel/c99/kernel_sgemm_4x4_lib4.c +src/kernel/c99/kernel_sgemv_4_lib4.c +src/blas/d_blas3_lib4.c +src/blas/d_lapack_lib4.c +src/blas/d_blas2_lib4.c +src/blas/d_blas1_lib4.c +src/blas/s_blas3_lib4.c +src/blas/s_lapack_lib4.c +src/blas/s_blas2_lib4.c) +set_target_properties(casadi_blasfeo_X64_INTEL_CORE PROPERTIES COMPILE_FLAGS "-O2 -fPIC -m64 -msse3 -DTARGET_X64_INTEL_CORE -DLA_BLASFEO -DREF_BLAS_OPENBLAS ${BLASFEO_FLAGS}") + +set_property(SOURCE src/kernel/sse3/kernel_dgemm_4x4_lib4.S PROPERTY LANGUAGE C) +casadi_external_library(casadi_blasfeo_X64_AMD_BULLDOZER src/aux/d_aux_lib4.c +src/aux/d_aux_extern_depend_lib4.c +src/aux/s_aux_lib4.c +src/aux/s_aux_extern_depend_lib4.c +src/aux/i_aux_extern_depend_lib4.c +src/aux/c99/kernel_dgecp_lib4.c +src/aux/c99/kernel_dgetr_lib4.c +src/kernel/fma/kernel_dgemm_4x4_lib4.S +src/kernel/c99/kernel_dgemm_4x4_lib4.c +src/kernel/c99/kernel_dgemv_4_lib4.c +src/kernel/c99/kernel_dgetrf_pivot_4_lib4.c +src/kernel/c99/kernel_dsymv_4_lib4.c +src/kernel/c99/kernel_sgemm_4x4_lib4.c +src/kernel/c99/kernel_sgemv_4_lib4.c +src/blas/d_blas3_lib4.c +src/blas/d_lapack_lib4.c +src/blas/d_blas2_lib4.c +src/blas/d_blas1_lib4.c +src/blas/s_blas3_lib4.c +src/blas/s_lapack_lib4.c +src/blas/s_blas2_lib4.c) +set_target_properties(casadi_blasfeo_X64_AMD_BULLDOZER PROPERTIES COMPILE_FLAGS "-O2 -fPIC -m64 -mavx -mfma -DTARGET_X64_AMD_BULLDOZER -DLA_BLASFEO -DREF_BLAS_OPENBLAS ${BLASFEO_FLAGS}") + +set_property(SOURCE src/kernel/fma/kernel_dgemm_4x4_lib4.S PROPERTY LANGUAGE C) +casadi_external_library(casadi_blasfeo_GENERIC src/aux/d_aux_lib4.c +src/aux/d_aux_extern_depend_lib4.c +src/aux/s_aux_lib4.c +src/aux/s_aux_extern_depend_lib4.c +src/aux/i_aux_extern_depend_lib4.c +src/aux/c99/kernel_dgecp_lib4.c +src/aux/c99/kernel_dgetr_lib4.c +src/kernel/c99/kernel_dgemm_4x4_lib4.c +src/kernel/c99/kernel_dgemv_4_lib4.c +src/kernel/c99/kernel_dgetrf_pivot_4_lib4.c +src/kernel/c99/kernel_dsymv_4_lib4.c +src/kernel/c99/kernel_sgemm_4x4_lib4.c +src/kernel/c99/kernel_sgemv_4_lib4.c +src/blas/d_blas3_lib4.c +src/blas/d_lapack_lib4.c +src/blas/d_blas2_lib4.c +src/blas/d_blas1_lib4.c +src/blas/s_blas3_lib4.c +src/blas/s_lapack_lib4.c +src/blas/s_blas2_lib4.c) +set_target_properties(casadi_blasfeo_GENERIC PROPERTIES COMPILE_FLAGS "-O2 -fPIC -march=native -DTARGET_GENERIC -DLA_BLASFEO -DREF_BLAS_OPENBLAS ${BLASFEO_FLAGS}") + diff --git a/external_packages/blasfeo/make2cmake.py b/external_packages/blasfeo/make2cmake.py new file mode 100644 index 0000000000..fa3b281ba2 --- /dev/null +++ b/external_packages/blasfeo/make2cmake.py @@ -0,0 +1,59 @@ +import subprocess +import re +import os + +targets = set() +for L in file("src/Makefile","r"): + m = re.match("ifeq \(\$\(TARGET\), ([A-Z0-9_]+)\)", L) + if m: + targets.add(m.group(1)) + +targets = ["X64_INTEL_HASWELL","X64_INTEL_SANDY_BRIDGE","X64_INTEL_CORE","X64_AMD_BULLDOZER","GENERIC"] + +target_data = {} + +this_pwd = os.getcwd() +for t in targets: + p = subprocess.Popen(["make","CC=echo compiler_action `pwd`","TARGET=%s" % t],stdout=subprocess.PIPE,stderr=subprocess.PIPE,cwd="src") + + stdout,stderr = p.communicate() + + data = [] + + for L in stdout.split("\n"): + if L.startswith("compiler_action"): + line = L.split(" ") + pwd = line[1][len(this_pwd)+1:] + cfile = line[-1] + flags = [f for f in line[2:-4] if not f.startswith("-I") and not f.startswith("-DOS")] + data.append((cfile,pwd,flags)) + + for cfile,pwd,f in data: + if not( f==data[0][2] or len(f)==0): + print "warning", f, data[0][2] + target_data[t] = data + +with file('CMakeLists.txt','w') as f: + f.write("cmake_minimum_required(VERSION 2.8.6)\n") + f.write( + """ + if(WIN32) + set(BLASFEO_FLAGS -DOS_WINDOWS) + elseif(APPLE) + set(BLASFEO_FLAGS -DOS_MAC) + else() + set(BLASFEO_FLAGS -DOS_LINUX) + endif() + """) + + for t in targets: + data = target_data[t] + + libname = "casadi_blasfeo_%s" % t + + f.write("casadi_external_library(%s %s)\n" % (libname, "\n".join([ os.path.join(pwd,cfile) for cfile, pwd, _ in data]))) + f.write("set_target_properties(%s PROPERTIES COMPILE_FLAGS \"%s ${BLASFEO_FLAGS}\")\n\n" % (libname, " ".join(data[0][2]))) + for cfile, pwd, _ in data: + if cfile.endswith("S"): + f.write("set_property(SOURCE %s PROPERTY LANGUAGE C)\n" % os.path.join(pwd,cfile)) + diff --git a/external_packages/blasfeo/src b/external_packages/blasfeo/src new file mode 160000 index 0000000000..6e3ee3d7a7 --- /dev/null +++ b/external_packages/blasfeo/src @@ -0,0 +1 @@ +Subproject commit 6e3ee3d7a7e971c0f3f6928e096e9a983679229f diff --git a/external_packages/hpmpc/CMakeLists.txt b/external_packages/hpmpc/CMakeLists.txt new file mode 100644 index 0000000000..e662a422ed --- /dev/null +++ b/external_packages/hpmpc/CMakeLists.txt @@ -0,0 +1,246 @@ +cmake_minimum_required(VERSION 2.8.6) + + if(WIN32) + set(HPMPC_FLAGS -DOS_WINDOWS) + elseif(APPLE) + set(HPMPC_FLAGS -DOS_MAC) + else() + set(HPMPC_FLAGS -DOS_LINUX) + endif() + if(BLASFEO_DLOPEN) + message("Cross compilation flags") + if(MINGW) + set(CMAKE_SHARED_LINKER_FLAGS ${CMAKE_SHARED_LINKER_FLAGS} -Wl,--unresolved-symbols=ignore-all) + endif() + if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + set(CMAKE_SHARED_LIBRARY_CREATE_C_FLAGS "${CMAKE_SHARED_LIBRARY_CREATE_C_FLAGS} -undefined dynamic_lookup") + endif() + endif() + + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../blasfeo/src/include) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dummy_include) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dummy_include/foo/bar) +casadi_external_library(casadi_hpmpc_X64_AVX_GENERIC src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/avx/kernel_dgemm_avx_lib4.c +src/kernel/avx/kernel_dtrmm_avx_lib4.c +src/kernel/avx/kernel_dtrsm_avx_lib4.c +src/kernel/avx/kernel_dsyrk_avx_lib4.c +src/kernel/avx/kernel_dpotrf_avx_lib4.c +src/kernel/avx/kernel_dgemv_avx_lib4.c +src/kernel/avx/kernel_dtrmv_avx_lib4.c +src/kernel/avx/kernel_dtrsv_avx_lib4.c +src/kernel/avx/kernel_dsymv_avx_lib4.c +src/kernel/avx/kernel_dtran_avx_lib4.c +src/kernel/avx/kernel_dttmm_avx_lib4.c +src/kernel/avx/kernel_dtrinv_avx_lib4.c +src/kernel/avx/kernel_dcopy_avx_lib4.c +src/kernel/avx/kernel_dgetrf_avx_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/avx/d_aux_ip_hard_lib4.c +src/mpc_solvers/avx/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_X64_AVX_GENERIC PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_GENERIC -DTARGET_X64_AVX -O2 -fPIC -DBLASFEO -m64 -mavx -DTARGET_X64_AVX ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_X64_AVX2_X64_INTEL_HASWELL src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/avx2/kernel_dgemm_avx2_lib4.c +src/kernel/avx2/kernel_dtrmm_avx2_lib4.c +src/kernel/avx2/kernel_dtrsm_avx2_lib4.c +src/kernel/avx2/kernel_dsyrk_avx2_lib4.c +src/kernel/avx2/kernel_dpotrf_avx2_lib4.c +src/kernel/avx2/kernel_dgemv_avx2_lib4.c +src/kernel/avx2/kernel_dtrmv_avx2_lib4.c +src/kernel/avx2/kernel_dtrsv_avx2_lib4.c +src/kernel/avx2/kernel_dsymv_avx2_lib4.c +src/kernel/avx2/kernel_dtran_avx2_lib4.c +src/kernel/avx2/kernel_dttmm_avx2_lib4.c +src/kernel/avx2/kernel_dtrinv_avx2_lib4.c +src/kernel/avx2/kernel_dgetrf_avx2_lib4.c +src/kernel/avx/kernel_dcopy_avx_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/avx/d_aux_ip_hard_lib4.c +src/mpc_solvers/avx/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_X64_AVX2_X64_INTEL_HASWELL PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_X64_INTEL_HASWELL -DTARGET_X64_AVX2 -O2 -fPIC -DBLASFEO -m64 -mavx2 -mfma -DTARGET_X64_AVX2 ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_X64_AVX_X64_INTEL_SANDY_BRIDGE src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/avx/kernel_dgemm_avx_lib4.c +src/kernel/avx/kernel_dtrmm_avx_lib4.c +src/kernel/avx/kernel_dtrsm_avx_lib4.c +src/kernel/avx/kernel_dsyrk_avx_lib4.c +src/kernel/avx/kernel_dpotrf_avx_lib4.c +src/kernel/avx/kernel_dgemv_avx_lib4.c +src/kernel/avx/kernel_dtrmv_avx_lib4.c +src/kernel/avx/kernel_dtrsv_avx_lib4.c +src/kernel/avx/kernel_dsymv_avx_lib4.c +src/kernel/avx/kernel_dtran_avx_lib4.c +src/kernel/avx/kernel_dttmm_avx_lib4.c +src/kernel/avx/kernel_dtrinv_avx_lib4.c +src/kernel/avx/kernel_dcopy_avx_lib4.c +src/kernel/avx/kernel_dgetrf_avx_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/avx/d_aux_ip_hard_lib4.c +src/mpc_solvers/avx/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_X64_AVX_X64_INTEL_SANDY_BRIDGE PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_X64_INTEL_SANDY_BRIDGE -DTARGET_X64_AVX -O2 -fPIC -DBLASFEO -m64 -mavx -DTARGET_X64_AVX ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_X64_SSE3_X64_INTEL_CORE src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/sse3/kernel_dgemm_sse3_lib4.c +src/kernel/sse3/kernel_dtrmm_sse3_lib4.c +src/kernel/sse3/kernel_dtrsm_sse3_lib4.c +src/kernel/sse3/kernel_dsyrk_sse3_lib4.c +src/kernel/sse3/kernel_dpotrf_sse3_lib4.c +src/kernel/c99/kernel_dgemv_c99_lib4.c +src/kernel/c99/kernel_dtrmv_c99_lib4.c +src/kernel/c99/kernel_dtrsv_c99_lib4.c +src/kernel/c99/kernel_dsymv_c99_lib4.c +src/kernel/c99/kernel_dtran_c99_lib4.c +src/kernel/c99/kernel_dttmm_c99_lib4.c +src/kernel/c99/kernel_dtrinv_c99_lib4.c +src/kernel/c99/kernel_dcopy_c99_lib4.c +src/kernel/c99/kernel_dgetrf_c99_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/c99/d_aux_ip_hard_lib4.c +src/mpc_solvers/c99/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_X64_SSE3_X64_INTEL_CORE PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_X64_INTEL_CORE -DTARGET_X64_SSE3 -O2 -fPIC -DBLASFEO -m64 -msse3 -DTARGET_X64_SSE3 ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_X64_SSE3_X64_AMD_BULLDOZER src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/sse3/kernel_dgemm_sse3_lib4.c +src/kernel/sse3/kernel_dtrmm_sse3_lib4.c +src/kernel/sse3/kernel_dtrsm_sse3_lib4.c +src/kernel/sse3/kernel_dsyrk_sse3_lib4.c +src/kernel/sse3/kernel_dpotrf_sse3_lib4.c +src/kernel/c99/kernel_dgemv_c99_lib4.c +src/kernel/c99/kernel_dtrmv_c99_lib4.c +src/kernel/c99/kernel_dtrsv_c99_lib4.c +src/kernel/c99/kernel_dsymv_c99_lib4.c +src/kernel/c99/kernel_dtran_c99_lib4.c +src/kernel/c99/kernel_dttmm_c99_lib4.c +src/kernel/c99/kernel_dtrinv_c99_lib4.c +src/kernel/c99/kernel_dcopy_c99_lib4.c +src/kernel/c99/kernel_dgetrf_c99_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/c99/d_aux_ip_hard_lib4.c +src/mpc_solvers/c99/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_X64_SSE3_X64_AMD_BULLDOZER PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_X64_AMD_BULLDOZER -DTARGET_X64_SSE3 -O2 -fPIC -DBLASFEO -m64 -msse3 -DTARGET_X64_SSE3 ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_C99_4X4_GENERIC src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/c99/kernel_dgemm_c99_lib4.c +src/kernel/c99/kernel_dtrmm_c99_lib4.c +src/kernel/c99/kernel_dtrsm_c99_lib4.c +src/kernel/c99/kernel_dsyrk_c99_lib4.c +src/kernel/c99/kernel_dpotrf_c99_lib4.c +src/kernel/c99/kernel_dgemv_c99_lib4.c +src/kernel/c99/kernel_dtrmv_c99_lib4.c +src/kernel/c99/kernel_dtrsv_c99_lib4.c +src/kernel/c99/kernel_dsymv_c99_lib4.c +src/kernel/c99/kernel_dtran_c99_lib4.c +src/kernel/c99/kernel_dttmm_c99_lib4.c +src/kernel/c99/kernel_dtrinv_c99_lib4.c +src/kernel/c99/kernel_dcopy_c99_lib4.c +src/kernel/c99/kernel_dgetrf_c99_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/c99/d_aux_ip_hard_lib4.c +src/mpc_solvers/c99/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_C99_4X4_GENERIC PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_GENERIC -DTARGET_C99_4X4 -O2 -fPIC -DBLASFEO -march=native -DTARGET_C99_4X4 ${HPMPC_FLAGS}") + +casadi_external_library(casadi_hpmpc_C99_4X4_PREFETCH_GENERIC src/auxiliary/d_aux_lib4.c +src/auxiliary/d_aux_extern_depend_lib4.c +src/auxiliary/i_aux.c +src/kernel/c99/kernel_dgemm_c99_prefetch_lib4.c +src/kernel/c99/kernel_dtrmm_c99_prefetch_lib4.c +src/kernel/c99/kernel_dtrsm_c99_prefetch_lib4.c +src/kernel/c99/kernel_dsyrk_c99_lib4.c +src/kernel/c99/kernel_dpotrf_c99_prefetch_lib4.c +src/kernel/c99/kernel_dgemv_c99_lib4.c +src/kernel/c99/kernel_dtrmv_c99_lib4.c +src/kernel/c99/kernel_dtrsv_c99_lib4.c +src/kernel/c99/kernel_dsymv_c99_lib4.c +src/kernel/c99/kernel_dtran_c99_lib4.c +src/kernel/c99/kernel_dttmm_c99_lib4.c +src/kernel/c99/kernel_dtrinv_c99_lib4.c +src/kernel/c99/kernel_dcopy_c99_lib4.c +src/kernel/c99/kernel_dgetrf_c99_lib4.c +src/blas/blas_d_lib4.c +src/lqcp_solvers/d_back_ric_rec.c +src/lqcp_solvers/d_for_schur_rec.c +src/lqcp_solvers/d_res.c +src/lqcp_solvers/d_part_cond.c +src/mpc_solvers/d_ip2_hard.c +src/mpc_solvers/d_res_ip_hard.c +src/mpc_solvers/d_ip2_res_hard.c +src/mpc_solvers/c99/d_aux_ip_hard_lib4.c +src/mpc_solvers/c99/d_res_ip_res_hard.c +src/interfaces/c/c_interface_work_space.c +src/interfaces/c/c_order_interface.c +src/interfaces/c/fortran_order_interface.c) +SET_TARGET_PROPERTIES(casadi_hpmpc_C99_4X4_PREFETCH_GENERIC PROPERTIES COMPILE_FLAGS "-DLA_BLASFEO -DTARGET_GENERIC -DTARGET_C99_4X4_PREFETCH -O2 -fPIC -DBLASFEO -march=native -DTARGET_C99_4X4_PREFETCH ${HPMPC_FLAGS}") + diff --git a/external_packages/hpmpc/dummy_include/blasfeo_target.h b/external_packages/hpmpc/dummy_include/blasfeo_target.h new file mode 100644 index 0000000000..8d1c8b69c3 --- /dev/null +++ b/external_packages/hpmpc/dummy_include/blasfeo_target.h @@ -0,0 +1 @@ + diff --git a/external_packages/hpmpc/dummy_include/foo/bar/dummy.txt b/external_packages/hpmpc/dummy_include/foo/bar/dummy.txt new file mode 100644 index 0000000000..8d1c8b69c3 --- /dev/null +++ b/external_packages/hpmpc/dummy_include/foo/bar/dummy.txt @@ -0,0 +1 @@ + diff --git a/external_packages/hpmpc/dummy_include/include/target.h b/external_packages/hpmpc/dummy_include/include/target.h new file mode 100644 index 0000000000..8d1c8b69c3 --- /dev/null +++ b/external_packages/hpmpc/dummy_include/include/target.h @@ -0,0 +1 @@ + diff --git a/external_packages/hpmpc/make2cmake.py b/external_packages/hpmpc/make2cmake.py new file mode 100644 index 0000000000..0349b8c3f1 --- /dev/null +++ b/external_packages/hpmpc/make2cmake.py @@ -0,0 +1,79 @@ +import subprocess +import re +import os + +targets = set() +for L in file("src/Makefile","r"): + m = re.match("ifeq \(\$\(TARGET\), ([A-Z0-9_]+)\)", L) + if m: + targets.add(m.group(1)) + +targets = sorted(list(targets)) + +combinations = [ +("X64_AVX","GENERIC"), +("X64_AVX2","X64_INTEL_HASWELL"), +("X64_AVX","X64_INTEL_SANDY_BRIDGE"), +("X64_SSE3","X64_INTEL_CORE"), +("X64_SSE3","X64_AMD_BULLDOZER"), +("C99_4X4","GENERIC"), +("C99_4X4_PREFETCH","GENERIC")] + +#("CORTEX_A7","GENERIC"), +#("CORTEX_A9","GENERIC"), +#("CORTEX_A15","GENERIC"), +#("CORTEX_A57","GENERIC"), +#("C99_4X4","GENERIC"), +#("C99_4X4_PREFETCH","GENERIC") +#] + +target_data = {} + +this_pwd = os.getcwd() +for t in targets: + p = subprocess.Popen(["make","CC=echo compiler_action `pwd`","TARGET=%s" % t],stdout=subprocess.PIPE,stderr=subprocess.PIPE,cwd="src") + + stdout,stderr = p.communicate() + + data = [] + + for L in stdout.split("\n"): + if L.startswith("compiler_action"): + line = L.split(" ") + pwd = line[1][len(this_pwd)+1:] + cfile = line[-1] + flags = [f for f in line[2:-4] if not f.startswith("-I") and not f.startswith("-DOS")] + data.append((cfile,pwd,flags)) + + for cfile,pwd,f in data: + assert f==data[0][2] or len(f)==0 + target_data[t] = data + +with file('CMakeLists.txt','w') as f: + f.write("cmake_minimum_required(VERSION 2.8.6)\n") + f.write(""" + if(WIN32) + set(HPMPC_FLAGS -DOS_WINDOWS) + elseif(APPLE) + set(HPMPC_FLAGS -DOS_MAC) + else() + set(HPMPC_FLAGS -DOS_LINUX) + endif() + if(BLASFEO_DLOPEN AND MINGW) + message("Cross compilation flags") + set(CMAKE_SHARED_LINKER_FLAGS ${CMAKE_SHARED_LINKER_FLAGS} -Wl,--unresolved-symbols=ignore-all) + endif() + """) + f.write("include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../blasfeo/src/include)\n") + f.write("include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dummy_include)\n") + f.write("include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dummy_include/foo/bar)\n") + #f.write("include_directories(/opt/blasfeo/include)\n") + + for t,blasfeo_target in combinations: + data = target_data[t] + + libname = "casadi_hpmpc_%s_%s" % (t,blasfeo_target) + + f.write("casadi_external_library(%s %s)\n" % (libname, "\n".join([ os.path.join(pwd,cfile) for cfile, pwd, _ in data]))) + f.write("SET_TARGET_PROPERTIES(%s PROPERTIES COMPILE_FLAGS \"-DLA_BLASFEO -DTARGET_%s -DTARGET_%s %s ${HPMPC_FLAGS}\")\n\n" % (libname, blasfeo_target,t , " ".join(data[0][2]))) + diff --git a/external_packages/hpmpc/src b/external_packages/hpmpc/src new file mode 160000 index 0000000000..e3e67ed797 --- /dev/null +++ b/external_packages/hpmpc/src @@ -0,0 +1 @@ +Subproject commit e3e67ed7971f9c6eda40b61c3b34b73b61c35266 diff --git a/test/python/conic.py b/test/python/conic.py index 9055aa3ef8..b878aebc5b 100644 --- a/test/python/conic.py +++ b/test/python/conic.py @@ -749,5 +749,225 @@ def test_linear2(self): self.assertAlmostEqual(solver_out["cost"][0],7,max(1,5-less_digits),str(conic)) + @requires_conic("hpmpc") + @requires_conic("qpoases") + def test_hpmc(self): + + inf = 100 + T = 10. # Time horizon + N = 4 # number of control intervals + + # Declare model variables + x1 = MX.sym('x1') + x2 = MX.sym('x2') + x = vertcat(x1, x2) + u = MX.sym('u') + + # Model equations + xdot = vertcat(0.6*x1 - 1.11*x2 + 0.3*u-0.03, 0.7*x1+0.01) + + # Objective term + L = x1**2 + 3*x2**2 + 7*u**2 -0.4*x1*x2-0.3*x1*u+u -x1-2*x2 + + # Fixed step Runge-Kutta 4 integrator + F = Function('F', [x, u], [x+xdot, L]) + + J = F.jacobian() + # Start with an empty NLP + w=[] + w0 = [] + lbw = [] + ubw = [] + J = 0 + g=[] + lbg = [] + ubg = [] + + Xs = SX.sym('X', 2, 1, N+1) + Us = SX.sym('U', 1, 1, N+1) + + for k in range(N): + + w += [Xs[k]] + + if k==0: + lbw += [-inf, 1] + ubw += [inf, 1] + w0 += [0.3, 0.7] + elif k==2: + lbw += [0, -inf] + ubw += [0, inf] + w0 += [0, 0.3] + else: + lbw += [-inf, -inf] + ubw += [ inf, inf] + w0 += [0, 1] + + w += [Us[k]] + lbw += [-inf] + ubw += [inf] + w0 += [0] + + xplus, l = F(Xs[k],Us[k]) + J+= l + # Add equality constraint + g += [+3*(xplus-Xs[k+1])] + lbg += [0, 0] + ubg += [0, 0] + g += [0.1*Xs[k][1]-0.05*Us[k]] + lbg += [-0.5*k-0.1] + ubg += [2] + g += [0.1*Xs[-1][1]] + lbg += [0.1] + ubg += [2] + + J+= mtimes(Xs[-1].T,Xs[-1]) + + w += [Xs[-1]] + lbw += [-inf, -inf] + ubw += [ inf, inf] + w0 += [0, 1] + + # Create an NLP solver + prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} + + + J = Function("J",[prob["x"]],[jacobian(prob["g"],prob["x"])]) + J(w0).print_dense() + + + solver_ref = qpsol('solver', 'qpoases', prob) + solver = qpsol('solver', 'hpmpc', prob,{"tol":1e-12,"mu0":2,"max_iter":20}) + #solver = qpsol('solver', 'hpmpc', prob,{"N":N,"nx":[2]*(N+1),"nu":[1]*N,"ng":[1]*(N+1),"tol":1e-12,"mu0":2,"max_iter":20}) + + sol_ref = solver_ref(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) + sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) + + self.checkarray(sol_ref["x"], sol["x"]) + self.checkarray(sol_ref["lam_g"], sol["lam_g"],digits=8) + self.checkarray(sol_ref["lam_x"], sol["lam_x"],digits=8) + self.checkarray(sol_ref["f"], sol["f"]) + + @requires_conic("hpmpc") + @requires_conic("qpoases") + def test_hpmc_timevarying(self): + + def mat(a): + def fl(a): + return float(a) if len(a)>0 else 0 + return sparsify(DM([list(map(fl,i.split("\t"))) for i in a.split("\n") if len(i)>0])) + def vec(a): + return DM(list(map(float,a.split("\n")))) + N = 2 + A = """1 0.2 1 -1 +-0.1 0.4 -1 +0.3 0.2 -1 +2 0.3 +1 1 0.4 + 1 4 2 1 0.3 -1 + 3 1 1 0.2 -1 + 1 1 1 1 1 + 2 4 -1 + 2 3 1 + 3""" + A = mat(A) + nx = [2,3,2,1] + nu = [1, 2,1] + ng = [2, 1, 1, 1] + N = 3 + H = """7 0.2 + 7 0.3 +0.2 0.3 1 + 3 1 + 2 0.1 0.7 + 0.1 1 1 + 1 0.1 + 1 0.7 1 0.1 2 + 6 1 + 6 + 1 4 + 9 +""" + + H = mat(H) + #solver = conic('solver', 'hpmpc', {"a": A.sparsity(), "h": H.sparsity()},{"N":N,"nx":nx,"nu":nu,"ng":ng,"tol":1e-12,"mu0":2,"max_iter":20}) + solver = conic('solver', 'hpmpc', {"a": A.sparsity(), "h": H.sparsity()},{"tol":1e-12,"mu0":2,"max_iter":20}) + solver_ref = conic('solver', 'qpoases', {"a": A.sparsity(), "h": H.sparsity()}) + + g = vec("""1 +1 +0.2 +0.4 +1 +0.5 +0.3 +1 +0.6 +1 +1 +0.7""") + lbg = vec("""0 + 0 + 0 + -2 + -2 + 0 + 0 + -2 + 0 + -2 + -2""") + + ubg = vec("""0 + 0 + 0 + 2 + 2 + 0 + 0 + 2 + 0 + 2 + 2""") + + lbx = vec("""0.5 + 0.2 + -1 + -1 + -1 + -1 + -1 + -1 + -1 + -1 + -1 + -1""") + ubx = vec("""0.5 + 0.2 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1""") + + sol = solver(a=A,h=H,lba=lbg,uba=ubg,g=g,lbx=lbx,ubx=ubx) + sol_ref = solver_ref(a=A,h=H,lba=lbg,uba=ubg,g=g,lbx=lbx,ubx=ubx) + + self.checkarray(sol_ref["x"], sol["x"],digits=7) + self.checkarray(sol_ref["lam_a"], sol["lam_a"],digits=8) + self.checkarray(sol_ref["lam_x"], sol["lam_x"],digits=8) + + solver = conic('solver', 'hpmpc', {"a": A.sparsity(), "h": H.sparsity()},{"tol":1e-12,"mu0":2,"max_iter":20,"warm_start":True}) + sol = solver(a=A,h=H,lba=lbg,uba=ubg,g=g,lbx=lbx,ubx=ubx,x0=sol["x"],lam_a0=sol["lam_a"],lam_x0=sol["lam_x"]) + + self.checkarray(sol_ref["x"], sol["x"],digits=7) + self.checkarray(sol_ref["lam_a"], sol["lam_a"],digits=8) + self.checkarray(sol_ref["lam_x"], sol["lam_x"],digits=8) + if __name__ == '__main__': unittest.main()