From ae823b9fab6059abff6a2d332dcddae7f9bbb0f6 Mon Sep 17 00:00:00 2001 From: John Wason Date: Sun, 18 Jun 2023 13:16:36 -0400 Subject: [PATCH] Enable qpOASES on Windows and fix Windows build (#327) --- .github/workflows/windows.yml | 11 ++-- trajopt/test/numerical_ik_unit.cpp | 2 +- trajopt/test/simple_collision_unit.cpp | 2 +- trajopt_ext/qpoases/CMakeLists.txt | 51 +++++++++++++++++++ trajopt_ext/qpoases/colcon.pkg | 3 ++ trajopt_ext/qpoases/package.xml | 15 ++++++ trajopt_ext/qpoases/patch_qpoases.cmake | 12 +++++ .../trajopt_sqp/test/CMakeLists.txt | 31 +++++++++-- .../include/trajopt_sco/solver_utils.hpp | 6 +-- trajopt_sco/package.xml | 1 + trajopt_sco/src/qpoases_interface.cpp | 40 ++++++++------- trajopt_sco/src/solver_utils.cpp | 6 +-- trajopt_sco/test/CMakeLists.txt | 11 ++-- 13 files changed, 152 insertions(+), 39 deletions(-) create mode 100644 trajopt_ext/qpoases/CMakeLists.txt create mode 100644 trajopt_ext/qpoases/colcon.pkg create mode 100644 trajopt_ext/qpoases/package.xml create mode 100644 trajopt_ext/qpoases/patch_qpoases.cmake diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 59d42d22..7b0c4cf8 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -25,6 +25,11 @@ jobs: with: path: workspace/src/trajopt + - name: configure-msvc + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x64 + - name: vcpkg build uses: johnwason/vcpkg-action@v4 with: @@ -36,6 +41,7 @@ jobs: extra-args: --clean-after-build token: ${{ github.token }} cache-key: ci-${{ matrix.os }} + revision: master - name: install-depends shell: cmd @@ -44,11 +50,6 @@ jobs: python -m pip install colcon-common-extensions -q python -m pip install ninja -q - - name: configure-msvc - uses: ilammy/msvc-dev-cmd@v1 - with: - arch: x64 - - name: build-workspace working-directory: workspace shell: cmd diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp index f134b782..6b0d3490 100644 --- a/trajopt/test/numerical_ik_unit.cpp +++ b/trajopt/test/numerical_ik_unit.cpp @@ -49,7 +49,7 @@ class NumericalIKTest : public testing::TestWithParam ipos["torso_lift_joint"] = 0.0; env_->setState(ipos); - gLogLevel = util::LevelDebug; + // gLogLevel = util::LevelDebug; } }; diff --git a/trajopt/test/simple_collision_unit.cpp b/trajopt/test/simple_collision_unit.cpp index b4c98f40..ccfd2c60 100644 --- a/trajopt/test/simple_collision_unit.cpp +++ b/trajopt/test/simple_collision_unit.cpp @@ -46,7 +46,7 @@ class SimpleCollisionTest : public testing::TestWithParam ResourceLocator::Ptr locator = std::make_shared(); EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator)); - gLogLevel = util::LevelDebug; + // gLogLevel = util::LevelDebug; // Create plotting tool // plotter_.reset(new tesseract_ros::ROSBasicPlotting(env_)); diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt new file mode 100644 index 00000000..5bf14d99 --- /dev/null +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.2.1) +find_package(ros_industrial_cmake_boilerplate REQUIRED) + +extract_package_metadata(pkg) +project(qpoases VERSION ${pkg_extracted_version} LANGUAGES CXX) + +find_package(qpOASES QUIET) + +if(NOT qpOASES_FOUND) + message(WARNING "No valid qpOASES version found. Cloning into build directory") + + include(ExternalProject) + + ExternalProject_Add( + ${PROJECT_NAME} + GIT_REPOSITORY "https://github.com/coin-or/qpOASES.git" + GIT_TAG 0b86dbf00c7fce34420bedc5914f71b176fe79d3 + SOURCE_DIR ${CMAKE_BINARY_DIR}-src + BINARY_DIR ${CMAKE_BINARY_DIR}-build + PATCH_COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/patch_qpoases.cmake + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX} + -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE} + -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET} + -DBUILD_SHARED_LIBS:BOOL=ON + -DQPOASES_BUILD_EXAMPLES:BOOL=OFF) + + if(TRAJOPT_PACKAGE) + find_package(ros_industrial_cmake_boilerplate REQUIRED) + set(CPACK_BUILD_SOURCE_DIRS "${CMAKE_BINARY_DIR}-src;${CMAKE_BINARY_DIR}-build") + set(CPACK_INSTALL_CMAKE_PROJECTS "${CMAKE_BINARY_DIR}-build;${PROJECT_NAME};ALL;/") + cpack( + VERSION + ${pkg_extracted_version} + MAINTAINER + + VENDOR + "ROS-Industrial" + DESCRIPTION + ${pkg_extracted_description} + PACKAGE_PREFIX + ${TRAJOPT_PACKAGE_PREFIX} + LINUX_DEPENDS + "libeigen3-dev" + WINDOWS_DEPENDS + "Eigen3") + endif() +endif() + +install(FILES package.xml DESTINATION share/${PROJECT_NAME}) diff --git a/trajopt_ext/qpoases/colcon.pkg b/trajopt_ext/qpoases/colcon.pkg new file mode 100644 index 00000000..0b0b3569 --- /dev/null +++ b/trajopt_ext/qpoases/colcon.pkg @@ -0,0 +1,3 @@ +{ + "hooks": ["share/qpoases/hook/ament_prefix_path.dsv", "share/qpoases/hook/ros_package_path.dsv"] +} diff --git a/trajopt_ext/qpoases/package.xml b/trajopt_ext/qpoases/package.xml new file mode 100644 index 00000000..476df1a8 --- /dev/null +++ b/trajopt_ext/qpoases/package.xml @@ -0,0 +1,15 @@ + + + qpoases + 3.2.1 + Downloads and builds the qpOASES Library + Levi Armstrong + LGPL + + ros_industrial_cmake_boilerplate + + + cmake + + + diff --git a/trajopt_ext/qpoases/patch_qpoases.cmake b/trajopt_ext/qpoases/patch_qpoases.cmake new file mode 100644 index 00000000..733f197b --- /dev/null +++ b/trajopt_ext/qpoases/patch_qpoases.cmake @@ -0,0 +1,12 @@ +file(READ CMakeLists.txt ROOT_FILE) +string( + REPLACE "MESSAGE(FATAL_ERROR \"Compiling qpOASES as a shared library in Windows is not supported.\")" + "set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)\ninclude(GNUInstallDirs)" + ROOT_FILE2 + ${ROOT_FILE}) +string( + REPLACE "SET(CMAKE_INSTALL_BINDIR \${CMAKE_INSTALL_LIBDIR})" + "" + ROOT_FILE3 + ${ROOT_FILE2}) +file(WRITE CMakeLists.txt ${ROOT_FILE3}) diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt index 7528d88d..0e18be37 100644 --- a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt @@ -45,7 +45,6 @@ macro(add_gtest test_name test_file) ifopt::ifopt_ipopt trajopt::trajopt_ifopt trajopt::trajopt_test_data - trajopt::trajopt GTest::GTest GTest::Main) target_include_directories(${test_name} PRIVATE "$" @@ -61,8 +60,6 @@ add_gtest(${PROJECT_NAME}_joint_position_optimization_unit joint_position_optimi add_gtest(${PROJECT_NAME}_joint_velocity_optimization_unit joint_velocity_optimization_unit.cpp) add_gtest(${PROJECT_NAME}_joint_acceleration_optimization_unit joint_acceleration_optimization_unit.cpp) add_gtest(${PROJECT_NAME}_joint_jerk_optimization_unit joint_jerk_optimization_unit.cpp) -add_gtest(${PROJECT_NAME}_cart_position_optimization_unit cart_position_optimization_unit.cpp) -add_gtest(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit cart_position_optimization_trajopt_sco_unit.cpp) add_gtest(${PROJECT_NAME}_cast_cost_attached_unit cast_cost_attached_unit.cpp) add_gtest(${PROJECT_NAME}_cast_cost_octomap_unit cast_cost_octomap_unit.cpp) add_gtest(${PROJECT_NAME}_cast_cost_unit cast_cost_unit.cpp) @@ -70,3 +67,31 @@ add_gtest(${PROJECT_NAME}_cast_cost_world_unit cast_cost_world_unit.cpp) add_gtest(${PROJECT_NAME}_numerical_ik_unit numerical_ik_unit.cpp) add_gtest(${PROJECT_NAME}_planning_unit planning_unit.cpp) add_gtest(${PROJECT_NAME}_simple_collision_unit simple_collision_unit.cpp) +add_gtest(${PROJECT_NAME}_cart_position_optimization_unit cart_position_optimization_unit.cpp) + +# This is for comparison and should be moved to trajopt +add_executable(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit + cart_position_optimization_trajopt_sco_unit.cpp) +target_compile_options(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit + PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} ${TRAJOPT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit + PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS} TRAJOPT_IFOPT_DIR="${CMAKE_SOURCE_DIR}") +target_cxx_version(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE VERSION ${TRAJOPT_CXX_VERSION}) +target_clang_tidy(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) +target_link_libraries( + ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit + ${PROJECT_NAME} + console_bridge + ifopt::ifopt_ipopt + trajopt::trajopt_ifopt + trajopt::trajopt_test_data + trajopt::trajopt + GTest::GTest + GTest::Main) +target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit + PRIVATE "$" ${GTEST_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit SYSTEM + PRIVATE ${PCL_INCLUDE_DIRS}) +add_gtest_discover_tests(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit) +add_dependencies(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ${PROJECT_NAME}) +add_dependencies(run_tests ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit) diff --git a/trajopt_sco/include/trajopt_sco/solver_utils.hpp b/trajopt_sco/include/trajopt_sco/solver_utils.hpp index c19c9333..52d6c459 100644 --- a/trajopt_sco/include/trajopt_sco/solver_utils.hpp +++ b/trajopt_sco/include/trajopt_sco/solver_utils.hpp @@ -19,7 +19,7 @@ namespace sco * @param [in] n_vars the number of variables in expr. It is usually equal * to `expr.size()`, but it might be larger. */ -void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars); +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const Eigen::Index& n_vars); /** * @brief transform a `QuadExpr` to an `Eigen::SparseMatrix` plus @@ -40,7 +40,7 @@ void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector void exprToEigen(const QuadExpr& expr, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const int& n_vars, + const Eigen::Index& n_vars, const bool& matrix_is_halved = false, const bool& force_diagonal = false); @@ -64,7 +64,7 @@ void exprToEigen(const QuadExpr& expr, void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const int& n_vars = -1); + const Eigen::Index& n_vars = -1); /** * @brief Converts triplets to an `Eigen::SparseMatrix`. * @param [in] rows_i a vector of row indices diff --git a/trajopt_sco/package.xml b/trajopt_sco/package.xml index 7dafb111..8cdca2a1 100644 --- a/trajopt_sco/package.xml +++ b/trajopt_sco/package.xml @@ -15,6 +15,7 @@ eigen libjsoncpp-dev osqp + qpoases trajopt_utils boost diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index 537210a0..de9a4827 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -63,14 +63,14 @@ Cnt qpOASESModel::addIneqCnt(const AffExpr& expr, const std::string& /*name*/) Cnt qpOASESModel::addIneqCnt(const QuadExpr&, const std::string& /*name*/) { assert(0 && "NOT IMPLEMENTED"); - return 0; + return {}; } void qpOASESModel::removeVars(const VarVector& vars) { IntVec inds; vars2inds(vars, inds); - for (unsigned i = 0; i < vars.size(); ++i) + for (size_t i = 0; i < vars.size(); ++i) vars[i].var_rep->removed = true; } @@ -78,19 +78,20 @@ void qpOASESModel::removeCnts(const CntVector& cnts) { IntVec inds; cnts2inds(cnts, inds); - for (unsigned i = 0; i < cnts.size(); ++i) + for (size_t i = 0; i < cnts.size(); ++i) cnts[i].cnt_rep->removed = true; } void qpOASESModel::updateObjective() { - const size_t n = vars_.size(); + const Eigen::Index n = Eigen::Index(vars_.size()); Eigen::SparseMatrix sm; exprToEigen(objective_, sm, g_, n, true, true); eigenToCSC(sm, H_row_indices_, H_column_pointers_, H_csc_data_); - H_ = SymSparseMat(vars_.size(), vars_.size(), H_row_indices_.data(), H_column_pointers_.data(), H_csc_data_.data()); + H_ = SymSparseMat( + (int)vars_.size(), (int)vars_.size(), H_row_indices_.data(), H_column_pointers_.data(), H_csc_data_.data()); H_.createDiagInfo(); } @@ -106,16 +107,17 @@ void qpOASESModel::updateConstraints() Eigen::SparseMatrix sm; Eigen::VectorXd v; - exprToEigen(cnt_exprs_, sm, v, n); + exprToEigen(cnt_exprs_, sm, v, Eigen::Index(n)); - for (int i_cnt = 0; i_cnt < m; ++i_cnt) + for (size_t i_cnt = 0; i_cnt < m; ++i_cnt) { - lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[i_cnt]; - ubA_[i_cnt] = v[i_cnt]; + lbA_[i_cnt] = (cnt_types_[i_cnt] == INEQ) ? -QPOASES_INFTY : v[Eigen::Index(i_cnt)]; + ubA_[i_cnt] = v[Eigen::Index(i_cnt)]; } eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); - A_ = SparseMatrix(cnts_.size(), vars_.size(), A_row_indices_.data(), A_column_pointers_.data(), A_csc_data_.data()); + A_ = SparseMatrix( + (int)cnts_.size(), (int)vars_.size(), A_row_indices_.data(), A_column_pointers_.data(), A_csc_data_.data()); } bool qpOASESModel::updateSolver() @@ -140,8 +142,8 @@ void qpOASESModel::createSolver() void qpOASESModel::update() { { - int inew = 0; - for (unsigned iold = 0; iold < vars_.size(); ++iold) + size_t inew = 0; + for (size_t iold = 0; iold < vars_.size(); ++iold) { Var& var = vars_[iold]; if (!var.var_rep->removed) @@ -162,10 +164,10 @@ void qpOASESModel::update() ub_.resize(inew, -QPOASES_INFTY); } { - int inew = 0; - for (unsigned iold = 0; iold < cnts_.size(); ++iold) + size_t inew = 0; + for (size_t iold = 0; iold < cnts_.size(); ++iold) { - const Cnt& cnt = cnts_[iold]; + Cnt& cnt = cnts_[iold]; if (!cnt.cnt_rep->removed) { cnts_[inew] = cnt; @@ -187,9 +189,9 @@ void qpOASESModel::update() void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, const DblVec& upper) { - for (unsigned i = 0; i < vars.size(); ++i) + for (size_t i = 0; i < vars.size(); ++i) { - const int varind = vars[i].var_rep->index; + const size_t varind = vars[i].var_rep->index; lb_[varind] = lower[i]; ub_[varind] = upper[i]; } @@ -197,9 +199,9 @@ void qpOASESModel::setVarBounds(const VarVector& vars, const DblVec& lower, cons DblVec qpOASESModel::getVarValues(const VarVector& vars) const { DblVec out(vars.size()); - for (unsigned i = 0; i < vars.size(); ++i) + for (size_t i = 0; i < vars.size(); ++i) { - const int varind = vars[i].var_rep->index; + const size_t varind = vars[i].var_rep->index; out[i] = solution_[varind]; } return out; diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp index 2ba82b8d..55bfc818 100644 --- a/trajopt_sco/src/solver_utils.cpp +++ b/trajopt_sco/src/solver_utils.cpp @@ -10,7 +10,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace sco { -void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const int& n_vars) +void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector, const Eigen::Index& n_vars) { sparse_vector.resize(n_vars); sparse_vector.reserve(static_cast(expr.size())); @@ -31,7 +31,7 @@ void exprToEigen(const AffExpr& expr, Eigen::SparseVector& sparse_vector void exprToEigen(const QuadExpr& expr, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const int& n_vars, + const Eigen::Index& n_vars, const bool& matrix_is_halved, const bool& force_diagonal) { @@ -85,7 +85,7 @@ void exprToEigen(const QuadExpr& expr, void exprToEigen(const AffExprVector& expr_vec, Eigen::SparseMatrix& sparse_matrix, Eigen::VectorXd& vector, - const int& n_vars) + const Eigen::Index& n_vars) { vector.resize(static_cast(expr_vec.size())); vector.setZero(); diff --git a/trajopt_sco/test/CMakeLists.txt b/trajopt_sco/test/CMakeLists.txt index 6affc262..2fc4b66b 100644 --- a/trajopt_sco/test/CMakeLists.txt +++ b/trajopt_sco/test/CMakeLists.txt @@ -30,11 +30,14 @@ endif() include(GoogleTest) -set(SCO_TEST_SOURCE - unit.cpp +set(SCO_TEST_SOURCE unit.cpp solver-utils-unit.cpp) +if(HAVE_GUROBI OR HAVE_qpOASES) + list( + APPEND + SCO_TEST_SOURCE small-problems-unit.cpp - solver-interface-unit.cpp - solver-utils-unit.cpp) + solver-interface-unit.cpp) +endif() add_executable(${PROJECT_NAME}-test ${SCO_TEST_SOURCE}) target_link_libraries(