Implement toString() methods in PlannedContact, ContactList, ContactPhase and ContactPhaseList

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)

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;
 };
 
 /**

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 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.
+ * @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;
 };
 
 /**

src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h

@@ -12,8 +12,8 @@
 #include <BipedalLocomotion/Contacts/ContactList.h>
 
 #include <chrono>
-#include <unordered_map>
 #include <string>
+#include <unordered_map>
 
 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

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<ContactPhase>::const_iterator;
  using const_reverse_iterator = std::vector<ContactPhase>::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<ContactList>& 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

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<std::chrono::milliseconds>(activationTime).count()
+ << "ms deactivation time: "
+ << std::chrono::duration_cast<std::chrono::milliseconds>(deactivationTime).count()
+ << "ms pose: " << pose.coeffs().transpose();
+ return ss.str();
+}
+
 std::pair<bool, std::chrono::nanoseconds> EstimatedContact::getContactDetails() const
 {
  return std::make_pair(isActive, switchTime);

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();
+}

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<std::chrono::milliseconds>(beginTime).count()
+ << "ms end time: " << std::chrono::duration_cast<std::chrono::milliseconds>(endTime).count()
+ << "ms active contacts: ";
+ for (const auto& [key, contact] : activeContacts)
+ {
+ ss << key << " " << contact->toString() << " ";
+ }
+ return ss.str();
 }

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();
+}