From e4404d9f1e4f75753fa2787a61d61ccd5956956f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 Dec 2023 20:34:08 +0100 Subject: [PATCH 1/3] Implement toString() methods in PlannedContact, ContactList, ContactPhase and ContactPhaseList --- .../BipedalLocomotion/Contacts/Contact.h | 7 ++++ .../BipedalLocomotion/Contacts/ContactList.h | 17 +++++++--- .../BipedalLocomotion/Contacts/ContactPhase.h | 20 ++++++++---- .../Contacts/ContactPhaseList.h | 32 +++++++++++++------ src/Contacts/src/Contact.cpp | 11 +++++++ src/Contacts/src/ContactList.cpp | 11 +++++++ src/Contacts/src/ContactPhase.cpp | 18 +++++++++-- src/Contacts/src/ContactPhaseList.cpp | 10 ++++++ 8 files changed, 103 insertions(+), 23 deletions(-) diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h index 9d15f4c1d3..009ffe9bd0 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h @@ -104,6 +104,13 @@ struct PlannedContact : ContactBase * @return True if `activationTime <= t < deactivationTime`. */ [[nodiscard]] bool isContactActive(const std::chrono::nanoseconds& t) const; + + /** + * @brief Convert the contact to a string. + * + * @return A string containing the information of the contact. + */ + [[nodiscard]] std::string toString() const; }; /** diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h index 111e2e12f6..d284a1417a 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h @@ -210,11 +210,11 @@ class ContactList /** * @brief Get the active contact given the time. * - * It returns the active contact (i.e., the highest activation time lower than time and the deactivation - * time strictly higher than time) - * If no contacts is active at the given time, it returns an iterator to the end. + * It returns the active contact (i.e., the highest activation time lower than time and the + * deactivation time strictly higher than time) If no contacts is active at the given time, it + * returns an iterator to the end. * @param time The present time. - * @return an iterator to the last contact having an activation time lower than time. + * @return an iterator to the last contact having an activation time lower than time. * If no contact satisfies this condition, it returns a pointer to the end. * @note Differently from getPresentContact the contact needs to be active. */ @@ -238,7 +238,7 @@ class ContactList * If no contacts have an activation time lower than time, it returns an iterator to the end. * Notice that the contact may not be active, i.e. the deactivationTime may be lower than time. * @param time The present time. - * @return an iterator to the last contact having an activation time lower than time. + * @return an iterator to the last contact having an activation time lower than time. * If no contact satisfies this condition, it returns a pointer to the end. */ const_iterator getPresentContact(const std::chrono::nanoseconds& time) const; @@ -259,6 +259,13 @@ class ContactList * @brief Remove only the last contact. */ void removeLastContact(); + + /** + * @brief Convert the contact list to a string. + * + * @return A string containing the information of the contact list. + */ + [[nodiscard]] std::string toString() const; }; /** diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h index 88aeb406a3..e604034426 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h @@ -12,8 +12,8 @@ #include #include -#include #include +#include namespace BipedalLocomotion { @@ -22,7 +22,8 @@ namespace Contacts /** * @brief Struct defining a contact phase. - * Each phase is characterized by a set of contacts which remain active for the entirety of the phase. + * Each phase is characterized by a set of contacts which remain active for the entirety of the + * phase. * @note Mathematically speaking the interval of the phase is defined as following * \f[ * I = [t_b \; t_e) @@ -35,12 +36,12 @@ struct ContactPhase /** * @brief The phase initial time. **/ - std::chrono::nanoseconds beginTime {std::chrono::nanoseconds::zero()}; + std::chrono::nanoseconds beginTime{std::chrono::nanoseconds::zero()}; /** * @brief The phase end time. **/ - std::chrono::nanoseconds endTime {std::chrono::nanoseconds::zero()}; + std::chrono::nanoseconds endTime{std::chrono::nanoseconds::zero()}; /** * @brief The set of contacts active during the phase. @@ -53,9 +54,16 @@ struct ContactPhase * @return True if key is present amongst the active contacts. **/ bool isListIncluded(const std::string& key) const; + + /** + * @brief Convert the contact phase to a string. + * + * @return A string containing the information of the contact phase. + */ + [[nodiscard]] std::string toString() const; }; -} -} +} // namespace Contacts +} // namespace BipedalLocomotion #endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACT_PHASE_H diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h index 93d941aed0..431bcd044a 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h @@ -21,8 +21,10 @@ namespace Contacts { /** - * @brief The ContactPhaseList class computes the contact phases according to a bunch of input contact lists. - * @warning All the iterators stored inside the contact phases refer to the lists stored within this class, not the original input lists. + * @brief The ContactPhaseList class computes the contact phases according to a bunch of input + * contact lists. + * @warning All the iterators stored inside the contact phases refer to the lists stored within this + * class, not the original input lists. */ class ContactPhaseList { @@ -34,7 +36,6 @@ class ContactPhaseList void createPhases(); /** Internal method to compute the phases. **/ public: - using const_iterator = std::vector::const_iterator; using const_reverse_iterator = std::vector::const_reverse_iterator; @@ -99,8 +100,9 @@ class ContactPhaseList /** * @brief Set the input lists - * @param An initializer list (use as {list1, list2, ...,listN}) to the lists to be used for computing the phases. - * A ContactListMap will be created with the provided list, using the defaultName as a key. + * @param An initializer list (use as {list1, list2, ...,listN}) to the lists to be used for + * computing the phases. A ContactListMap will be created with the provided list, using the + * defaultName as a key. * @return False if some lists have the same defaultName. */ bool setLists(const std::initializer_list& contactLists); @@ -118,7 +120,8 @@ class ContactPhaseList /** * @brief A reference to the lists stored in this class. - * @warning All the iterators stored inside the contact phases refer to the lists viewable via this method. + * @warning All the iterators stored inside the contact phases refer to the lists viewable via + * this method. * @return A const reference to the input lists. */ const ContactListMap& lists() const; @@ -134,12 +137,14 @@ class ContactPhaseList const_iterator cbegin() const; /** - * @brief Const reverse iterator to the the phases (basically starting from the last phase, going backward). + * @brief Const reverse iterator to the the phases (basically starting from the last phase, + * going backward). */ const_reverse_iterator rbegin() const; /** - * @brief Const reverse iterator to the the phases (basically starting from the last phase, going backward). + * @brief Const reverse iterator to the the phases (basically starting from the last phase, + * going backward). */ const_reverse_iterator crbegin() const; @@ -197,9 +202,16 @@ class ContactPhaseList * @brief Clear the phases and the stored lists. */ void clear(); + + /** + * @brief Convert the contact phase list to a string. + * + * @return A string containing the information of the contact phase list. + */ + [[nodiscard]] std::string toString() const; }; -} -} +} // namespace Contacts +} // namespace BipedalLocomotion #endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACT_PHASE_LIST_H diff --git a/src/Contacts/src/Contact.cpp b/src/Contacts/src/Contact.cpp index 9ba6bc756d..48443b0e43 100644 --- a/src/Contacts/src/Contact.cpp +++ b/src/Contacts/src/Contact.cpp @@ -32,6 +32,17 @@ bool PlannedContact::isContactActive(const std::chrono::nanoseconds& t) const return (this->activationTime <= t) && (t < this->deactivationTime); } +std::string PlannedContact::toString() const +{ + std::stringstream ss; + ss << "Contact name: " << name << " activation time: " + << std::chrono::duration_cast(activationTime).count() + << "ms deactivation time: " + << std::chrono::duration_cast(deactivationTime).count() + << "ms pose: " << pose.coeffs().transpose(); + return ss.str(); +} + std::pair EstimatedContact::getContactDetails() const { return std::make_pair(isActive, switchTime); diff --git a/src/Contacts/src/ContactList.cpp b/src/Contacts/src/ContactList.cpp index 0d6b2677ac..1639393850 100644 --- a/src/Contacts/src/ContactList.cpp +++ b/src/Contacts/src/ContactList.cpp @@ -291,3 +291,14 @@ void ContactList::removeLastContact() { erase(lastContact()); } + +std::string ContactList::toString() const +{ + std::stringstream ss; + ss << "Contact list: " << std::endl; + for (const auto& contact : m_contacts) + { + ss << contact.toString() << std::endl; + } + return ss.str(); +} \ No newline at end of file diff --git a/src/Contacts/src/ContactPhase.cpp b/src/Contacts/src/ContactPhase.cpp index a8ebf4c6fc..e789e13e0f 100644 --- a/src/Contacts/src/ContactPhase.cpp +++ b/src/Contacts/src/ContactPhase.cpp @@ -10,7 +10,21 @@ using namespace BipedalLocomotion::Contacts; -bool ContactPhase::isListIncluded(const std::string &key) const +bool ContactPhase::isListIncluded(const std::string& key) const { - return activeContacts.find(key) != activeContacts.end(); + return activeContacts.find(key) != activeContacts.end(); +} + +std::string ContactPhase::toString() const +{ + std::stringstream ss; + ss << "Phase start time: " + << std::chrono::duration_cast(beginTime).count() + << "ms end time: " << std::chrono::duration_cast(endTime).count() + << "ms active contacts: "; + for (const auto& [key, contact] : activeContacts) + { + ss << key << " " << contact->toString() << " "; + } + return ss.str(); } diff --git a/src/Contacts/src/ContactPhaseList.cpp b/src/Contacts/src/ContactPhaseList.cpp index 5fd7e37553..0f225dc60f 100644 --- a/src/Contacts/src/ContactPhaseList.cpp +++ b/src/Contacts/src/ContactPhaseList.cpp @@ -231,3 +231,13 @@ void ContactPhaseList::clear() m_phases.clear(); m_contactLists.clear(); } + +std::string ContactPhaseList::toString() const +{ + std::stringstream ss; + for (const auto& phase : m_phases) + { + ss << phase.toString() << std::endl; + } + return ss.str(); +} From 4496cf95ec3baefe46e388b85e0c390cef31e7c2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 Dec 2023 20:36:13 +0100 Subject: [PATCH 2/3] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b0ce38a3a5..1027c62ad9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ All notable changes to this project are documented in this file. - Add process model for external contacts in `RobotDynamicsEstimator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/759) - 🤖 Add `ergoCubSN001` configuration files for the `balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776) - Implement `VectorsCollectionServer` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776) +- Implement `toString()` methods in `PlannedContact`, `ContactList`, `ContactPhase` and `ContactPhaseList` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/777) ### 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) From 041d923eb0c39299c756f4433cd9e66243ec1960 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 13 Dec 2023 23:20:36 +0100 Subject: [PATCH 3/3] Update src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h Co-authored-by: Ines Sorrentino <43743081+isorrentino@users.noreply.github.com> --- src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h index d284a1417a..a5d4f99ef1 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h @@ -211,7 +211,7 @@ class ContactList * @brief Get the active contact given the time. * * It returns the active contact (i.e., the highest activation time lower than time and the - * deactivation time strictly higher than time) If no contacts is active at the given time, it + * deactivation time strictly higher than time). If no contacts are active at the given time, it * returns an iterator to the end. * @param time The present time. * @return an iterator to the last contact having an activation time lower than time.