From 3cb0f54d040404cde0dbf92f9bde0968ff239d2d Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 3 Jun 2023 01:47:26 -0400 Subject: [PATCH 01/15] Use vcpkg master revision --- .github/workflows/windows.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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 From 5ec4862422a54d3cbf3f65ed291e5e9905452cb6 Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 3 Jun 2023 03:26:55 -0400 Subject: [PATCH 02/15] Disable failing tests in trajopt_sco when only oqsp is available --- trajopt_sco/test/CMakeLists.txt | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/trajopt_sco/test/CMakeLists.txt b/trajopt_sco/test/CMakeLists.txt index 6affc262..bb717e1a 100644 --- a/trajopt_sco/test/CMakeLists.txt +++ b/trajopt_sco/test/CMakeLists.txt @@ -32,9 +32,12 @@ include(GoogleTest) set(SCO_TEST_SOURCE unit.cpp - small-problems-unit.cpp - solver-interface-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) +endif() add_executable(${PROJECT_NAME}-test ${SCO_TEST_SOURCE}) target_link_libraries( From e466b3b6922c47bacf5f0233716c9cdf67e4ac86 Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 3 Jun 2023 03:51:11 -0400 Subject: [PATCH 03/15] Add qpoases --- trajopt_ext/qpoases/CMakeLists.txt | 49 +++++++++++++++++++++++++++ trajopt_ext/qpoases/colcon.pkg | 3 ++ trajopt_ext/qpoases/package.xml | 15 ++++++++ trajopt_sco/package.xml | 1 + trajopt_sco/src/qpoases_interface.cpp | 2 +- 5 files changed, 69 insertions(+), 1 deletion(-) create mode 100644 trajopt_ext/qpoases/CMakeLists.txt create mode 100644 trajopt_ext/qpoases/colcon.pkg create mode 100644 trajopt_ext/qpoases/package.xml diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt new file mode 100644 index 00000000..484e6602 --- /dev/null +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -0,0 +1,49 @@ +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 qpOAESES version found. Cloning into build directory") + + include(ExternalProject) + + ExternalProject_Add( + ${PROJECT_NAME} + GIT_REPOSITORY "https://github.com/coin-or/qpOASES.git" + GIT_TAG releases/3.2.1 + SOURCE_DIR ${CMAKE_BINARY_DIR}-src + BINARY_DIR ${CMAKE_BINARY_DIR}-build + 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=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..f6bae7ba --- /dev/null +++ b/trajopt_ext/qpoases/colcon.pkg @@ -0,0 +1,3 @@ +{ + "hooks": ["share/osqp_eigen/hook/ament_prefix_path.dsv", "share/osqp_eigen/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_sco/package.xml b/trajopt_sco/package.xml index 8b78bf1e..2853489a 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..02bcf397 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -165,7 +165,7 @@ void qpOASESModel::update() int inew = 0; for (unsigned iold = 0; iold < cnts_.size(); ++iold) { - const Cnt& cnt = cnts_[iold]; + Cnt& cnt = cnts_[iold]; if (!cnt.cnt_rep->removed) { cnts_[inew] = cnt; From 3c2d7268631818b7b5d749de065402376b392abd Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 3 Jun 2023 14:52:45 -0400 Subject: [PATCH 04/15] Add patch to build shared qpoases, only build qpoases on Windows --- trajopt_ext/qpoases/CMakeLists.txt | 8 +++++--- trajopt_ext/qpoases/patch_qpoases.cmake | 8 ++++++++ 2 files changed, 13 insertions(+), 3 deletions(-) create mode 100644 trajopt_ext/qpoases/patch_qpoases.cmake diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt index 484e6602..5bbe618d 100644 --- a/trajopt_ext/qpoases/CMakeLists.txt +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -6,7 +6,7 @@ project(qpoases VERSION ${pkg_extracted_version} LANGUAGES CXX) find_package(qpoases QUIET) -if(NOT qpOASES_FOUND) +if(NOT qpOASES_FOUND AND WIN32) message(WARNING "No valid qpOAESES version found. Cloning into build directory") include(ExternalProject) @@ -14,15 +14,17 @@ if(NOT qpOASES_FOUND) ExternalProject_Add( ${PROJECT_NAME} GIT_REPOSITORY "https://github.com/coin-or/qpOASES.git" - GIT_TAG releases/3.2.1 + 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=OFF) + -DBUILD_SHARED_LIBS:BOOL=ON + -DQPOASES_BUILD_EXAMPLES:BOOL=OFF) # if(TRAJOPT_PACKAGE) # find_package(ros_industrial_cmake_boilerplate REQUIRED) diff --git a/trajopt_ext/qpoases/patch_qpoases.cmake b/trajopt_ext/qpoases/patch_qpoases.cmake new file mode 100644 index 00000000..e2529b79 --- /dev/null +++ b/trajopt_ext/qpoases/patch_qpoases.cmake @@ -0,0 +1,8 @@ +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}) From 7917bac6d96ad96cbe41b339ce4c1062ca5d287e Mon Sep 17 00:00:00 2001 From: John Wason Date: Mon, 5 Jun 2023 18:00:21 -0400 Subject: [PATCH 05/15] Code format --- trajopt_ext/qpoases/CMakeLists.txt | 27 ++++++------------------- trajopt_ext/qpoases/patch_qpoases.cmake | 6 +++++- trajopt_sco/test/CMakeLists.txt | 10 ++++----- 3 files changed, 16 insertions(+), 27 deletions(-) diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt index 5bbe618d..91e7fd26 100644 --- a/trajopt_ext/qpoases/CMakeLists.txt +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -10,7 +10,7 @@ if(NOT qpOASES_FOUND AND WIN32) message(WARNING "No valid qpOAESES version found. Cloning into build directory") include(ExternalProject) - + ExternalProject_Add( ${PROJECT_NAME} GIT_REPOSITORY "https://github.com/coin-or/qpOASES.git" @@ -26,26 +26,11 @@ if(NOT qpOASES_FOUND AND WIN32) -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() + # 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/patch_qpoases.cmake b/trajopt_ext/qpoases/patch_qpoases.cmake index e2529b79..733f197b 100644 --- a/trajopt_ext/qpoases/patch_qpoases.cmake +++ b/trajopt_ext/qpoases/patch_qpoases.cmake @@ -4,5 +4,9 @@ string( "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}) +string( + REPLACE "SET(CMAKE_INSTALL_BINDIR \${CMAKE_INSTALL_LIBDIR})" + "" + ROOT_FILE3 + ${ROOT_FILE2}) file(WRITE CMakeLists.txt ${ROOT_FILE3}) diff --git a/trajopt_sco/test/CMakeLists.txt b/trajopt_sco/test/CMakeLists.txt index bb717e1a..2fc4b66b 100644 --- a/trajopt_sco/test/CMakeLists.txt +++ b/trajopt_sco/test/CMakeLists.txt @@ -30,11 +30,11 @@ endif() include(GoogleTest) -set(SCO_TEST_SOURCE - unit.cpp - solver-utils-unit.cpp) -if (HAVE_GUROBI OR HAVE_qpOASES) -list(APPEND SCO_TEST_SOURCE +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) endif() From f69c614cf62fa2f1396e3278cc5de5f7e9795d6c Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 16 Jun 2023 22:19:59 -0400 Subject: [PATCH 06/15] Fix qpoases interface build errors and sign conversion warnings --- .../include/trajopt_sco/solver_utils.hpp | 6 ++-- trajopt_sco/src/qpoases_interface.cpp | 36 +++++++++---------- trajopt_sco/src/solver_utils.cpp | 6 ++-- 3 files changed, 24 insertions(+), 24 deletions(-) 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/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index 02bcf397..af8fd979 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,19 @@ 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 +106,16 @@ 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 +140,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,8 +162,8 @@ 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) { Cnt& cnt = cnts_[iold]; if (!cnt.cnt_rep->removed) @@ -187,9 +187,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 +197,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(); From 40e329e0cbc0de5745507ec62aa62debcabc3f07 Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 16 Jun 2023 22:20:40 -0400 Subject: [PATCH 07/15] Fix colcon.pkg in qpoases ext package --- trajopt_ext/qpoases/colcon.pkg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trajopt_ext/qpoases/colcon.pkg b/trajopt_ext/qpoases/colcon.pkg index f6bae7ba..0b0b3569 100644 --- a/trajopt_ext/qpoases/colcon.pkg +++ b/trajopt_ext/qpoases/colcon.pkg @@ -1,3 +1,3 @@ { - "hooks": ["share/osqp_eigen/hook/ament_prefix_path.dsv", "share/osqp_eigen/hook/ros_package_path.dsv"] + "hooks": ["share/qpoases/hook/ament_prefix_path.dsv", "share/qpoases/hook/ros_package_path.dsv"] } From bde105ebc8e7798af8013f20aa1015378a570a1e Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 16 Jun 2023 22:24:48 -0400 Subject: [PATCH 08/15] Build qpoases on all platforms and fix typo in message --- trajopt_ext/qpoases/CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt index 91e7fd26..9161281f 100644 --- a/trajopt_ext/qpoases/CMakeLists.txt +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -6,8 +6,8 @@ project(qpoases VERSION ${pkg_extracted_version} LANGUAGES CXX) find_package(qpoases QUIET) -if(NOT qpOASES_FOUND AND WIN32) - message(WARNING "No valid qpOAESES version found. Cloning into build directory") +if(NOT qpOASES_FOUND) + message(WARNING "No valid qpOASES version found. Cloning into build directory") include(ExternalProject) @@ -26,11 +26,11 @@ if(NOT qpOASES_FOUND AND WIN32) -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() + 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}) From 7aa48a73b09b506f49000bc5bc8c6c5e8149f8a4 Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 16 Jun 2023 22:25:12 -0400 Subject: [PATCH 09/15] code format --- trajopt_sco/src/qpoases_interface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/trajopt_sco/src/qpoases_interface.cpp b/trajopt_sco/src/qpoases_interface.cpp index af8fd979..de9a4827 100644 --- a/trajopt_sco/src/qpoases_interface.cpp +++ b/trajopt_sco/src/qpoases_interface.cpp @@ -90,7 +90,8 @@ void qpOASESModel::updateObjective() exprToEigen(objective_, sm, g_, n, true, true); eigenToCSC(sm, H_row_indices_, H_column_pointers_, H_csc_data_); - H_ = SymSparseMat((int)vars_.size(), (int)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(); } @@ -115,7 +116,8 @@ void qpOASESModel::updateConstraints() } eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); - A_ = SparseMatrix((int)cnts_.size(), (int)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() From 7a68882a1617d58585fec317640014cdaea0f32f Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 16 Jun 2023 22:27:20 -0400 Subject: [PATCH 10/15] code format --- trajopt_ext/qpoases/CMakeLists.txt | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt index 9161281f..15c4250e 100644 --- a/trajopt_ext/qpoases/CMakeLists.txt +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -26,11 +26,26 @@ if(NOT qpOASES_FOUND) -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() + 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}) From 68a9eda11c76ce65802217f78830e9312411bcc7 Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 17 Jun 2023 17:23:04 -0400 Subject: [PATCH 11/15] Disable log printing for sphere collision test --- trajopt/test/simple_collision_unit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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_)); From 26def6193b869eadecf86d5b9275126f12b73f9f Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 17 Jun 2023 17:52:10 -0400 Subject: [PATCH 12/15] Disable log printing for numerical ik test --- trajopt/test/numerical_ik_unit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; } }; From f213da369309a3da75bf2a756fede150ad68b007 Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 17 Jun 2023 20:39:14 -0400 Subject: [PATCH 13/15] Fix trajopt_sqp tests with qpoases enabled --- .../trajopt_sqp/test/CMakeLists.txt | 28 +++++++++++++++++-- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt index 7528d88d..6010746d 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,28 @@ 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) From 3f05e28950c9d91895b759691bff75389f6a9fdf Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 17 Jun 2023 20:40:17 -0400 Subject: [PATCH 14/15] Fix find_package qpOASES --- trajopt_ext/qpoases/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trajopt_ext/qpoases/CMakeLists.txt b/trajopt_ext/qpoases/CMakeLists.txt index 15c4250e..5bf14d99 100644 --- a/trajopt_ext/qpoases/CMakeLists.txt +++ b/trajopt_ext/qpoases/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) project(qpoases VERSION ${pkg_extracted_version} LANGUAGES CXX) -find_package(qpoases QUIET) +find_package(qpOASES QUIET) if(NOT qpOASES_FOUND) message(WARNING "No valid qpOASES version found. Cloning into build directory") From 78d03d7f9d421a2e8d60b3eac71b677a66fb99ae Mon Sep 17 00:00:00 2001 From: John Wason Date: Sat, 17 Jun 2023 20:46:49 -0400 Subject: [PATCH 15/15] code format --- .../trajopt_sqp/test/CMakeLists.txt | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt index 6010746d..0e18be37 100644 --- a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt @@ -70,10 +70,12 @@ 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}") +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( @@ -86,9 +88,10 @@ target_link_libraries( 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}) +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)