Enable qpOASES on Windows and fix Windows build (#327)

.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

trajopt/test/numerical_ik_unit.cpp

@@ -49,7 +49,7 @@ class NumericalIKTest : public testing::TestWithParam<const char*>
     ipos["torso_lift_joint"] = 0.0;
     env_->setState(ipos);
 
-    gLogLevel = util::LevelDebug;
+    // gLogLevel = util::LevelDebug;
   }
 };
 

trajopt/test/simple_collision_unit.cpp

@@ -46,7 +46,7 @@ class SimpleCollisionTest : public testing::TestWithParam<const char*>
     ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
     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_));

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
+      <https://github.com/tesseract-robotics/trajopt>
+      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})

trajopt_ext/qpoases/colcon.pkg

@@ -0,0 +1,3 @@
+{
+    "hooks": ["share/qpoases/hook/ament_prefix_path.dsv", "share/qpoases/hook/ros_package_path.dsv"]
+}

trajopt_ext/qpoases/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>qpoases</name>
+  <version>3.2.1</version>
+  <description>Downloads and builds the qpOASES Library</description>
+  <maintainer email="[email protected]">Levi Armstrong</maintainer>
+  <license>LGPL</license>
+
+  <build_depend>ros_industrial_cmake_boilerplate</build_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+
+</package>

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})

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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
@@ -61,12 +60,38 @@ 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)
 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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>" ${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)

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<double>& sparse_vector, const int& n_vars);
+void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& 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<double>& sparse_vector
 void exprToEigen(const QuadExpr& expr,
                  Eigen::SparseMatrix<double>& 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<double>& 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

trajopt_sco/package.xml

@@ -15,6 +15,7 @@
   <build_export_depend>eigen</build_export_depend>
   <depend>libjsoncpp-dev</depend>
   <depend>osqp</depend>
+  <depend>qpoases</depend>
   <depend>trajopt_utils</depend>
   <depend>boost</depend>
 

trajopt_sco/src/qpoases_interface.cpp

@@ -63,34 +63,35 @@ 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;
 }
 
 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<double> 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<double> 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,19 +189,19 @@ 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];
   }
 }
 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;

trajopt_sco/src/solver_utils.cpp

@@ -10,7 +10,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 namespace sco
 {
-void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& sparse_vector, const int& n_vars)
+void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& sparse_vector, const Eigen::Index& n_vars)
 {
   sparse_vector.resize(n_vars);
   sparse_vector.reserve(static_cast<long int>(expr.size()));
@@ -31,7 +31,7 @@ void exprToEigen(const AffExpr& expr, Eigen::SparseVector<double>& sparse_vector
 void exprToEigen(const QuadExpr& expr,
                  Eigen::SparseMatrix<double>& 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<double>& sparse_matrix,
                  Eigen::VectorXd& vector,
-                 const int& n_vars)
+                 const Eigen::Index& n_vars)
 {
   vector.resize(static_cast<long int>(expr_vec.size()));
   vector.setZero();

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(