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 → bindings/python/Contacts/src/GlobalCoPEvaluator.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);
-}