diff --git a/bindings/python/Contacts/CMakeLists.txt b/bindings/python/Contacts/CMakeLists.txt index b99ff794db..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/GlobalZMPEvaluator.cpp src/Module.cpp - HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/GlobalZMPEvaluator.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/GlobalZMPEvaluator.h b/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h similarity index 58% rename from bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h rename to bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h index 0d4167a54a..9774e4925f 100644 --- a/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h +++ b/bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h @@ -1,12 +1,12 @@ /** - * @file GlobaZMPEvaluator.h + * @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_ZMP_EVALUATOR_H -#define BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_ZMP_EVALUATOR_H +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H +#define BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H #include @@ -17,10 +17,10 @@ namespace bindings namespace Contacts { -void CreateGlobalZMPEvaluator(pybind11::module& module); +void CreateGlobalCoPEvaluator(pybind11::module& module); } // namespace Contacts } // namespace bindings } // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_ZMP_EVALUATOR_H +#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_COP_EVALUATOR_H diff --git a/bindings/python/Contacts/src/Contacts.cpp b/bindings/python/Contacts/src/Contacts.cpp index 05912f31f1..d26832aa35 100644 --- a/bindings/python/Contacts/src/Contacts.cpp +++ b/bindings/python/Contacts/src/Contacts.cpp @@ -85,9 +85,7 @@ void CreateContact(pybind11::module& module) py::class_(module, "ContactWrench") .def(py::init()) - .def_readwrite("wrench", &ContactWrench::wrench) - .def("get_local_zmp", &ContactWrench::getLocalZMP) - .def("get_global_zmp", &ContactWrench::getGlobalZMP); + .def_readwrite("wrench", &ContactWrench::wrench); py::class_(module, "DiscreteGeometryContact") .def(py::init()) .def_readwrite("corners", &DiscreteGeometryContact::corners); diff --git a/bindings/python/Contacts/src/GlobalZMPEvaluator.cpp b/bindings/python/Contacts/src/GlobalZMPEvaluator.cpp index 2ef23f3550..d1f1255bf5 100644 --- a/bindings/python/Contacts/src/GlobalZMPEvaluator.cpp +++ b/bindings/python/Contacts/src/GlobalZMPEvaluator.cpp @@ -1,5 +1,5 @@ /** - * @file GlobalZMPEvaluator.cpp + * @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. @@ -9,9 +9,9 @@ #include #include -#include +#include -#include +#include #include namespace BipedalLocomotion @@ -21,7 +21,7 @@ namespace bindings namespace Contacts { -void CreateGlobalZMPEvaluator(pybind11::module& module) +void CreateGlobalCoPEvaluator(pybind11::module& module) { namespace py = ::pybind11; namespace Contacts = ::BipedalLocomotion::Contacts; @@ -29,10 +29,10 @@ void CreateGlobalZMPEvaluator(pybind11::module& module) BipedalLocomotion::bindings::System::CreateAdvanceable, // Eigen::Vector3d>(module, - "GlobalZMPEvaluator"); - py::class_, // - Eigen::Vector3d>>(module, "GlobalZMPEvaluator") + Eigen::Vector3d>>(module, "GlobalCoPEvaluator") .def(py::init()); } diff --git a/bindings/python/Contacts/src/Module.cpp b/bindings/python/Contacts/src/Module.cpp index 6fcab84eb0..821fe86b93 100644 --- a/bindings/python/Contacts/src/Module.cpp +++ b/bindings/python/Contacts/src/Module.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include namespace BipedalLocomotion @@ -32,7 +32,7 @@ void CreateModule(pybind11::module& module) CreateSchmittTriggerDetector(module); CreateFixedFootDetector(module); - CreateGlobalZMPEvaluator(module); + CreateGlobalCoPEvaluator(module); } } // namespace Contacts } // namespace bindings diff --git a/src/Contacts/CMakeLists.txt b/src/Contacts/CMakeLists.txt index ffd3177c69..13abc5578e 100644 --- a/src/Contacts/CMakeLists.txt +++ b/src/Contacts/CMakeLists.txt @@ -9,9 +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}/GlobalZMPEvaluator.h + ${H_PREFIX}/GlobalCoPEvaluator.h SOURCES src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ContactListJsonParser.cpp - src/GlobalZMPEvaluator.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 3f9122ec2b..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. */ @@ -132,34 +131,6 @@ struct ContactWrench : public ContactBase { /**< Wrench acting on the contact */ BipedalLocomotion::Math::Wrenchd wrench{BipedalLocomotion::Math::Wrenchd::Zero()}; - - /** - * Get the local ZMP associated with the wrench. - * @note The ZMP (Zero-Moment Point) is defined to be a point on the ground at which the - * tangential component of the moment generated by the ground reaction force/moment becomes - * zero (K. Harada, S. Kajita, K. Kaneko, and H. Hirukawa, “ZMP analysis for arm/leg - * coordination,” in Intelligent Robots and Systems, 2003. (IROS 2003). Proc. IEEE/RSJ Int. - * Conf. on, vol. 1, 2003, pp.) - * @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. - * @warning This function assumes that the z-component of the contact force is non-zero. - * @return the ZMP expressed in the local frame - */ - Eigen::Vector3d getLocalZMP() const; - - /** - * Get the global ZMP associated with the wrench. - * @note The ZMP (Zero-Moment Point) is defined to be a point on the ground at which the - * tangential component of the moment generated by the ground reaction force/moment becomes - * zero (K. Harada, S. Kajita, K. Kaneko, and H. Hirukawa, “ZMP analysis for arm/leg - * coordination,” in Intelligent Robots and Systems, 2003. (IROS 2003). Proc. IEEE/RSJ Int. - * Conf. on, vol. 1, 2003, pp.) - * @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. - * @warning This function assumes that the z-component of the contact force is non-zero. - * @return the ZMP expressed in the global (i.e., inertial) frame - */ - Eigen::Vector3d getGlobalZMP() const; }; using EstimatedLandmark = EstimatedContact; diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/GlobalZMPEvaluator.h b/src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h similarity index 64% rename from src/Contacts/include/BipedalLocomotion/Contacts/GlobalZMPEvaluator.h rename to src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h index da6b42fdf9..363ed8fc04 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/GlobalZMPEvaluator.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/GlobalCoPEvaluator.h @@ -1,12 +1,12 @@ /** - * @file GlobalZMPEvaluator.h + * @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_ZMP_EVALUATOR_H -#define BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_ZMP_EVALUATOR_H +#ifndef BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H +#define BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H #include #include @@ -22,17 +22,17 @@ namespace Contacts { /** - * GlobalZMPEvaluator is a class that computes the global ZMP given a set of contact wrenches. - * The ZMP is computed as the weighted average of the ZMP of each contact, with the weight + * 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. * @note This class assumes that the contact wrenches stored in Contacts::ContactWrench list are * expressed in the body frame (left trivialized). - * @note In addition to evaluating the ZMP, this class checks that at least one contact is active - * and that the ZMP is not constant for a given number of iterations. Moreover, the class verifies - * that the local ZMP belongs to a given region; otherwise, the associated local ZMP is not - * considered in the computation of the global ZMP. + * @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 GlobalZMPEvaluator +class GlobalCoPEvaluator : public BipedalLocomotion::System::Advanceable, Eigen::Vector3d> { @@ -45,9 +45,9 @@ class GlobalZMPEvaluator * | Parameter Name | Type | Description | Mandatory | * |:--------------------------:|:----------------:|:-----------------------------------------------------------------------------------------:|:---------:| * | `minimum_normal_force` | `double` | Minimum normal force required to consider a contact active (in N) | Yes | - * | `zmp_admissible_limits` | `vector` | 2D vector defines ZMP region, comparing absolute ZMP x and y to the 1st and 2nd elements | Yes | - * | `constant_zmp_tolerance` | `double` | Radius (in m) of a sphere used to considered if the global ZMP is constant | Yes | - * | `constant_zmp_max_counter` | `int` | Maximum number of samples after which a constant ZMP generates an error | Yes | + * | `cop_admissible_limits` | `vector` | 2D vector defines CoP region, comparing absolute 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 | Yes | * @return true in case of success, false otherwise. */ bool initialize(std::weak_ptr handler) override; @@ -72,43 +72,43 @@ class GlobalZMPEvaluator bool setInput(const std::vector& input) override; /** - * Compute the global ZMP. + * Compute the global CoP. * @return true in case of success and false otherwise */ bool advance() override; /** - * Check if the ZMP evaluated by the class is valid. + * Check if the CoP evaluated by the class is valid. * @return true if valid, false otherwise. */ bool isOutputValid() const override; /** - * Get the global ZMP - * @return a 3D vector containing the position of the global ZMP expressed respect to the global + * 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_zmp{Eigen::Vector3d::Zero()}; /**< Global ZMP position in the inertial frame + Eigen::Vector3d m_cop{Eigen::Vector3d::Zero()}; /**< Global CoP position in the inertial frame */ - std::vector m_contacts; /**< */ + 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_constantZMPCounter{0}; /**< Counter used to store the number of constant ZMP over time */ + int m_constantCoPCounter{0}; /**< Counter used to store the number of constant CoP over time */ - Eigen::Vector2d m_zmpAdmissibleLimits; /**< Vector containing the local admissible limits for - the ZMP */ - int m_constantZMPMaxCounter{-1}; /**< Maximum number of samples after which a constant ZMP + 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_constantZMPTolerance{0.0}; /**< Radius (in m) of a sphere used to considered if the - global ZMP is constant */ + 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_ZMP_EVALUATOR_H +#endif // BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_COP_EVALUATOR_H diff --git a/src/Contacts/src/Contact.cpp b/src/Contacts/src/Contact.cpp index 32e2d7d71c..9f75e03f8b 100644 --- a/src/Contacts/src/Contact.cpp +++ b/src/Contacts/src/Contact.cpp @@ -38,18 +38,3 @@ void EstimatedContact::setContactStateStamped(const std::pairgetLocalZMP(); - return this->pose.act(zmp); -} diff --git a/src/Contacts/src/GlobalCoPEvaluator.cpp b/src/Contacts/src/GlobalCoPEvaluator.cpp new file mode 100644 index 0000000000..6d7895d911 --- /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 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 = globalCoP; + m_isOutputValid = true; + return m_isOutputValid; +} diff --git a/src/Contacts/src/GlobalZMPEvaluator.cpp b/src/Contacts/src/GlobalZMPEvaluator.cpp deleted file mode 100644 index 2df8a7da59..0000000000 --- a/src/Contacts/src/GlobalZMPEvaluator.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/** - * @file GlobalZMPEvaluator.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 GlobalZMPEvaluator::initialize( - std::weak_ptr handler) -{ - constexpr auto logPrefix = "[GlobalZMPEvaluator::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("zmp_admissible_limits", m_zmpAdmissibleLimits)) - { - log()->error("{} Unable to retrieve the zmp admissible limits.", logPrefix); - return false; - } - - if (!ptr->getParameter("constant_zmp_tolerance", m_constantZMPTolerance)) - { - log()->error("{} Unable to retrieve the constant zmp tolerance.", logPrefix); - return false; - } - - if (!ptr->getParameter("constant_zmp_max_counter", m_constantZMPMaxCounter)) - { - log()->error("{} Unable to retrieve the constant zmp max counter.", logPrefix); - return false; - } - - // set the ZMP to zero - m_zmp.setZero(); - m_constantZMPCounter = 0; - - m_isInitialized = true; - m_isOutputValid = false; - - return true; -} - -bool GlobalZMPEvaluator::setInput(const std::initializer_list& input) -{ - // save the contacts - m_contacts = input; - return true; -} - -bool GlobalZMPEvaluator::setInput(const std::vector& input) -{ - // save the contacts - m_contacts = input; - return true; -} - -bool GlobalZMPEvaluator::isOutputValid() const -{ - return m_isOutputValid; -} - -const Eigen::Vector3d& GlobalZMPEvaluator::getOutput() const -{ - return m_zmp; -} - -bool GlobalZMPEvaluator::advance() -{ - constexpr auto logPrefix = "[GlobalZMPEvaluator::advance]"; - m_isOutputValid = false; - - if (!m_isInitialized) - { - log()->error("{} The object is not initialized. Please call " - "GlobalZMPEvaluator::initialize().", - logPrefix); - return false; - } - - Eigen::Vector3d globalZMP; - Eigen::Vector3d localZMP; - 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 ZMP is defined since fz is greater than zero - localZMP = contact.getLocalZMP(); - - // check if the zmp is outside the admissible limits - if (std::abs(localZMP[0]) > m_zmpAdmissibleLimits[0] - || std::abs(localZMP[1]) > m_zmpAdmissibleLimits[1]) - { - continue; - } - - globalZMP += contact.wrench.force()[2] * contact.pose.act(localZMP); - totalForce += contact.wrench.force()[2]; - - numberOfActiveSupports++; - } - - if (numberOfActiveSupports == 0) - { - log()->error("{} Zero contacts are active the ZMP is not valid.", logPrefix); - return false; - } - - // compute the global ZMP. - globalZMP /= totalForce; - - // at least two contacts are active and the counter is active - if (numberOfActiveSupports > 1 && m_constantZMPMaxCounter > 0) - { - const double zmpDifference = (globalZMP - m_zmp).norm(); - - // check if the ZMP is constant - if (zmpDifference < m_constantZMPTolerance) - { - // ZMP considered constant so increase the counter - m_constantZMPCounter++; - } else - { - // ZMP is not constant so reset the counter - m_constantZMPCounter = 0; - } - - if (m_constantZMPCounter >= m_constantZMPMaxCounter / 2) - { - log()->warn("{} The ZMP was constant (in a {}m radius) for {} times.", - logPrefix, - m_constantZMPTolerance, - m_constantZMPCounter); - } - - if (m_constantZMPCounter >= m_constantZMPMaxCounter) - { - log()->error("{} The ZMP was constant (in a {} m radius) for {} times.", - logPrefix, - m_constantZMPTolerance, - m_constantZMPCounter); - - return false; - } - } else - { - m_constantZMPCounter = 0; - } - - // the zmp is valid - m_zmp = std::move(globalZMP); - m_isOutputValid = true; - return m_isOutputValid; -} diff --git a/src/Contacts/tests/Contacts/CMakeLists.txt b/src/Contacts/tests/Contacts/CMakeLists.txt index 0016bc5e3d..94e4d2afb0 100644 --- a/src/Contacts/tests/Contacts/CMakeLists.txt +++ b/src/Contacts/tests/Contacts/CMakeLists.txt @@ -12,14 +12,9 @@ add_bipedal_test( SOURCES ContactPhaseListTest.cpp LINKS BipedalLocomotion::Contacts) - add_bipedal_test( - NAME Contact - SOURCES ContactTest.cpp - LINKS BipedalLocomotion::Contacts) - add_bipedal_test( - NAME GlobalZMPEvaluator - SOURCES GlobalZMPEvaluatorTest.cpp + NAME GlobalCoPEvaluator + SOURCES GlobalCoPEvaluatorTest.cpp LINKS BipedalLocomotion::Contacts) diff --git a/src/Contacts/tests/Contacts/ContactTest.cpp b/src/Contacts/tests/Contacts/ContactTest.cpp deleted file mode 100644 index f06e09ed88..0000000000 --- a/src/Contacts/tests/Contacts/ContactTest.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/** - * @file Contact.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 - -// manif -#include - -#include - -using namespace BipedalLocomotion::Contacts; - -TEST_CASE("ContactWrench") -{ - ContactWrench contact; - contact.pose = manif::SE3d::Random(); - contact.wrench.force() = Eigen::Vector3d::Random(); - contact.wrench.force()[2] = 10; - contact.wrench.torque() = Eigen::Vector3d::Random(); - - Eigen::Vector3d expectedZMP; - expectedZMP[0] = -contact.wrench.torque()[1] / contact.wrench.force()[2]; - expectedZMP[1] = contact.wrench.torque()[0] / contact.wrench.force()[2]; - expectedZMP[2] = 0; - - REQUIRE(contact.getLocalZMP().isApprox(expectedZMP)); - - expectedZMP = contact.pose.act(expectedZMP); - REQUIRE(contact.getGlobalZMP().isApprox(expectedZMP)); -} diff --git a/src/Contacts/tests/Contacts/GlobalZMPEvaluatorTest.cpp b/src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp similarity index 89% rename from src/Contacts/tests/Contacts/GlobalZMPEvaluatorTest.cpp rename to src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp index a85ab8d251..b537d20afd 100644 --- a/src/Contacts/tests/Contacts/GlobalZMPEvaluatorTest.cpp +++ b/src/Contacts/tests/Contacts/GlobalCoPEvaluatorTest.cpp @@ -1,5 +1,5 @@ /** - * @file GlobalZMPEvaluatorTest.cpp + * @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. @@ -10,21 +10,21 @@ // Catch2 #include -#include +#include #include using namespace BipedalLocomotion::Contacts; using namespace BipedalLocomotion::ParametersHandler; -TEST_CASE("GlobalZMPEvaluator") +TEST_CASE("GlobalCoPEvaluator") { auto handler = std::make_shared(); handler->setParameter("minimum_normal_force", 1.0); - handler->setParameter("zmp_admissible_limits", std::vector{0.1, 0.1}); - handler->setParameter("constant_zmp_tolerance", 0.01); - handler->setParameter("constant_zmp_max_counter", 10); + 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); - GlobalZMPEvaluator evaluator; + GlobalCoPEvaluator evaluator; REQUIRE(evaluator.initialize(handler)); REQUIRE_FALSE(evaluator.isOutputValid()); diff --git a/src/Math/include/BipedalLocomotion/Math/Wrench.h b/src/Math/include/BipedalLocomotion/Math/Wrench.h index 655e8d8f1d..3da8273888 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 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..fbddc81b81 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 ZMP") + { + // ensure that the force is positive + wrench.force()[2] = 10; + + Eigen::Vector3d expectedZMP; + expectedZMP[0] = -wrench.torque()[1] / wrench.force()[2]; + expectedZMP[1] = wrench.torque()[0] / wrench.force()[2]; + expectedZMP[2] = 0; + + REQUIRE(wrench.getLocalCoP().isApprox(expectedZMP)); + } }