Add the possibility to resample the contact in a given contact list a…

…nd force the MPC to require correctly sampled contacts (#788)

CHANGELOG.md

@@ -14,6 +14,7 @@ All notable changes to this project are documented in this file.
 - Implement `LinearSpline` in `Math` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/782)
 - Add the support of QP problems with no constraint in `QPInverseKinematics` and `QPTSID` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/784)
 - Implement `blf-joints-grid-position-tracking` application in `utilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/787)
+- Add the possibility to resample the contact in a given contact list (https://github.com/ami-iit/bipedal-locomotion-framework/pull/788)
 
 ### Changed
 - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766)
@@ -28,6 +29,7 @@ All notable changes to this project are documented in this file.
 - 🤖 Modify the `YarpRobotLoggerDevice` of `ergoCubSN001` to be compliant with the latest [robots-configuration files](https://github.com/robotology/robots-configuration/pull/598) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/772)
 - Modify the `balancing-position-control` application to be compliant with the new implementation of the spline and the `VectorsCollectionServer` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776)
 - Add the joint tracking error plot in joint-position-tracking script (https://github.com/ami-iit/bipedal-locomotion-framework/pull/775/)
+- Force the MPC to require correctly time-sampled contacts (https://github.com/ami-iit/bipedal-locomotion-framework/pull/788)
 
 ### Removed
 - Remove the latex dependency in the joint-position-tracking script (https://github.com/ami-iit/bipedal-locomotion-framework/pull/775/)

bindings/python/Contacts/src/Contacts.cpp

@@ -28,29 +28,6 @@ namespace bindings
 namespace Contacts
 {
 
-std::string toString(const BipedalLocomotion::Contacts::PlannedContact& contact)
-{
-    const Eigen::IOFormat FormatEigenVector //
-        (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
-
-    const auto& position = contact.pose.coeffs().segment<3>(0);
-    const auto& quaternion = contact.pose.coeffs().segment<4>(3);
-
-    std::stringstream pose;
-    pose << "SE3 (position = " << position.format(FormatEigenVector)
-         << ", quaternion = " << quaternion.format(FormatEigenVector) << ")";
-
-    std::stringstream description;
-    description << "Contact (name = " << contact.name << ", pose = " << pose.str()
-                << std::setprecision(7) << ", activation_time = "
-                << std::chrono::duration<double>(contact.activationTime).count()
-                << ", deactivation_time = "
-                << std::chrono::duration<double>(contact.deactivationTime).count()
-                << ", type = " << static_cast<int>(contact.type) << ")";
-
-    return description.str();
-}
-
 void CreateContact(pybind11::module& module)
 {
     namespace py = ::pybind11;
@@ -71,7 +48,7 @@ void CreateContact(pybind11::module& module)
         .def(py::init())
         .def_readwrite("activation_time", &PlannedContact::activationTime)
         .def_readwrite("deactivation_time", &PlannedContact::deactivationTime)
-        .def("__repr__", &toString)
+        .def("__repr__", &PlannedContact::toString)
         .def("__eq__", &PlannedContact::operator==, py::is_operator())
         .def("__ne__", &PlannedContact::operator!=, py::is_operator())
         .def("is_contact_active", &PlannedContact::isContactActive);
@@ -209,7 +186,9 @@ void CreateContactList(pybind11::module& module)
                 }
                 return *ptr;
             },
-            py::return_value_policy::reference_internal);
+            py::return_value_policy::reference_internal)
+        .def("force_sample_time", &ContactList::forceSampleTime, py::arg("dt"))
+        .def("are_contacts_sampled", &ContactList::areContactsSampled, py::arg("dt"));
 }
 
 void CreateContactPhase(pybind11::module& module)

src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h

@@ -266,6 +266,28 @@ class ContactList
      * @return A string containing the information of the contact list.
      */
     [[nodiscard]] std::string toString() const;
+
+    /**
+     * @brief Force the sample time of the contact list.
+     * @param dt The new sample time.
+     * @return true if the contact list has been correctly resampled.
+     * @note the activation time is rounded down to the nearest multiple of dt, while the
+     * deactivation time is rounded up to the nearest multiple of dt.
+     * @note If the deactivation time of a contact in the list is equal to
+     * `std::chrono::nanoseconds::max()` it will not be rounded up.
+     * @warning This will change the sample time of all the contacts in the list. All the
+     * iterators to the contacts will be invalidated.
+     */
+    bool forceSampleTime(const std::chrono::nanoseconds& dt);
+
+    /**
+     * @brief Check if the contacts are sampled with the given sample time.
+     * @param dt The sample time.
+     * @return true if the contacts are sampled with the given sample time.
+     * @note If the deactivation time of a contact in the list is equal to
+     * `std::chrono::nanoseconds::max()` it will consider it as sampled with the given sample time.
+     */
+    [[nodiscard]] bool areContactsSampled(const std::chrono::nanoseconds& dt) const;
 };
 
 /**

src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h

@@ -209,6 +209,18 @@ class ContactPhaseList
      * @return A string containing the information of the contact phase list.
      */
     [[nodiscard]] std::string toString() const;
+
+    /**
+     * @brief Force the sample time of the contact list stored in the contact phase list.
+     * @param dt The new sample time.
+     * @return true if the contact lists has been correctly resampled.
+     * @note the activation time is rounded down to the nearest multiple of dt, while the
+     * deactivation time is rounded up to the nearest multiple of dt.
+     * @note If the deactivation time of a contact in the lists is equal to
+     * `std::chrono::nanoseconds::max()` it will not be rounded up.
+     * @note The phases are recomputed after the resampling.
+     */
+    bool forceSampleTime(const std::chrono::nanoseconds& dt);
 };
 
 } // namespace Contacts

src/Contacts/src/ContactList.cpp

@@ -176,7 +176,7 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
 {
     if (element == end())
     {
-        log()->error("[ContactList::addContact] The element is not valid.");
+        log()->error("[ContactList::editContact] The element is not valid.");
         return false;
     }
 
@@ -188,7 +188,8 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
         --previousElement;
         if (newContact.activationTime < previousElement->deactivationTime)
         {
-            log()->error("[ContactList::addContact] The new contact cannot have an activation time "
+            log()->error("[ContactList::editContact] The new contact cannot have an activation "
+                         "time "
                          "smaller than the previous contact.");
             return false;
         }
@@ -198,7 +199,7 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
     {
         if (newContact.deactivationTime > nextElement->activationTime)
         {
-            log()->error("[ContactList::addContact] The new contact cannot have a deactivation "
+            log()->error("[ContactList::editContact] The new contact cannot have a deactivation "
                          "time greater than the next contact.");
             return false;
         }
@@ -301,4 +302,62 @@ std::string ContactList::toString() const
         ss << contact.toString() << std::endl;
     }
     return ss.str();
-}
+}
+
+bool ContactList::forceSampleTime(const std::chrono::nanoseconds& dT)
+{
+    using namespace std::chrono_literals;
+    PlannedContact newContact;
+    for (int i = 0; i < size(); i++)
+    {
+        newContact = (*this)[i];
+
+        // if the activation and deactivation time are already a multiple of dT, we skip the
+        // contact.
+        if (newContact.activationTime % dT == 0ns
+            && (newContact.deactivationTime % dT == 0ns
+                || newContact.deactivationTime == std::chrono::nanoseconds::max()))
+        {
+            continue;
+        }
+
+        // modify the activation and deactivation time to be a multiple of dT
+        // for the activation time we round down, for the deactivation time we round up
+        // This will increase the contact duration, but it will not change the contact sequence
+        // and location.
+        newContact.activationTime -= (newContact.activationTime % dT);
+
+        // if the deactivation time is equal to std::chrono::nanoseconds::max() we do not round
+        if (newContact.deactivationTime != std::chrono::nanoseconds::max())
+        {
+            newContact.deactivationTime += (dT - (newContact.deactivationTime % dT));
+        }
+
+        if (!this->editContact(std::next(this->begin(), i), newContact))
+        {
+            log()->error("[ContactList::forceSampleTime] Failed to edit contact {}. Original "
+                         "contact: {}. New contact: {}.",
+                         i,
+                         (*this)[i].toString(),
+                         newContact.toString());
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool ContactList::areContactsSampled(const std::chrono::nanoseconds& dT) const
+{
+    using namespace std::chrono_literals;
+    for (const auto& contact : m_contacts)
+    {
+        if (contact.activationTime % dT != 0ns
+            || (contact.deactivationTime % dT != 0ns
+                && contact.deactivationTime != std::chrono::nanoseconds::max()))
+        {
+            return false;
+        }
+    }
+    return true;
+}

src/Contacts/src/ContactPhaseList.cpp

@@ -241,3 +241,35 @@ std::string ContactPhaseList::toString() const
     }
     return ss.str();
 }
+
+bool ContactPhaseList::forceSampleTime(const std::chrono::nanoseconds& dT)
+{
+    bool updatePhasesRequired = false;
+    for (auto& [key, list] : m_contactLists)
+    {
+        const bool resampleRequired = !list.areContactsSampled(dT);
+        if (!resampleRequired)
+        {
+            continue;
+        }
+
+        // if the list has not been resampled, we need to recompute the phases
+        updatePhasesRequired = true;
+        if (!list.forceSampleTime(dT))
+        {
+            log()->error("[ContactPhaseList::forceSampleTime] Unable to force the sample time of "
+                         "the list named: {}.",
+                         key);
+            return false;
+        }
+    }
+
+    // if at least one list has been resampled, we need to recompute the phases
+    if (!updatePhasesRequired)
+    {
+        return true;
+    }
+
+    createPhases();
+    return true;
+}

src/Contacts/tests/Contacts/ContactListTest.cpp

@@ -122,6 +122,23 @@ TEST_CASE("ContactList")
         }
         REQUIRE(ok);
     }
+
+    SECTION("Resample")
+    {
+        constexpr std::chrono::nanoseconds dT = 70ms;
+        PlannedContact p3;
+        p3.activationTime = 1s + 800ms;
+        p3.deactivationTime = 2s + 400ms;
+
+        REQUIRE(list.addContact(p3));
+
+        REQUIRE_FALSE(list.areContactsSampled(dT));
+
+        REQUIRE(list.forceSampleTime(dT));
+
+        REQUIRE(list.areContactsSampled(dT));
+    }
+
 }
 
 TEST_CASE("ContactList JSON parser")

src/Planners/src/SwingFootPlanner.cpp

@@ -186,8 +186,8 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList)
                          "tolerance "
                          "{} m, admissible orientation tolerance {} rad. Current time {}.",
                          logPrefix,
-                         activeContact->pose.coeffs().transpose(),
-                         activeContactNewList->pose.coeffs().transpose(),
+                         activeContact->toString(),
+                         activeContactNewList->toString(),
                          (activeContactNewList->pose - activeContact->pose).coeffs().transpose(),
                          m_positionTolerance,
                          m_orientationTolerance,

src/ReducedModelControllers/src/CentroidalMPC.cpp

@@ -1200,7 +1200,6 @@ bool CentroidalMPC::advance()
                 = m_pimpl->currentTime + m_pimpl->optiSettings.samplingTime * index;
 
             auto nextPlannedContact = contactList.getPresentContact(nextPlannedContactTime);
-
             if (nextPlannedContact == contactList.end())
             {
                 log()->error("[CentroidalMPC::advance] Unable to get the next planned contact");
@@ -1214,7 +1213,12 @@ bool CentroidalMPC::advance()
 
             if (!contactList.editContact(nextPlannedContact, modifiedNextPlannedContact))
             {
-                log()->error("[CentroidalMPC::advance] Unable to edit the next planned contact");
+                log()->error("{} Unable to edit the next planned contact at time {}. The contact "
+                             "list contains the following contacts: {}",
+                             errorPrefix,
+                             std::chrono::duration_cast<std::chrono::milliseconds>(
+                                 nextPlannedContactTime),
+                             contactList.toString());
                 return false;
             }
         }
@@ -1382,6 +1386,20 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac
         return false;
     }
 
+    for (const auto& [key, list] : contactPhaseList.lists())
+    {
+        if (!list.areContactsSampled(m_pimpl->optiSettings.samplingTime))
+        {
+            log()->error("{} The contact list {} is not sampled at the sampling time {}. Please "
+                         "resample the contacts lists before calling this method.",
+                         errorPrefix,
+                         key,
+                         std::chrono::duration_cast<std::chrono::milliseconds>(
+                             m_pimpl->optiSettings.samplingTime));
+            return false;
+        }
+    }
+
     m_pimpl->contactPhaseList = contactPhaseList;
 
     // The orientation is stored as a vectorized version of the rotation matrix
@@ -1422,8 +1440,12 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac
     auto initialPhase = contactPhaseList.getPresentPhase(m_pimpl->currentTime);
     if (initialPhase == contactPhaseList.end())
     {
-        log()->error("{} Unable to find the contact phase related to the current time.",
-                     errorPrefix);
+        log()->error("{} Unable to find the contact phase related to the current at time {}. The "
+                     "contact "
+                     "list contains the following contacts: {}",
+                     errorPrefix,
+                     std::chrono::duration_cast<std::chrono::milliseconds>(m_pimpl->currentTime),
+                     contactPhaseList.toString());
         return false;
     }
 
@@ -1442,7 +1464,8 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac
         const std::chrono::nanoseconds tInitial = std::max(m_pimpl->currentTime, it->beginTime);
         const std::chrono::nanoseconds tFinal = std::min(absoluteTimeHorizon, it->endTime);
 
-        const int numberOfSamples = (tFinal - tInitial) / m_pimpl->optiSettings.samplingTime;
+        const std::chrono::nanoseconds duration = tFinal - tInitial;
+        const int numberOfSamples = duration / m_pimpl->optiSettings.samplingTime;
 
         for (const auto& [key, contact] : it->activeContacts)
         {
@@ -1505,50 +1528,5 @@ bool CentroidalMPC::setContactPhaseList(const Contacts::ContactPhaseList& contac
     // we store the contact phase list for the output
     m_pimpl->output.contactPhaseList = contactPhaseList;
 
-    for (auto& [key, contact] : m_pimpl->output.contacts)
-    {
-        const ContactList& contactList = m_pimpl->output.contactPhaseList.lists().at(key);
-        auto inputContact = inputs.contacts.find(key);
-
-        // this is required for toEigen
-        using namespace BipedalLocomotion::Conversions;
-
-        int index = toEigen(*(inputContact->second.isEnabled)).size();
-        const int size = toEigen(*(inputContact->second.isEnabled)).size();
-        for (int i = 0; i < size; i++)
-        {
-            // read it as: "if the contact is active at a given time instant"
-            if (toEigen(*(inputContact->second.isEnabled))(i) > 0.5)
-            {
-                // if the contact is active now
-                if (i == 0)
-                {
-                    break;
-                } // in this case we break if the contact is active and at the previous time
-                  // step it was not active
-                else if (toEigen(*(inputContact->second.isEnabled))(i - 1) < 0.5)
-                {
-                    index = i;
-                    break;
-                }
-            }
-        }
-
-        // In this case the contact is not active and there will be a next planned contact
-        if (index < size)
-        {
-            const std::chrono::nanoseconds nextPlannedContactTime
-                = m_pimpl->currentTime + m_pimpl->optiSettings.samplingTime * index;
-
-            auto nextPlannedContact = contactList.getPresentContact(nextPlannedContactTime);
-
-            if (nextPlannedContact == contactList.end())
-            {
-                log()->error("[CentroidalMPC::advance] Unable to get the next planned contact");
-                return false;
-            }
-        }
-    }
-
     return true;
 }