-
Notifications
You must be signed in to change notification settings - Fork 46
Implement the ConvexHullHelper class #51
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Merged
Merged
Changes from all commits
Commits
Show all changes
15 commits
Select commit
Hold shift + click to select a range
817d0b6
Add Qhull depency for compiling the Planners component
GiulioRomualdi cdbec22
Implement the first version of the ConvexHullHelper class
GiulioRomualdi dbbc6a6
Implement a test for the ConvexHullHelper
GiulioRomualdi ccff481
Enable the compilation of the ConvexHullHelper class
GiulioRomualdi 79183bc
Enable to compilation of the ConvexHullHelperTest
GiulioRomualdi b32dc2e
Add qhull library to GitHub action workflow
GiulioRomualdi eecabbf
Update the Readme.md
GiulioRomualdi 02524c6
Install qhull from brew on macOS in ci.yml
GiulioRomualdi 7d5aeee
Remove qhull compilation for windows on ci.yml
GiulioRomualdi 63e153e
Link the qhullstatic_r instead of the qhull_r in the Planner Component
GiulioRomualdi 1ff0ce0
Restore the compilation of Qhull on Windows CI
GiulioRomualdi 58b1d2a
Enable the Position Independent Code (PIC) option for building qhull …
GiulioRomualdi d07e6f7
Avoid undefined beheviour in ConvexHullHelperTest::getA() and ConvexH…
GiulioRomualdi 509d1ab
Fix typos in ConvexHullHelper::doesPointBelongToConvexHull() function
GiulioRomualdi f1bdb21
Fix typos in ConvexHullHelper::buildConvexHull() function
GiulioRomualdi File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
85 changes: 85 additions & 0 deletions
85
src/Planners/include/BipedalLocomotion/Planners/ConvexHullHelper.h
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <iDynTree/Core/MatrixDynSize.h> | ||
| #include <iDynTree/Core/VectorDynSize.h> | ||
|
|
||
| #include <vector> | ||
| #include <memory> | ||
|
|
||
| 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<Impl> 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<iDynTree::VectorDynSize>& 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. | ||
| */ | ||
|
GiulioRomualdi marked this conversation as resolved.
|
||
| bool doesPointBelongToConvexHull(const iDynTree::VectorDynSize& point) const; | ||
| }; | ||
| } // namespace Planners | ||
| } // namespace BipedalLocomotion | ||
|
|
||
| #endif // BIPEDAL_LOCOMOTION_PLANNERS_CONVEX_HULL_HELPER_H | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <libqhullcpp/PointCoordinates.h> | ||
| #include <libqhullcpp/Qhull.h> | ||
| #include <libqhullcpp/QhullFacetList.h> | ||
|
|
||
| #include <iDynTree/Core/MatrixDynSize.h> | ||
| #include <iDynTree/Core/VectorDynSize.h> | ||
| #include <iDynTree/Core/EigenHelpers.h> | ||
|
|
||
| #include <BipedalLocomotion/Planners/ConvexHullHelper.h> | ||
|
|
||
| using namespace BipedalLocomotion::Planners; | ||
|
|
||
| // Implementation | ||
| struct ConvexHullHelper::Impl | ||
| { | ||
| iDynTree::VectorDynSize b; | ||
| iDynTree::MatrixDynSize A; | ||
| }; | ||
|
|
||
| ConvexHullHelper::ConvexHullHelper() | ||
| { | ||
| m_pimpl = std::make_unique<Impl>(); | ||
|
|
||
| m_pimpl->A.resize(0,0); | ||
| m_pimpl->b.resize(0); | ||
| } | ||
|
|
||
| ConvexHullHelper::~ConvexHullHelper() | ||
| { | ||
| } | ||
|
|
||
| bool ConvexHullHelper::buildConvexHull(const std::vector<iDynTree::VectorDynSize>& 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<double> 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; | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.