diff --git a/CHANGELOG.md b/CHANGELOG.md index 422f52322a..fc2f7e8bfa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,7 @@ All notable changes to this project are documented in this file. - Implement `SO3Task` in `TSID` component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/281) - Implement clone method in ParametersHandler classes (https://github.com/dic-iit/bipedal-locomotion-framework/pull/288) - Implement `VariablesHandler::clear()` and `VariablesHandler::initialize()` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/291) +- Implement the possibility to set the default contact in the `ContactList` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/297) ### Changed - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204) diff --git a/src/Contacts/CMakeLists.txt b/src/Contacts/CMakeLists.txt index b0a8d617e8..0a8b4f6eed 100644 --- a/src/Contacts/CMakeLists.txt +++ b/src/Contacts/CMakeLists.txt @@ -10,7 +10,7 @@ if (FRAMEWORK_COMPILE_Contact) SUBDIRECTORIES tests/Contacts PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h SOURCES src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp - PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math + PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math BipedalLocomotion::TextLogging INSTALLATION_FOLDER Contacts) set(H_PREFIX include/BipedalLocomotion/ContactDetectors) diff --git a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h index bfe825e198..878976983a 100644 --- a/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h +++ b/src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h @@ -45,6 +45,7 @@ class ContactList contacts. **/ std::string m_defaultName{"ContactList"}; /** Default name for the contact list. **/ BipedalLocomotion::Contacts::ContactType m_defaultContactType{BipedalLocomotion::Contacts::ContactType::FULL}; /** Default contact type. **/ + int m_defaultIndex{-1}; /**< Default Frame index of the contact */ public: using const_iterator @@ -70,6 +71,18 @@ class ContactList */ void setDefaultContactType(const BipedalLocomotion::Contacts::ContactType& type); + /** + * @brief Set the default frame index of the contact + * @param defaultIndex the default index. + */ + void setDefaultIndex(int defaultIndex); + + /** + * @brief Get the default frame index of the contact + * @return the default index. + */ + int defaultIndex() const; + /** * @brief Get the default contact type. * @return the default contact type. diff --git a/src/Contacts/src/ContactList.cpp b/src/Contacts/src/ContactList.cpp index d8feb8dd0f..a39db37c4c 100644 --- a/src/Contacts/src/ContactList.cpp +++ b/src/Contacts/src/ContactList.cpp @@ -6,7 +6,8 @@ */ #include -#include +#include + #include #include @@ -37,24 +38,41 @@ const ContactType &ContactList::defaultContactType() const return m_defaultContactType; } +void ContactList::setDefaultIndex(int defaultIndex) +{ + m_defaultIndex = defaultIndex; +} + +int ContactList::defaultIndex() const +{ + return m_defaultIndex; +} + bool ContactList::addContact(const PlannedContact &newContact) { + constexpr auto errorPrefix = "[ContactList::addContact]"; + if (newContact.activationTime > newContact.deactivationTime) { - std::cerr << "[ContactList::addContact] The activation time cannot be greater than the deactivation time." <error("{} The activation time cannot be greater than the deactivation time.", + errorPrefix); return false; } using iterator = std::set::iterator; std::pair res = m_contacts.insert(newContact); if (!res.second) { - std::cerr << "[ContactList::addContact] Failed to insert new element."; + log()->error("{} Failed to insert new element.", errorPrefix); if (res.first != end()) { - std::cerr << " The new contact (activationTime: " << newContact.activationTime << " "; - std::cerr << "deactivationTime: " << newContact.deactivationTime << ") is not compatible"; - std::cerr << " with an element already present in the list (activationTime: " << res.first->activationTime << " "; - std::cerr << "deactivationTime: " << res.first->deactivationTime << ")" << std::endl; + log()->error("{} The new contact (activationTime: {} deactivationTime: {}) is not " + "compatible with an element already present in the list (activationTime: " + "{} deactivationTime: {}).", + errorPrefix, + newContact.activationTime, + newContact.deactivationTime, + res.first->activationTime, + res.first->deactivationTime); } return false; } @@ -68,6 +86,7 @@ bool ContactList::addContact(const manif::SE3d &newTransform, double activationT newContact.activationTime = activationTime; newContact.deactivationTime = deactivationTime; newContact.name = m_defaultName; + newContact.index = m_defaultIndex; newContact.type = m_defaultContactType; return addContact(newContact); @@ -155,7 +174,7 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned { if (element == end()) { - std::cerr << "[ContactList::addContact] The element is not valid." << std::endl; + log()->error("[ContactList::addContact] The element is not valid."); return false; } @@ -167,7 +186,8 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned --previousElement; if (newContact.activationTime < previousElement->deactivationTime) { - std::cerr << "[ContactList::addContact] The new contact cannot have an activation time smaller than the previous contact." << std::endl; + log()->error("[ContactList::addContact] The new contact cannot have an activation time " + "smaller than the previous contact."); return false; } } @@ -176,7 +196,8 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned { if (newContact.deactivationTime > nextElement->activationTime) { - std::cerr << "[ContactList::addContact] The new contact cannot have a deactivation time greater than the next contact." << std::endl; + log()->error("[ContactList::addContact] The new contact cannot have a deactivation " + "time greater than the next contact."); return false; } } @@ -207,7 +228,8 @@ bool ContactList::keepOnlyPresentContact(double time) if (dropPoint == end()) { - std::cerr << "[ContactList::addContact] No contact has activation time lower than the specified time." << std::endl; + log()->error("[ContactList::addContact] No contact has activation time lower than the " + "specified time."); return false; }