diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ad797cd009..8a2b7be7a7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,6 +14,7 @@ env: YARP_TAG: 964bb26fa4791d83d72882711ea7509306248106 iDynTree_TAG: v1.1.0 Catch2_TAG: v2.11.3 + Qhull_TAG: v8.0.0 jobs: build: @@ -66,7 +67,8 @@ jobs: - name: Dependencies [macOS] if: matrix.os == 'macOS-latest' run: | - brew install ace boost eigen swig qt5 orocos-kdl catch2 + brew update + brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull - name: Dependencies [Ubuntu] if: matrix.os == 'ubuntu-latest' @@ -82,7 +84,7 @@ jobs: with: path: ${{ github.workspace }}/install/deps # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45 - key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }} + key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }} - name: Source-based Dependencies [Windows] @@ -128,6 +130,19 @@ jobs: cmake --build . --config ${{ matrix.build_type }} --target install + # Qhull + cd ${GITHUB_WORKSPACE} + git clone https://github.com/qhull/qhull + cd qhull + git checkout ${Qhull_TAG} + mkdir -p build + cd build + cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ + -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. + cmake --build . --config ${{ matrix.build_type }} --target install + - name: Source-based Dependencies [Ubuntu/macOS] if: steps.cache-source-deps.outputs.cache-hit != 'true' && (matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest') shell: bash @@ -166,15 +181,29 @@ jobs: if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'ubuntu-latest' shell: bash run: | - git clone -b ${Catch2_TAG} https://github.com/catchorg/Catch2.git - cd Catch2 - mkdir -p build - cd build - cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ - -DBUILD_TESTING=OFF .. - - cmake --build . --config ${{ matrix.build_type }} --target install + # Catch2 + git clone -b ${Catch2_TAG} https://github.com/catchorg/Catch2.git + cd Catch2 + mkdir -p build + cd build + cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DBUILD_TESTING=OFF .. + + cmake --build . --config ${{ matrix.build_type }} --target install + + # Qhull + cd ${GITHUB_WORKSPACE} + git clone https://github.com/qhull/qhull + cd qhull + git checkout ${Qhull_TAG} + mkdir -p build + cd build + cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. + cmake --build . --config ${{ matrix.build_type }} --target install + # =================== # CMAKE-BASED PROJECT diff --git a/README.md b/README.md index 8763f8839f..87dc1f9c47 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,7 @@ The **BipedalLocomotionFramework** project is a _suite_ of libraries for achievi - [**BipedalLocomotion::ParametersHandler**](./src/ParametersHandler): Library for retrieving parameters from configuration files and not only - [**BipedalLocomotion::Estimators**](./src/Estimators): Library containing observers +- [**BipedalLocomotion::Planner**](./src/Planner): Library containing planner useful for locomotion # :page_facing_up: Dependencies @@ -48,6 +49,13 @@ file. Please note that the indicated version is the the minimum required version - [`Catch2`](https://github.com/catchorg/Catch2) - [`YARP`](https://github.com/robotology/YARP) +- `Planner` requires: + - For using it: + - [`iDynTree`](https://github.com/robotology/idyntree) (version 0.11.105) + - [`Qhull`](https://github.com/qhull/qhull) (version 8.0.0) + - For testing: + - [`Catch2`](https://github.com/catchorg/Catch2) + # :hammer: Build the suite ## Linux/macOs diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index c9ad923f19..d3977a50c6 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -128,6 +128,9 @@ checkandset_dependency(YARP) find_package(Eigen3 3.2.92 QUIET) checkandset_dependency(Eigen3) +find_package(Qhull 8.0.0 QUIET) +checkandset_dependency(Qhull) + framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_HAS_YARP" OFF) @@ -142,7 +145,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators framework_dependent_option(FRAMEWORK_COMPILE_Planners "Compile Planners libraries?" ON - "FRAMEWORK_HAS_Eigen3" OFF) + "FRAMEWORK_HAS_Eigen3;FRAMEWORK_HAS_Qhull" OFF) framework_dependent_option(FRAMEWORK_COMPILE_ContactModels "Compile ContactModels library?" ON diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index b98c670526..0fd9b34dac 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -7,12 +7,13 @@ if (FRAMEWORK_COMPILE_Planners) set(H_PREFIX include/BipedalLocomotion/Planners) add_bipedal_locomotion_library( - NAME Contact - PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h - SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp - PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core - INSTALLATION_FOLDER Planners - SUBDIRECTORIES tests - ) + NAME Contact + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ConvexHullHelper.h + SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ConvexHullHelper.cpp + PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core + PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r + INSTALLATION_FOLDER Planners + SUBDIRECTORIES tests + DEPENDS_ON_EIGEN_PRIVATE) endif() diff --git a/src/Planners/include/BipedalLocomotion/Planners/ConvexHullHelper.h b/src/Planners/include/BipedalLocomotion/Planners/ConvexHullHelper.h new file mode 100644 index 0000000000..d4c2857ef4 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/ConvexHullHelper.h @@ -0,0 +1,85 @@ +/** + * @file ConvexHullHelper.h + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_CONVEX_HULL_HELPER_H +#define BIPEDAL_LOCOMOTION_PLANNERS_CONVEX_HULL_HELPER_H + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ +/** + * ConvexHullHelper is an helper class that simplifies the building of a convex hull given a set of + * points that belongs to \f$\mathbb{R} ^n\f$. The helper computes the vertex enumeration of the + * convex hull. Then it converts the V-representation of the convex polyhedron in the + * H-representation, i.e. it performs the convex-hull computation. The class can be used to retrieve + * the matrices and vectors that describes the Half space representation of the form: + * \f[ + * P = \left \{ x= \begin{bmatrix} x_1 & ... & x_d \end{bmatrix} ^ \top : b - A x \ge 0 \right \}. + * \f] + * Where \f$b\f$ is a m-vector and A is a m x n real matrix. + * @warning The ConvexHullHelper is based on the qhull library https://github.com/qhull/qhull + */ +class ConvexHullHelper +{ + struct Impl; + /** Private implementation */ + std::unique_ptr m_pimpl; + +public: + /** + * Constructor + */ + ConvexHullHelper(); + + /** + * Destructor + */ + ~ConvexHullHelper(); + + /** + * Given a set of points in \f$ \mathbb{R} ^ n\f$ the function build the convex hull. + * @warning the points must belong to the same vectorial space. + * @param points a vector of the points that describes the convex hull. + * @return true/false in case of success/failure. + */ + bool buildConvexHull(const std::vector& points); + + /** + * Return the \f$A\f$ constraint matrix, such that \f$ Ax \le b\f$ iff the point \f$x\f$ is in + * the convex hull. + * @warning Please call buildConvexHull before asking for \f$A\f$. If the convex hull has not be + * built yet a reference to a 0-size matrix is returned. + * @return the constraint matrix. + */ + const iDynTree::MatrixDynSize& getA() const; + + /** + * Return the \f$b\f$ constraint vector, such that \f$ Ax \le b\f$ iff the point \f$x\f$ is in + * the convex hull. + * @warning Please call buildConvexHull before asking for \f$b\f$. If the convex hull has not be + * built yet a reference to a 0-size vector is returned. + * @return the constraint vector. + */ + const iDynTree::VectorDynSize& getB() const; + + /** + * Check if a point belong to the convex hull (The frontier of the set is also included). + * @return true if the point belongs to the convex hull false otherwise. + */ + bool doesPointBelongToConvexHull(const iDynTree::VectorDynSize& point) const; +}; +} // namespace Planners +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONVEX_HULL_HELPER_H diff --git a/src/Planners/src/ConvexHullHelper.cpp b/src/Planners/src/ConvexHullHelper.cpp new file mode 100644 index 0000000000..98b35d0476 --- /dev/null +++ b/src/Planners/src/ConvexHullHelper.cpp @@ -0,0 +1,137 @@ +/** + * @file ConvexHullHelper.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include +#include + +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Planners; + +// Implementation +struct ConvexHullHelper::Impl +{ + iDynTree::VectorDynSize b; + iDynTree::MatrixDynSize A; +}; + +ConvexHullHelper::ConvexHullHelper() +{ + m_pimpl = std::make_unique(); + + m_pimpl->A.resize(0,0); + m_pimpl->b.resize(0); +} + +ConvexHullHelper::~ConvexHullHelper() +{ +} + +bool ConvexHullHelper::buildConvexHull(const std::vector& points) +{ + const std::size_t size = points.front().size(); + + // check if the size of the vectors are all the same + auto point = points.cbegin(); + // it is useless to check the size of the first vector + std::advance(point, 1); + for (; point < points.end(); std::advance(point, 1)) + { + if (size != point->size()) + { + std::cerr << "[ConvexHullHelper::buildConvexHull] All the vectors should belong to the " + "same vectorial space." + << std::endl; + return false; + } + } + + // the qhull object can be called only once + orgQhull::Qhull qhull; + + // it seems that the pointCoordinates element cannot be cleaned so a new point coordinates has to be instantiate + orgQhull::PointCoordinates pointCoordinates; + pointCoordinates.setDimension(size); + + std::vector allPoints; + for (const auto& point : points) + { + for (const auto& coordinate : point) + allPoints.push_back(coordinate); + } + pointCoordinates.append(allPoints); + + // find the convex hull + qhull.runQhull(pointCoordinates.comment().c_str(), + pointCoordinates.dimension(), + pointCoordinates.count(), + &*pointCoordinates.coordinates(), + "Qt"); + + auto facetList = qhull.facetList(); + const std::size_t numberOfFacet = facetList.count(); + + // resize matrix and vectors + m_pimpl->A.resize(numberOfFacet, size); + m_pimpl->b.resize(numberOfFacet); + + // fill the A matrix and the b vector + std::size_t row = 0; + for (const auto& facet : facetList) + { + // hyperplane contains d normal coefficients and an offset. The length of the normal is one. + // The hyperplane defines a halfspace. If V is a normal, b is an offset, and x is a point + // inside the convex hull, then V x + b < 0. + const auto hyperplane = facet.hyperplane(); + if (hyperplane.isValid()) + { + const auto coord = hyperplane.coordinates(); + for (std::size_t column = 0; column < size; column++) + { + m_pimpl->A(row, column) = coord[column]; + } + + m_pimpl->b[row] = -hyperplane.offset(); + row++; + } + } + + return true; +} + +const iDynTree::MatrixDynSize& ConvexHullHelper::getA() const +{ + return m_pimpl->A; +} + +const iDynTree::VectorDynSize& ConvexHullHelper::getB() const +{ + return m_pimpl->b; +} + +bool ConvexHullHelper::doesPointBelongToConvexHull(const iDynTree::VectorDynSize& point) const +{ + if (point.size() != m_pimpl->A.cols()) + { + std::cerr << "[ConvexHullHelper::doesPointBelongToConvexHull] Unexpected size of the point." + << std::endl; + return false; + } + + Eigen::VectorXd tmp = iDynTree::toEigen(m_pimpl->A) * iDynTree::toEigen(point); + + for(std::size_t i = 0; i < m_pimpl->b.size(); i++) + if(tmp[i] > m_pimpl->b[i]) + return false; + + return true; +} diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt index 2ec3baec52..c4e6e90538 100644 --- a/src/Planners/tests/CMakeLists.txt +++ b/src/Planners/tests/CMakeLists.txt @@ -15,3 +15,8 @@ add_bipedal_test( NAME ContactPhaseList SOURCES ContactPhaseListTest.cpp LINKS BipedalLocomotion::Contact) + +add_bipedal_test( + NAME ConvexHullHelper + SOURCES ConvexHullHelperTest.cpp + LINKS BipedalLocomotion::Contact) diff --git a/src/Planners/tests/ConvexHullHelperTest.cpp b/src/Planners/tests/ConvexHullHelperTest.cpp new file mode 100644 index 0000000000..bd056a9460 --- /dev/null +++ b/src/Planners/tests/ConvexHullHelperTest.cpp @@ -0,0 +1,67 @@ +/** + * @file ConvexHullHelperTest.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +#include + +using namespace BipedalLocomotion::Planners; + +TEST_CASE("Convex Hull helper") +{ + ConvexHullHelper helper; + + // initialize the points + std::vector points; + iDynTree::VectorDynSize p(3); + + p(0) = 0.6269; + p(1) = 0.7207; + p(2) = 0.3000; + points.push_back(p); + + p(0) = 0.5538; + p(1) = 0.6526; + points.push_back(p); + + p(0) = 0.6901; + p(1) = 0.5062; + points.push_back(p); + + p(0) = 0.7633; + p(1) = 0.5744; + points.push_back(p); + + p(0) = 0.8927; + p(1) = 0.7319; + p(2) = 0.2400; + points.push_back(p); + + p(0) = 0.8101; + p(1) = 0.6754; + points.push_back(p); + + p(0) = 0.9231; + p(1) = 0.5103; + points.push_back(p); + + p(0) = 1.0056; + p(1) = 0.5668; + points.push_back(p); + + REQUIRE(helper.buildConvexHull(points)); + + // check if the points belong to convex hull + for(const auto& point : points) + REQUIRE(helper.doesPointBelongToConvexHull(point)); + + // p = [0 0 0] does not belong to the convex hull + iDynTree::VectorDynSize pointOutsideConvexHull(3); + pointOutsideConvexHull.zero(); + REQUIRE_FALSE(helper.doesPointBelongToConvexHull(pointOutsideConvexHull)); +}