Rename GlobalZMPEvaluator in GlobalCoPEvaluator

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)
 

bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h → 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 <pybind11/pybind11.h>
 
@@ -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

bindings/python/Contacts/src/Contacts.cpp

@@ -85,9 +85,7 @@ void CreateContact(pybind11::module& module)
 
     py::class_<ContactWrench, ContactBase>(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_<DiscreteGeometryContact, ContactBase>(module, "DiscreteGeometryContact")
         .def(py::init())
         .def_readwrite("corners", &DiscreteGeometryContact::corners);

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 <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include <BipedalLocomotion/Contacts/GlobalZMPEvaluator.h>
+#include <BipedalLocomotion/Contacts/GlobalCoPEvaluator.h>
 
-#include <BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h>
+#include <BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h>
 #include <BipedalLocomotion/bindings/System/Advanceable.h>
 
 namespace BipedalLocomotion
@@ -21,18 +21,18 @@ namespace bindings
 namespace Contacts
 {
 
-void CreateGlobalZMPEvaluator(pybind11::module& module)
+void CreateGlobalCoPEvaluator(pybind11::module& module)
 {
     namespace py = ::pybind11;
     namespace Contacts = ::BipedalLocomotion::Contacts;
     namespace System = ::BipedalLocomotion::System;
 
     BipedalLocomotion::bindings::System::CreateAdvanceable<std::vector<Contacts::ContactWrench>, //
                                                            Eigen::Vector3d>(module,
-                                                                            "GlobalZMPEvaluator");
-    py::class_<Contacts::GlobalZMPEvaluator,
+                                                                            "GlobalCoPEvaluator");
+    py::class_<Contacts::GlobalCoPEvaluator,
                System::Advanceable<std::vector<Contacts::ContactWrench>, //
-                                   Eigen::Vector3d>>(module, "GlobalZMPEvaluator")
+                                   Eigen::Vector3d>>(module, "GlobalCoPEvaluator")
         .def(py::init());
 }
 

bindings/python/Contacts/src/Module.cpp

@@ -9,7 +9,7 @@
 
 #include <BipedalLocomotion/bindings/Contacts/ContactDetectors.h>
 #include <BipedalLocomotion/bindings/Contacts/Contacts.h>
-#include <BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h>
+#include <BipedalLocomotion/bindings/Contacts/GlobalCoPEvaluator.h>
 #include <BipedalLocomotion/bindings/Contacts/Module.h>
 
 namespace BipedalLocomotion
@@ -32,7 +32,7 @@ void CreateModule(pybind11::module& module)
     CreateSchmittTriggerDetector(module);
     CreateFixedFootDetector(module);
 
-    CreateGlobalZMPEvaluator(module);
+    CreateGlobalCoPEvaluator(module);
 }
 } // namespace Contacts
 } // namespace bindings

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)

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;

src/Contacts/include/BipedalLocomotion/Contacts/GlobalZMPEvaluator.h → 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 <initializer_list>
 #include <vector>
@@ -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<std::vector<Contacts::ContactWrench>,
                                                     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<double>` |  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<double>` |  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<const ParametersHandler::IParametersHandler> handler) override;
@@ -72,43 +72,43 @@ class GlobalZMPEvaluator
     bool setInput(const std::vector<Contacts::ContactWrench>& 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<Contacts::ContactWrench> m_contacts; /**< */
+    std::vector<Contacts::ContactWrench> 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

src/Contacts/src/Contact.cpp

@@ -38,18 +38,3 @@ void EstimatedContact::setContactStateStamped(const std::pair<bool, std::chrono:
     isActive = pair.first;
     switchTime = pair.second;
 }
-
-Eigen::Vector3d ContactWrench::getLocalZMP() const
-{
-    Eigen::Vector3d zmp;
-    zmp(0) = -wrench.torque()(1) / wrench.force()(2);
-    zmp(1) = wrench.torque()(0) / wrench.force()(2);
-    zmp(2) = 0.0;
-    return zmp;
-}
-
-Eigen::Vector3d ContactWrench::getGlobalZMP() const
-{
-    const Eigen::Vector3d zmp = this->getLocalZMP();
-    return this->pose.act(zmp);
-}