From c4ec6d6f41dfee915e1ee11a13d85abbbb2669d6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 30 Oct 2023 18:43:25 +0100 Subject: [PATCH] Implement GlobalCoPEvaluator in contact Component (#745) --- CHANGELOG.md | 2 + bindings/python/Contacts/CMakeLists.txt | 4 +- .../bindings/Contacts/GlobalCoPEvaluator.h | 26 +++ .../Contacts/src/GlobalCoPEvaluator.cpp | 41 ++++ bindings/python/Contacts/src/Module.cpp | 3 + src/Contacts/CMakeLists.txt | 2 + .../BipedalLocomotion/Contacts/Contact.h | 2 +- .../Contacts/GlobalCoPEvaluator.h | 137 ++++++++++++++ src/Contacts/src/GlobalCoPEvaluator.cpp | 178 ++++++++++++++++++ src/Contacts/tests/Contacts/CMakeLists.txt | 5 + .../tests/Contacts/GlobalCoPEvaluatorTest.cpp | 116 ++++++++++++ .../include/BipedalLocomotion/Math/Wrench.h | 25 +++ src/Math/tests/WrenchTest.cpp | 15 +- 13 files changed, 552 insertions(+), 4 deletions(-) create mode 100644 bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h create mode 100644 bindings/python/Contacts/src/GlobalCoPEvaluator.cpp create mode 100644 src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h create mode 100644 src/Contacts/src/GlobalCoPEvaluator.cpp create mode 100644 src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index bdeb9d9011..5f576e5a26 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,8 @@ All notable changes to this project are documented in this file. ## [Unreleased] - Added the ``DistanceTask`` and ``GravityTask`` to the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/717) - Add the possibility to control a subset of linear coordinates in `TSID::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/738) +- Implement `GlobalCoPEvaluator` in `Contacts` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745) +- Implement `Wrench::getLocalCoP()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745) ### Added - Add the possibility to control a subset of coordinates in `TSID::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/724, https://github.com/ami-iit/bipedal-locomotion-framework/pull/727) diff --git a/bindings/python/Contacts/CMakeLists.txt b/bindings/python/Contacts/CMakeLists.txt index d84fd38c6f..f7f2223258 100644 --- a/bindings/python/Contacts/CMakeLists.txt +++ b/bindings/python/Contacts/CMakeLists.txt @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::Contacts AND TARGET BipedalLocomotion::ContactDetec add_bipedal_locomotion_python_module( NAME ContactsBindings - SOURCES src/Contacts.cpp src/ContactDetectors.cpp src/Module.cpp - HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/Module.h + SOURCES src/Contacts.cpp src/ContactDetectors.cpp src/GlobalCoPEvaluator.cpp src/Module.cpp + HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/GlobalCoPEvaluator.h ${H_PREFIX}/Module.h LINK_LIBRARIES BipedalLocomotion::Contacts BipedalLocomotion::ContactDetectors TESTS tests/test_contact.py tests/test_fixed_foot_detector.py tests/test_schmitt_trigger_detector.py) diff --git a/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h b/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h new file mode 100644 index 0000000000..9774e4925f --- /dev/null +++ b/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h @@ -0,0 +1,26 @@ +/** + * @file GlobaCoPEvaluator.h + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H +#define BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace Contacts +{ + +void CreateGlobalCoPEvaluator(pybind11::module& module); + +} // namespace Contacts +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H diff --git a/bindings/python/Contacts/src/GlobalCoPEvaluator.cpp b/bindings/python/Contacts/src/GlobalCoPEvaluator.cpp new file mode 100644 index 0000000000..d1f1255bf5 --- /dev/null +++ b/bindings/python/Contacts/src/GlobalCoPEvaluator.cpp @@ -0,0 +1,41 @@ +/** + * @file GlobalCoPEvaluator.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace Contacts +{ + +void CreateGlobalCoPEvaluator(pybind11::module& module) +{ + namespace py = ::pybind11; + namespace Contacts = ::BipedalLocomotion::Contacts; + namespace System = ::BipedalLocomotion::System; + + BipedalLocomotion::bindings::System::CreateAdvanceable, // + Eigen::Vector3d>(module, + "GlobalCoPEvaluator"); + py::class_, // + Eigen::Vector3d>>(module, "GlobalCoPEvaluator") + .def(py::init()); +} + +} // namespace Contacts +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/Contacts/src/Module.cpp b/bindings/python/Contacts/src/Module.cpp index cba0d1cfbf..821fe86b93 100644 --- a/bindings/python/Contacts/src/Module.cpp +++ b/bindings/python/Contacts/src/Module.cpp @@ -9,6 +9,7 @@ #include #include +#include #include namespace BipedalLocomotion @@ -30,6 +31,8 @@ void CreateModule(pybind11::module& module) CreateContactDetector(module); CreateSchmittTriggerDetector(module); CreateFixedFootDetector(module); + + CreateGlobalCoPEvaluator(module); } } // namespace Contacts } // namespace bindings diff --git a/src/Contacts/CMakeLists.txt b/src/Contacts/CMakeLists.txt index 8ff9fa8666..13abc5578e 100644 --- a/src/Contacts/CMakeLists.txt +++ b/src/Contacts/CMakeLists.txt @@ -9,7 +9,9 @@ if (FRAMEWORK_COMPILE_Contact) NAME Contacts SUBDIRECTORIES tests/Contacts PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ContactListJsonParser.h + ${H_PREFIX}/GlobalCoPEvaluator.h SOURCES src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ContactListJsonParser.cpp + src/GlobalCoPEvaluator.cpp PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math BipedalLocomotion::TextLogging PRIVATE_LINK_LIBRARIES nlohmann_json::nlohmann_json INSTALLATION_FOLDER Contacts) diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h index d049b0e8e5..64d0e8764b 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h @@ -103,7 +103,6 @@ struct PlannedContact : ContactBase */ struct EstimatedContact : ContactBase { - /** * Instant at which the contact state was toggled. */ @@ -130,6 +129,7 @@ struct EstimatedContact : ContactBase */ struct ContactWrench : public ContactBase { + /**< Wrench acting on the contact */ BipedalLocomotion::Math::Wrenchd wrench{BipedalLocomotion::Math::Wrenchd::Zero()}; }; diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h b/src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h new file mode 100644 index 0000000000..6ad60aa19e --- /dev/null +++ b/src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h @@ -0,0 +1,137 @@ +/** + * @file GlobalCoPEvaluator.h + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H +#define BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H + +#include +#include + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace Contacts +{ + +/** + * GlobalCoPEvaluator is a class that computes the global CoP given a set of contact wrenches. + * Following P. Sardain and G. Bessonnet, "Forces acting on a biped robot. Center of + * pressure-zero moment point," in IEEE Transactions on Systems, Man, and Cybernetics, + * we defined the global CoP as the weighted average of the CoP of each contact, with the weight + * determined by the normal force of the contact. + * In detail, given \f$n\f$ contacts, with \f$n > 0\f$, the global CoP is defined as + * \f[ + * {}^{I} x_{\text{CoP}}^{\text{global}} = \frac{1}{\sum_{i=1}^{n} f_{z,i}} \sum_{i=1}^{n} ({}^I f_{z,i} \; \; x_{\text{CoP},i}) + * \f] + * where \f${}^I x_{\text{CoP}}^{\text{global}}\f$ is the global CoP expressed in the inertial frame + * \f$I\f$, \f${}^I f_{z,i}\f$ is the z component of the contact force expressed in the inertial + * frame \f$I\f$, \f${}^I x_{\text{CoP},i}\f$ is the CoP of the \f$i\f$-th contact expressed in the + * inertial frame \f$I\f$. + * The local CoP \f${}^I x_{\text{CoP},i}\f$ can be computed as follows + * \f[ + * {}^I x_{\text{CoP},i} = {}^I x_{\text{C},i} + {}^I R_{\text{C},i} \; {}^C x_{\text{CoP},i} + * \f] + * where \f${}^I x_{\text{C},i}\f$ is the contact position in the inertial frame \f$I\f$, + * \f${}^I R_{\text{C},i}\f$ is the rotation matrix from the inertial frame \f$I\f$ to the contact + * frame \f$C\f$, and \f${}^C x_{\text{CoP},i}\f$ is the CoP of the \f$i\f$-th contact expressed in + * the contact frame \f$C\f$ that can be computed as follows + * \f[ + * {}^C x_{\text{CoP},i} = \frac{1}{f_{z,i}} \begin{bmatrix} -\mu_{y,i} \\ \mu_{x,i} \\ 0 \end{bmatrix} + * \f] + * with \f$\mu_{x,i}\f$ and \f$\mu_{y,i}\f$ being the x and y component of the moment of the contact + * wrench expressed in the contact frame \f$C\f$. + * @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are + * expressed in the body frame (left trivialized). + * @note This class assumes that all the contacts are coplanar, that the normal of the common + * plane is the z axis, and that the contact force on the z axis is positive. + * @note In addition to evaluating the CoP, this class checks that at least one contact is active + * and that the CoP is not constant for a given number of iterations. + */ +class GlobalCoPEvaluator + : public BipedalLocomotion::System::Advanceable, + Eigen::Vector3d> +{ +public: + // clang-format off + /** + * Initialize the class + * @param handler pointer to a parameter handler + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:--------------------------:|:----------------:|:------------------------------------------------------------------------------------------------------------:|:---------:| + * | `minimum_normal_force` | `double` | Minimum normal force required to consider a contact active (in N) | Yes | + * | `cop_admissible_limits` | `vector` | 2D vector defining the admissible CoP region, comparing the local CoP x and y to the 1st and 2nd elements | Yes | + * | `constant_cop_tolerance` | `double` | Radius (in m) of a sphere used to considered if the global CoP is constant | Yes | + * | `constant_cop_max_counter` | `int` | Maximum number of samples after which a constant CoP generates an error. If negative the check is disabled. | Yes | + * @return true in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + // clang-format on + + /** + * Set the input to the class. + * @param input list containing the contact wrench + * @return true in case of success and false otherwise + * @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are + * expressed in the body frame (left trivialized). + */ + bool setInput(const std::initializer_list& input); + + /** + * Set the input to the class. + * @param input list containing the contact wrench + * @return true in case of success and false otherwise + * @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are + * expressed in the body frame (left trivialized). + */ + bool setInput(const std::vector& input) override; + + /** + * Compute the global CoP. + * @return true in case of success and false otherwise + */ + bool advance() override; + + /** + * Check if the CoP evaluated by the class is valid. + * @return true if valid, false otherwise. + */ + bool isOutputValid() const override; + + /** + * Get the global CoP + * @return a 3D vector containing the position of the global CoP expressed respect to the global + * (inertial) frame. + */ + const Eigen::Vector3d& getOutput() const override; + +private: + Eigen::Vector3d m_cop{Eigen::Vector3d::Zero()}; /**< Global CoP position in the inertial frame + */ + std::vector m_contacts; /**< Vector containing the contacts */ + bool m_isInitialized{false}; /**< True if the object is initialized */ + bool m_isOutputValid{false}; /**< True if the output is valid */ + + int m_constantCoPCounter{0}; /**< Counter used to store the number of constant CoP over time */ + + Eigen::Vector2d m_CoPAdmissibleLimits; /**< Vector containing the local admissible limits for + the CoP */ + int m_constantCoPMaxCounter{-1}; /**< Maximum number of samples after which a constant CoP + generates an error */ + double m_minimumNormalForce{0.0}; /**< Minimum required contact force */ + double m_constantCoPTolerance{0.0}; /**< Radius (in m) of a sphere used to considered if the + global CoP is constant */ +}; + +} // namespace Contacts +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H diff --git a/src/Contacts/src/GlobalCoPEvaluator.cpp b/src/Contacts/src/GlobalCoPEvaluator.cpp new file mode 100644 index 0000000000..8d18a51506 --- /dev/null +++ b/src/Contacts/src/GlobalCoPEvaluator.cpp @@ -0,0 +1,178 @@ +/** + * @file GlobalCoPEvaluator.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +using namespace BipedalLocomotion::Contacts; + +bool GlobalCoPEvaluator::initialize( + std::weak_ptr handler) +{ + constexpr auto logPrefix = "[GlobalCoPEvaluator::initialize]"; + + m_isInitialized = false; + m_isOutputValid = false; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("minimum_normal_force", m_minimumNormalForce)) + { + log()->error("{} Unable to retrieve the minimum normal force.", logPrefix); + return false; + } + + if (!ptr->getParameter("cop_admissible_limits", m_CoPAdmissibleLimits)) + { + log()->error("{} Unable to retrieve the CoP admissible limits.", logPrefix); + return false; + } + + if (!ptr->getParameter("constant_cop_tolerance", m_constantCoPTolerance)) + { + log()->error("{} Unable to retrieve the constant CoP tolerance.", logPrefix); + return false; + } + + if (!ptr->getParameter("constant_cop_max_counter", m_constantCoPMaxCounter)) + { + log()->error("{} Unable to retrieve the constant CoP max counter.", logPrefix); + return false; + } + + // set the CoP to zero + m_cop.setZero(); + m_constantCoPCounter = 0; + + m_isInitialized = true; + m_isOutputValid = false; + + return true; +} + +bool GlobalCoPEvaluator::setInput(const std::initializer_list& input) +{ + // save the contacts + m_contacts = input; + return true; +} + +bool GlobalCoPEvaluator::setInput(const std::vector& input) +{ + // save the contacts + m_contacts = input; + return true; +} + +bool GlobalCoPEvaluator::isOutputValid() const +{ + return m_isOutputValid; +} + +const Eigen::Vector3d& GlobalCoPEvaluator::getOutput() const +{ + return m_cop; +} + +bool GlobalCoPEvaluator::advance() +{ + constexpr auto logPrefix = "[GlobalCoPEvaluator::advance]"; + m_isOutputValid = false; + + if (!m_isInitialized) + { + log()->error("{} The object is not initialized. Please call " + "GlobalCoPEvaluator::initialize().", + logPrefix); + return false; + } + + Eigen::Vector3d globalCoP = Eigen::Vector3d::Zero(); + Eigen::Vector3d localCoP; + double totalForce = 0; + int numberOfActiveSupports = 0; + for (const auto& contact : m_contacts) + { + // check if the z component of the force is greater than a threshold + if (contact.wrench.force()[2] < m_minimumNormalForce) + { + continue; + } + + // the local CoP is defined since fz is greater than zero + localCoP = contact.wrench.getLocalCoP(); + + // check if the CoP is outside the admissible limits + if (std::abs(localCoP[0]) > m_CoPAdmissibleLimits[0] + || std::abs(localCoP[1]) > m_CoPAdmissibleLimits[1]) + { + continue; + } + + globalCoP += contact.wrench.force()[2] * contact.pose.act(localCoP); + totalForce += contact.wrench.force()[2]; + + numberOfActiveSupports++; + } + + if (numberOfActiveSupports == 0) + { + log()->error("{} Zero contacts are active, the CoP is not valid.", logPrefix); + return false; + } + + // compute the global CoP. + globalCoP /= totalForce; + + // at least two contacts are active and the counter is active + if (numberOfActiveSupports > 1 && m_constantCoPMaxCounter > 0) + { + const double CoPDifference = (globalCoP - m_cop).norm(); + + // check if the CoP is constant + if (CoPDifference < m_constantCoPTolerance) + { + // CoP considered constant so increase the counter + m_constantCoPCounter++; + } else + { + // CoP is not constant so reset the counter + m_constantCoPCounter = 0; + } + + if (m_constantCoPCounter >= m_constantCoPMaxCounter / 2) + { + log()->warn("{} The CoP was constant (in a {}m radius) for {} times.", + logPrefix, + m_constantCoPTolerance, + m_constantCoPCounter); + } + + if (m_constantCoPCounter >= m_constantCoPMaxCounter) + { + log()->error("{} The CoP was constant (in a {} m radius) for {} times.", + logPrefix, + m_constantCoPTolerance, + m_constantCoPCounter); + + return false; + } + } else + { + m_constantCoPCounter = 0; + } + + // the CoP is valid + m_cop = std::move(globalCoP); + m_isOutputValid = true; + return m_isOutputValid; +} diff --git a/src/Contacts/tests/Contacts/CMakeLists.txt b/src/Contacts/tests/Contacts/CMakeLists.txt index 862c619ea3..94e4d2afb0 100644 --- a/src/Contacts/tests/Contacts/CMakeLists.txt +++ b/src/Contacts/tests/Contacts/CMakeLists.txt @@ -12,4 +12,9 @@ add_bipedal_test( SOURCES ContactPhaseListTest.cpp LINKS BipedalLocomotion::Contacts) +add_bipedal_test( + NAME GlobalCoPEvaluator + SOURCES GlobalCoPEvaluatorTest.cpp + LINKS BipedalLocomotion::Contacts) + diff --git a/src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp b/src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp new file mode 100644 index 0000000000..b537d20afd --- /dev/null +++ b/src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp @@ -0,0 +1,116 @@ +/** + * @file GlobalCoPEvaluatorTest.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +// Catch2 +#include + +#include +#include + +using namespace BipedalLocomotion::Contacts; +using namespace BipedalLocomotion::ParametersHandler; + +TEST_CASE("GlobalCoPEvaluator") +{ + auto handler = std::make_shared(); + handler->setParameter("minimum_normal_force", 1.0); + handler->setParameter("cop_admissible_limits", std::vector{0.1, 0.1}); + handler->setParameter("constant_cop_tolerance", 0.01); + handler->setParameter("constant_cop_max_counter", 10); + + GlobalCoPEvaluator evaluator; + REQUIRE(evaluator.initialize(handler)); + REQUIRE_FALSE(evaluator.isOutputValid()); + + SECTION("Zero contact") + { + // this should fail + REQUIRE_FALSE(evaluator.advance()); + REQUIRE_FALSE(evaluator.isOutputValid()); + } + + SECTION("One contact but invalid") + { + ContactWrench contact; + contact.pose.setIdentity(); + contact.wrench.setZero(); + + // this is lower than the threshold + contact.wrench.force()(2) = 0.01; + + REQUIRE(evaluator.setInput({contact})); + + // this should fail + REQUIRE_FALSE(evaluator.advance()); + REQUIRE_FALSE(evaluator.isOutputValid()); + } + + SECTION("One valid contact") + { + Eigen::Vector3d translation; + translation[0] = 2.1; + translation[1] = 1.1; + translation[2] = 0.9; + + ContactWrench contact; + contact.pose.setIdentity(); + contact.pose.translation(translation); + contact.wrench.setZero(); + + // this is lower than the threshold + contact.wrench.force()(2) = 10.0; + + REQUIRE(evaluator.setInput({contact})); + + // this should fail + REQUIRE(evaluator.advance()); + REQUIRE(evaluator.isOutputValid()); + + REQUIRE(translation.isApprox(evaluator.getOutput())); + } + + SECTION("Two valid contacts") + { + Eigen::Vector3d translation1; + translation1[0] = 2.1; + translation1[1] = 1.1; + translation1[2] = 0.9; + + ContactWrench contact1; + contact1.pose.setIdentity(); + contact1.pose.translation(translation1); + contact1.wrench.setZero(); + + // this is lower than the threshold + contact1.wrench.force()(2) = 10.0; + + Eigen::Vector3d translation2; + translation2[0] = 2.1; + translation2[1] = -1.1; + translation2[2] = 0.9; + + ContactWrench contact2; + contact2.pose.setIdentity(); + contact2.pose.translation(translation2); + contact2.wrench.setZero(); + + // this is lower than the threshold + contact2.wrench.force()(2) = 10.0; + + REQUIRE(evaluator.setInput({contact1, contact2})); + + // this should fail + REQUIRE(evaluator.advance()); + REQUIRE(evaluator.isOutputValid()); + + // compute the average translation + Eigen::Vector3d averageTranslation = (translation1 + translation2) / 2.0; + REQUIRE(averageTranslation.isApprox(evaluator.getOutput())); + } +} diff --git a/src/Math/include/BipedalLocomotion/Math/Wrench.h b/src/Math/include/BipedalLocomotion/Math/Wrench.h index 655e8d8f1d..0009437ce7 100644 --- a/src/Math/include/BipedalLocomotion/Math/Wrench.h +++ b/src/Math/include/BipedalLocomotion/Math/Wrench.h @@ -66,6 +66,31 @@ template class Wrench : public Eigen::Matrix { return this->Base::template tail<3>(); } + + /** + * Get the local CoP associated with the wrench. + * The center of pressure (CoP) is defined is a dynamic point defined in between two + * bodies in contact. The CoP is a local quantity defined from interaction forces at the contact + * surface. + * @note A more detailed explanation can be found in: P. Sardain and G. Bessonnet, "Forces + * acting on a biped robot. Center of pressure-zero moment point," in IEEE Transactions on + * Systems, Man, and Cybernetics - Part A: Systems and Humans, vol. 34, no. 5, pp. 630-637, + * Sept. 2004, doi: 10.1109/TSMCA.2004.832811. An interested reader can also check the following + * https://scaron.info/robotics/zero-tilting-moment-point.html + * @warning This function assumes that the wrench is expressed in the body frame. I.e., a frame + * attached to the body belonging to the contact area and the z-component of the contact force + * is the resultant pressure force. + * @warning This function assumes that the z-component of the contact force is non-zero. + * @return the CoP expressed in the local frame + */ + Eigen::Matrix getLocalCoP() const + { + Eigen::Matrix localCoP; + localCoP[0] = -this->torque()[1] / this->force()[2]; + localCoP[1] = this->torque()[0] / this->force()[2]; + localCoP[2] = Scalar(0); + return localCoP; + } }; /** diff --git a/src/Math/tests/WrenchTest.cpp b/src/Math/tests/WrenchTest.cpp index cc30cac54a..94b43b714b 100644 --- a/src/Math/tests/WrenchTest.cpp +++ b/src/Math/tests/WrenchTest.cpp @@ -17,7 +17,7 @@ using namespace BipedalLocomotion::Math; TEST_CASE("Wrench test") { - const Wrenchd wrench = Wrenchd::Random(); + Wrenchd wrench = Wrenchd::Random(); SECTION("Get force and torque") { @@ -56,4 +56,17 @@ TEST_CASE("Wrench test") REQUIRE(rotatedWrench.force().isApprox(rotation.rotation() * wrench.force())); REQUIRE(rotatedWrench.torque().isApprox(rotation.rotation() * wrench.torque())); } + + SECTION("Local CoP") + { + // ensure that the force is positive + wrench.force()[2] = 10; + + Eigen::Vector3d expectedCoP; + expectedCoP[0] = -wrench.torque()[1] / wrench.force()[2]; + expectedCoP[1] = wrench.torque()[0] / wrench.force()[2]; + expectedCoP[2] = 0; + + REQUIRE(wrench.getLocalCoP().isApprox(expectedCoP)); + } }