Implement GlobalZMPEvaluator

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/GlobalZMPEvaluator.cpp src/Module.cpp
+    HEADERS ${H_PREFIX}/Contacts.h ${H_PREFIX}/ContactDetectors.h ${H_PREFIX}/GlobalZMPEvaluator.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

@@ -0,0 +1,26 @@
+/**
+ * @file GlobaZMPEvaluator.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
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace Contacts
+{
+
+void CreateGlobalZMPEvaluator(pybind11::module& module);
+
+} // namespace Contacts
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_CONTACTS_GLOBAL_ZMP_EVALUATOR_H

bindings/python/Contacts/src/GlobalZMPEvaluator.cpp

@@ -0,0 +1,41 @@
+/**
+ * @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 <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/Contacts/GlobalZMPEvaluator.h>
+
+#include <BipedalLocomotion/bindings/Contacts/GlobalZMPEvaluator.h>
+#include <BipedalLocomotion/bindings/System/Advanceable.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace Contacts
+{
+
+void CreateGlobalZMPEvaluator(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,
+               System::Advanceable<std::vector<Contacts::ContactWrench>, //
+                                   Eigen::Vector3d>>(module, "GlobalZMPEvaluator")
+        .def(py::init());
+}
+
+} // namespace Contacts
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/Contacts/src/Module.cpp

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

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}/GlobalZMPEvaluator.h
     SOURCES               src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ContactListJsonParser.cpp
+                          src/GlobalZMPEvaluator.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/GlobalZMPEvaluator.h

@@ -0,0 +1,114 @@
+/**
+ * @file GlobalZMPEvaluator.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
+
+#include <initializer_list>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/Contacts/Contact.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+namespace BipedalLocomotion
+{
+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
+ * 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.
+ */
+class GlobalZMPEvaluator
+    : public BipedalLocomotion::System::Advanceable<std::vector<Contacts::ContactWrench>,
+                                                    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    |
+     * |   `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    |
+     * @return true in case of success, false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> 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<Contacts::ContactWrench>& 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<Contacts::ContactWrench>& input) override;
+
+    /**
+     * Compute the global ZMP.
+     * @return true in case of success and false otherwise
+     */
+    bool advance() override;
+
+    /**
+     * Check if the ZMP 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
+     * (inertial) frame.
+     */
+    const Eigen::Vector3d& getOutput() const override;
+
+private:
+    Eigen::Vector3d m_zmp{Eigen::Vector3d::Zero()}; /**< Global ZMP position in the inertial frame
+                                                     */
+    std::vector<Contacts::ContactWrench> m_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 */
+
+    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
+                                        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 */
+};
+
+} // namespace Contacts
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_CONTACTS_GLOBAL_ZMP_EVALUATOR_H