From 79ac4fcea79fe28691845183d1aa8a55a5f8dbb2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 17 May 2020 16:13:05 +0200 Subject: [PATCH 01/10] Added ContactList. --- src/Planners/CMakeLists.txt | 5 +- .../BipedalLocomotion/Planners/Contact.h | 10 +- .../BipedalLocomotion/Planners/ContactList.h | 94 ++++++++ src/Planners/src/ContactList.cpp | 206 ++++++++++++++++++ src/Planners/tests/CMakeLists.txt | 11 + src/Planners/tests/ContactListTest.cpp | 84 +++++++ 6 files changed, 403 insertions(+), 7 deletions(-) create mode 100644 src/Planners/include/BipedalLocomotion/Planners/ContactList.h create mode 100644 src/Planners/src/ContactList.cpp create mode 100644 src/Planners/tests/CMakeLists.txt create mode 100644 src/Planners/tests/ContactListTest.cpp diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 6d50f8f2fb..fd22bf2521 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -4,8 +4,9 @@ add_bipedal_locomotion_library( NAME Contact - PUBLIC_HEADERS include/BipedalLocomotion/Planners/Contact.h + PUBLIC_HEADERS include/BipedalLocomotion/Planners/Contact.h include/BipedalLocomotion/Planners/ContactList.h + SOURCES src/ContactList.cpp PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core - IS_INTERFACE INSTALLATION_FOLDER Planners + SUBDIRECTORIES tests ) diff --git a/src/Planners/include/BipedalLocomotion/Planners/Contact.h b/src/Planners/include/BipedalLocomotion/Planners/Contact.h index bfe3f85813..c5c1b6feb5 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/Contact.h +++ b/src/Planners/include/BipedalLocomotion/Planners/Contact.h @@ -37,27 +37,27 @@ struct Contact /** * Pose of the contact. */ - iDynTree::Transform pose; + iDynTree::Transform pose {iDynTree::Transform::Identity()}; /** * Instant from which the contact can be considered active. */ - double activationTime; + double activationTime {0.0}; /** * Instant after which the contact is no more active. */ - double deactivationTime; + double deactivationTime {0.0}; /** * Name of the contact. */ - std::string name; + std::string name {"Contact"}; /** * Type of contact. */ - ContactType type; + ContactType type {ContactType::FULL}; }; } diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h new file mode 100644 index 0000000000..9a96eba75c --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h @@ -0,0 +1,94 @@ +/** + * @file ContactList.h + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H +#define BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ + +/** + * @brief Class containing a list of steps. + * The steps are added such that the activation time is strictly growing. + * It represents a series of contact activations and deactivations of a single entity. + * Once items are inserted, items cannot change since they are ordered according to their timing. + */ +class ContactList +{ + struct ContactCompare { + bool operator()(const Contact& lhs, const Contact& rhs) const { + return lhs.deactivationTime < rhs.activationTime; + } + }; + + std::set m_contacts; + std::string m_defaultName{"ContactList"}; + ContactType m_defaultContactType{ContactType::FULL}; + +public: + + using const_iterator = std::set::const_iterator; + using const_reverse_iterator = std::set::const_reverse_iterator; + + void setDefaultName(const std::string& defaultName); + + const std::string& defaultName() const; + + void setDefaultContactType(const ContactType& type); + + const ContactType& defaultContactType() const; + + bool addContact(const Contact& newContact); + + bool addContact(const iDynTree::Transform& newTransform, double activationTime, double deactivationTime); + + const_iterator erase(const_iterator iterator); + + const_iterator begin() const; + const_iterator cbegin() const; + + const_reverse_iterator rbegin() const; + const_reverse_iterator crbegin() const; + + const_iterator end() const; + const_iterator cend() const; + + const_reverse_iterator rend() const; + const_reverse_iterator crend() const; + + size_t size() const; + + const_iterator firstStep() const; + + const_iterator lastStep() const; + + bool editContact(const_iterator element, const Contact& newContact); + + const_iterator getPresentStep(double time) const; + + bool keepOnlyPresentStep(double time); + + void clear(); + + void removeLastStep(); + +}; + +} //namespace Planners +} //namespace BipedalLocomotion + + + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H diff --git a/src/Planners/src/ContactList.cpp b/src/Planners/src/ContactList.cpp new file mode 100644 index 0000000000..cec6b87dd8 --- /dev/null +++ b/src/Planners/src/ContactList.cpp @@ -0,0 +1,206 @@ +/** + * @file ContactList.cpp + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include + +using namespace BipedalLocomotion::Planners; + +void ContactList::setDefaultName(const std::string &defaultName) +{ + m_defaultName = defaultName; +} + +const std::string &ContactList::defaultName() const +{ + return m_defaultName; +} + +void ContactList::setDefaultContactType(const ContactType &type) +{ + m_defaultContactType = type; +} + +const ContactType &ContactList::defaultContactType() const +{ + return m_defaultContactType; +} + +bool ContactList::addContact(const Contact &newContact) +{ + if (newContact.activationTime > newContact.deactivationTime) + { + std::cerr << "[ContactList::addContact] The activation time cannot be greater than the deactivation time." <::iterator; + std::pair res = m_contacts.insert(newContact); + if (!res.second) + { + std::cerr << "[ContactList::addContact] Failed to insert new element."; + 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; + } + return false; + } + return true; +} + +bool ContactList::addContact(const iDynTree::Transform &newTransform, double activationTime, double deactivationTime) +{ + Contact newContact; + newContact.pose = newTransform; + newContact.activationTime = activationTime; + newContact.deactivationTime = deactivationTime; + newContact.name = m_defaultName; + newContact.type = m_defaultContactType; + + return addContact(newContact); +} + +ContactList::const_iterator ContactList::erase(const_iterator iterator) +{ + return m_contacts.erase(iterator); +} + +ContactList::const_iterator ContactList::begin() const +{ + return m_contacts.begin(); +} + +ContactList::const_iterator ContactList::cbegin() const +{ + return m_contacts.cbegin(); +} + +ContactList::const_reverse_iterator ContactList::rbegin() const +{ + return m_contacts.rbegin(); +} + +ContactList::const_reverse_iterator ContactList::crbegin() const +{ + return m_contacts.crbegin(); +} + +ContactList::const_iterator ContactList::end() const +{ + return m_contacts.end(); +} + +ContactList::const_iterator ContactList::cend() const +{ + return m_contacts.cend(); +} + +ContactList::const_reverse_iterator ContactList::rend() const +{ + return m_contacts.rend(); +} + +ContactList::const_reverse_iterator ContactList::crend() const +{ + return m_contacts.crend(); +} + +size_t ContactList::size() const +{ + return m_contacts.size(); +} + +ContactList::const_iterator ContactList::firstStep() const +{ + return begin(); +} + +ContactList::const_iterator ContactList::lastStep() const +{ + return --end(); +} + +bool ContactList::editContact(ContactList::const_iterator element, const Contact &newContact) +{ + if (element == end()) + { + std::cerr << "[ContactList::addContact] The element is not valid." << std::endl; + return false; + } + + ContactList::const_iterator previousElement = element; + ContactList::const_iterator nextElement = element; + nextElement++; + if (element != begin()) + { + --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; + return false; + } + } + + if (nextElement != end()) + { + if (newContact.deactivationTime > nextElement->activationTime) + { + std::cerr << "[ContactList::addContact] The new contact cannot have a deactivation time greater than the next contact." << std::endl; + return false; + } + } + + m_contacts.erase(element); + m_contacts.insert(nextElement, newContact); + + return true; +} + +ContactList::const_iterator ContactList::getPresentStep(double time) const +{ + // With the reverse iterator we find the last step such that the activation time is smaller that time + ContactList::const_reverse_iterator presentReverse = std::find_if(rbegin(), rend(), + [time](const Contact & a) -> bool { return a.activationTime <= time; }); + if (presentReverse == rend()) + { + // No contact has activation time lower than the specified time. + return end(); + } + + return --(presentReverse.base()); //This is to convert a reverse iterator to a forward iterator. The -- is because base() returns a forward iterator to the next element. +} + +bool ContactList::keepOnlyPresentStep(double time) +{ + ContactList::const_iterator dropPoint = getPresentStep(time); + + if (dropPoint == end()) + { + std::cerr << "[ContactList::addContact] No contact has activation time lower than the specified time." << std::endl; + return false; + } + + Contact present = *dropPoint; + + clear(); + addContact(present); + + return true; +} + +void ContactList::clear() +{ + m_contacts.clear(); +} + +void ContactList::removeLastStep() +{ + erase(lastStep()); +} + diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt new file mode 100644 index 0000000000..ac9fb4c2cb --- /dev/null +++ b/src/Planners/tests/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +if (FRAMEWORK_HAS_Eigen3) + add_bipedal_test( + NAME ContactList + SOURCES ContactListTest.cpp + LINKS BipedalLocomotion::Contact + DEPENDS_ON_EIGEN_PRIVATE) +endif() diff --git a/src/Planners/tests/ContactListTest.cpp b/src/Planners/tests/ContactListTest.cpp new file mode 100644 index 0000000000..57def7d573 --- /dev/null +++ b/src/Planners/tests/ContactListTest.cpp @@ -0,0 +1,84 @@ +/** + * @file ContactListTest.cpp + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +#include +#include + +#include + +using namespace BipedalLocomotion::Planners; + +bool contactsAreEqual(const Contact& c1, const Contact& c2) +{ + return (c1.type == c2.type) && + (c1.name == c2.name) && + (iDynTree::toEigen((c1.pose * c2.pose.inverse()).asHomogeneousTransform()).isApprox( + iDynTree::toEigen(iDynTree::Transform::Identity().asHomogeneousTransform()))) && + (iDynTree::checkDoublesAreEqual(c1.activationTime, c2.activationTime)) && + (iDynTree::checkDoublesAreEqual(c1.deactivationTime, c2.deactivationTime)); +} + +TEST_CASE("ContactList") +{ + ContactList list; + Contact p1, p2; + p1.activationTime = 0.1; + p1.deactivationTime = 0.5; + + p2.activationTime = 1.0; + p2.deactivationTime = 1.5; + + bool ok1 = list.addContact(p2); + bool ok2 = list.addContact(p1); + + SECTION("Insertion") + { + REQUIRE(ok1); + REQUIRE(ok2); + } + + SECTION("Insertion order") + { + REQUIRE(contactsAreEqual(p1, *list.firstStep())); + REQUIRE(contactsAreEqual(p2, *list.lastStep())); + } + + SECTION("Size") + { + REQUIRE(list.size() == 2); + } + + SECTION("Invalid insertion") + { + Contact p3; + p3.activationTime = 0.9; + p3.deactivationTime = 1.6; + + REQUIRE_FALSE(list.addContact(p3)); + } + + SECTION("Edit") + { + Contact p2Modified; + p2Modified = p2; + p2Modified.type = ContactType::POINT; + + REQUIRE(list.editContact(list.lastStep(), p2Modified)); + REQUIRE(contactsAreEqual(p2Modified, *list.lastStep())); + } + + SECTION("Present step") + { + REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.2))); + REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.6))); + REQUIRE(contactsAreEqual(p1, *list.getPresentStep(0.6))); + REQUIRE(list.getPresentStep(0.0) == list.end()); + } +} From ac15e3e6ddde17accbda8fd3819ec4986c626795 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 17 May 2020 17:20:05 +0200 Subject: [PATCH 02/10] Added ContactPhase.h Trying to fix WIndows failure. --- src/Planners/CMakeLists.txt | 4 ++- .../BipedalLocomotion/Planners/ContactPhase.h | 32 +++++++++++++++++++ src/Planners/tests/ContactListTest.cpp | 17 +++++++++- 3 files changed, 51 insertions(+), 2 deletions(-) create mode 100644 src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index fd22bf2521..8a9c04765d 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -2,9 +2,11 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +set(H_PREFIX include/BipedalLocomotion/Planners) + add_bipedal_locomotion_library( NAME Contact - PUBLIC_HEADERS include/BipedalLocomotion/Planners/Contact.h include/BipedalLocomotion/Planners/ContactList.h + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h SOURCES src/ContactList.cpp PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core INSTALLATION_FOLDER Planners diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h new file mode 100644 index 0000000000..990d2d0347 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h @@ -0,0 +1,32 @@ +/** + * @file ContactPhase.h + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_H +#define BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ + +struct ContactPhase +{ + double begin {0.0}; + + double end {0.0}; + + std::vector activeContacts; +}; + +} +} + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_H diff --git a/src/Planners/tests/ContactListTest.cpp b/src/Planners/tests/ContactListTest.cpp index 57def7d573..7bf39e4788 100644 --- a/src/Planners/tests/ContactListTest.cpp +++ b/src/Planners/tests/ContactListTest.cpp @@ -48,6 +48,14 @@ TEST_CASE("ContactList") { REQUIRE(contactsAreEqual(p1, *list.firstStep())); REQUIRE(contactsAreEqual(p2, *list.lastStep())); + + Contact p3; + p3.activationTime = 0.6; + p3.deactivationTime = 0.8; + + REQUIRE(list.addContact(p3)); + REQUIRE(list.size() == 3); + REQUIRE(contactsAreEqual(p3, *(++list.begin()))); } SECTION("Size") @@ -79,6 +87,13 @@ TEST_CASE("ContactList") REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.2))); REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.6))); REQUIRE(contactsAreEqual(p1, *list.getPresentStep(0.6))); - REQUIRE(list.getPresentStep(0.0) == list.end()); + bool same = list.getPresentStep(0.0) == list.end(); + REQUIRE(same); + } + + SECTION("Keep present step") + { + list.clear(); + REQUIRE(list.size() == 0); } } From 65508eb444bfc83fae21a542dbdbb3ff0a475572 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 18 May 2020 17:18:04 +0200 Subject: [PATCH 03/10] Added definition of phases and a way to compute them. --- src/Planners/CMakeLists.txt | 4 +- .../BipedalLocomotion/Planners/ContactPhase.h | 17 +- .../Planners/ContactPhaseList.h | 72 +++++++ src/Planners/src/ContactPhase.cpp | 24 +++ src/Planners/src/ContactPhaseList.cpp | 199 ++++++++++++++++++ src/Planners/tests/CMakeLists.txt | 6 + src/Planners/tests/ContactPhaseListTest.cpp | 124 +++++++++++ 7 files changed, 441 insertions(+), 5 deletions(-) create mode 100644 src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h create mode 100644 src/Planners/src/ContactPhase.cpp create mode 100644 src/Planners/src/ContactPhaseList.cpp create mode 100644 src/Planners/tests/ContactPhaseListTest.cpp diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 8a9c04765d..52e472b32e 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -6,8 +6,8 @@ set(H_PREFIX include/BipedalLocomotion/Planners) add_bipedal_locomotion_library( NAME Contact - PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h - SOURCES src/ContactList.cpp + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h + SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core INSTALLATION_FOLDER Planners SUBDIRECTORIES tests diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h index 990d2d0347..7b46d1be08 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h @@ -11,19 +11,30 @@ #include #include #include +#include namespace BipedalLocomotion { namespace Planners { +struct ContactReference +{ + std::string listLabel; + ContactList::const_iterator contact_it; +}; + struct ContactPhase { - double begin {0.0}; + double beginTime {0.0}; + + double endTime {0.0}; + + std::vector activeContacts; - double end {0.0}; + bool isListIncluded(const std::string& key) const; - std::vector activeContacts; + std::vector::const_iterator getContactGivenList(const std::string& key) const; }; } diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h new file mode 100644 index 0000000000..54fb7ba1cd --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h @@ -0,0 +1,72 @@ +/** + * @file ContactPhase.h + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_LIST_H +#define BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_LIST_H + +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ + +using ContactListMap = std::unordered_map; + +class ContactPhaseList +{ + + ContactListMap m_contactLists; + + std::vector m_phases; + + void createPhases(); + +public: + + using const_iterator = std::vector::const_iterator; + using const_reverse_iterator = std::vector::const_reverse_iterator; + + void setLists(const ContactListMap& contactLists); + + bool setLists(const std::initializer_list& contactLists); + + const ContactListMap& lists() const; + + const_iterator begin() const; + const_iterator cbegin() const; + + const_reverse_iterator rbegin() const; + const_reverse_iterator crbegin() const; + + const_iterator end() const; + const_iterator cend() const; + + const_reverse_iterator rend() const; + const_reverse_iterator crend() const; + + const ContactPhase& operator[](size_t index) const; + + const_iterator firstPhase() const; + + const_iterator lastPhase() const; + + size_t size() const; + + void clear(); +}; + +} +} + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_LIST_H diff --git a/src/Planners/src/ContactPhase.cpp b/src/Planners/src/ContactPhase.cpp new file mode 100644 index 0000000000..1c914ec6f2 --- /dev/null +++ b/src/Planners/src/ContactPhase.cpp @@ -0,0 +1,24 @@ +/** + * @file ContactPhase.cpp + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include + +using namespace BipedalLocomotion::Planners; + +bool ContactPhase::isListIncluded(const std::string &key) const +{ + std::vector::const_iterator it = getContactGivenList(key); + + return it != activeContacts.end(); +} + +std::vector::const_iterator ContactPhase::getContactGivenList(const std::string &key) const +{ + return std::find_if(activeContacts.begin(), activeContacts.end(), + [key](const ContactReference& ref){return ref.listLabel == key;}); +} diff --git a/src/Planners/src/ContactPhaseList.cpp b/src/Planners/src/ContactPhaseList.cpp new file mode 100644 index 0000000000..e8abeacf5d --- /dev/null +++ b/src/Planners/src/ContactPhaseList.cpp @@ -0,0 +1,199 @@ +/** + * @file ContactPhase.cpp + * @authors Stefano Dafarra + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +#include +#include +#include +#include + +using namespace BipedalLocomotion::Planners; + +void BipedalLocomotion::Planners::ContactPhaseList::createPhases() +{ + m_phases.clear(); + + std::map> activations, deactivations; + + for (ContactListMap::iterator list = m_contactLists.begin(); list != m_contactLists.end(); ++list) + { + const std::string& key = list->first; + for (ContactList::const_iterator step = list->second.begin(); step != list->second.end(); ++step) + { + ContactReference newReference; + newReference.listLabel = key; + newReference.contact_it = step; + + activations[step->activationTime].push_back(newReference); + deactivations[step->deactivationTime].push_back(newReference); + } + } + + if (!activations.size()) + { + return; + } + + ContactPhase currentPhase; + currentPhase.beginTime = activations.begin()->first; + currentPhase.activeContacts = activations.begin()->second; + + activations.erase(activations.begin()); + + while (activations.size()) + { + if (deactivations.begin()->first <= activations.begin()->first) + { + //Here I need to remove from the current phase the contacts that are going to end + currentPhase.endTime = deactivations.begin()->first; + m_phases.push_back(currentPhase); + + currentPhase.beginTime = deactivations.begin()->first; + const std::vector& toBeRemoved = deactivations.begin()->second; + + //The following lambda returns true if the label inside reference is contained in toBeRemoved + auto deleteLambda = [toBeRemoved](const ContactReference &reference) + { + const std::string& key = reference.listLabel; + std::vector::const_iterator it = std::find_if(toBeRemoved.begin(), toBeRemoved.end(), + [key](const ContactReference& ref){return ref.listLabel == key;}); + + return it != toBeRemoved.end(); + }; + + //Remove from activeContacts all the elements for which deleteLambda returns true. + currentPhase.activeContacts.erase(std::remove_if(currentPhase.activeContacts.begin(), + currentPhase.activeContacts.end(), + deleteLambda), currentPhase.activeContacts.end()); + + deactivations.erase(deactivations.begin()); + + if (deactivations.begin()->first == activations.begin()->first) + { + currentPhase.activeContacts.insert(currentPhase.activeContacts.end(), + activations.begin()->second.begin(), + activations.begin()->second.end()); //Add the new contacts to the list. + + activations.erase(activations.begin()); + } + } + else // (activations.begin()->first < deactivations.begin()->first) + { + currentPhase.endTime = activations.begin()->first; + m_phases.push_back(currentPhase); + + currentPhase.beginTime = activations.begin()->first; + currentPhase.activeContacts.insert(currentPhase.activeContacts.end(), + activations.begin()->second.begin(), + activations.begin()->second.end()); //Add the new contacts to the list. + + activations.erase(activations.begin()); + } + } + + assert(deactivations.size() == 1); //Only one element should be left (deactivations and activations were equal in number, but the head of activations was deleted at the beginning). + currentPhase.endTime = deactivations.begin()->first; + m_phases.push_back(currentPhase); +} + +void ContactPhaseList::setLists(const ContactListMap &contactLists) +{ + m_contactLists = contactLists; + createPhases(); +} + +bool ContactPhaseList::setLists(const std::initializer_list &contactLists) +{ + m_contactLists.clear(); + for (const ContactList& list : contactLists) + { + std::pair res = m_contactLists.insert(ContactListMap::value_type(list.defaultName(), list)); + + if (!res.second) + { + std::cerr << "[ContactPhaseList::setLists] Multiple items have the same defaultName." < + +#include + +using namespace BipedalLocomotion::Planners; + +TEST_CASE("ContactPhaseList") +{ + ContactPhaseList phaseList; + + SECTION("Set from map") + { + ContactListMap contactListMap; + REQUIRE(contactListMap["left"].addContact(iDynTree::Transform::Identity(), 0.0, 1.0)); + REQUIRE(contactListMap["left"].addContact(iDynTree::Transform::Identity(), 2.0, 5.0)); + REQUIRE(contactListMap["left"].addContact(iDynTree::Transform::Identity(), 6.0, 7.0)); + + REQUIRE(contactListMap["right"].addContact(iDynTree::Transform::Identity(), 0.0, 3.0)); + REQUIRE(contactListMap["right"].addContact(iDynTree::Transform::Identity(), 4.0, 7.0)); + + phaseList.setLists(contactListMap); + } + + ContactList contactListLeft, contactListRight; + contactListLeft.setDefaultName("left"); + contactListRight.setDefaultName("right"); + + REQUIRE(contactListLeft.addContact(iDynTree::Transform::Identity(), 0.0, 1.0)); + REQUIRE(contactListLeft.addContact(iDynTree::Transform::Identity(), 2.0, 5.0)); + REQUIRE(contactListLeft.addContact(iDynTree::Transform::Identity(), 6.0, 7.0)); + + REQUIRE(contactListRight.addContact(iDynTree::Transform::Identity(), 0.0, 3.0)); + REQUIRE(contactListRight.addContact(iDynTree::Transform::Identity(), 4.0, 7.0)); + + REQUIRE(phaseList.setLists({contactListLeft, contactListRight})); + + SECTION("Check phases") + { + REQUIRE(phaseList.size() == 7); + + const ContactListMap& contactListMap = phaseList.lists(); + ContactList::const_iterator expectedLeft = contactListMap.at("left").begin(); + ContactList::const_iterator expectedRight = contactListMap.at("right").begin(); + + ContactPhaseList::const_iterator phase = phaseList.begin(); + REQUIRE(phase->beginTime == 0.0); + REQUIRE(phase->endTime == 1.0); + bool ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + REQUIRE(ok); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + expectedLeft++; + + REQUIRE(phase->beginTime == 1.0); + REQUIRE(phase->endTime == 2.0); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + + REQUIRE(phase->beginTime == 2.0); + REQUIRE(phase->endTime == 3.0); + ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + REQUIRE(ok); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + expectedRight++; + + REQUIRE(phase->beginTime == 3.0); + REQUIRE(phase->endTime == 4.0); + ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + REQUIRE(ok); + + phase++; + + REQUIRE(phase->beginTime == 4.0); + REQUIRE(phase->endTime == 5.0); + ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + REQUIRE(ok); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + expectedLeft++; + + REQUIRE(phase->beginTime == 5.0); + REQUIRE(phase->endTime == 6.0); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + + REQUIRE(phase->beginTime == 6.0); + REQUIRE(phase->endTime == 7.0); + ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + REQUIRE(ok); + ok = phase->getContactGivenList("right")->contact_it == expectedRight; + REQUIRE(ok); + + phase++; + expectedLeft++; + expectedRight++; + + ok = phase == phaseList.end(); + REQUIRE(ok); + ok = expectedLeft == contactListMap.at("left").end(); + REQUIRE(ok); + ok = expectedRight == contactListMap.at("right").end(); + REQUIRE(ok); + } + +} From d7f7d9c5ae4725d50e466a93a09b533bc7937fef Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 10:29:23 +0200 Subject: [PATCH 04/10] Fixed while loop condition when computing the phases. --- src/Planners/src/ContactPhaseList.cpp | 6 ++-- src/Planners/tests/ContactPhaseListTest.cpp | 35 +++++++++++++++++++-- 2 files changed, 35 insertions(+), 6 deletions(-) diff --git a/src/Planners/src/ContactPhaseList.cpp b/src/Planners/src/ContactPhaseList.cpp index e8abeacf5d..4bb0eb5505 100644 --- a/src/Planners/src/ContactPhaseList.cpp +++ b/src/Planners/src/ContactPhaseList.cpp @@ -45,9 +45,9 @@ void BipedalLocomotion::Planners::ContactPhaseList::createPhases() activations.erase(activations.begin()); - while (activations.size()) + while ((activations.size() + deactivations.size()) > 1) { - if (deactivations.begin()->first <= activations.begin()->first) + if ((activations.size() == 0) || (deactivations.begin()->first <= activations.begin()->first)) { //Here I need to remove from the current phase the contacts that are going to end currentPhase.endTime = deactivations.begin()->first; @@ -73,7 +73,7 @@ void BipedalLocomotion::Planners::ContactPhaseList::createPhases() deactivations.erase(deactivations.begin()); - if (deactivations.begin()->first == activations.begin()->first) + if (activations.size() && (deactivations.begin()->first == activations.begin()->first)) { currentPhase.activeContacts.insert(currentPhase.activeContacts.end(), activations.begin()->second.begin(), diff --git a/src/Planners/tests/ContactPhaseListTest.cpp b/src/Planners/tests/ContactPhaseListTest.cpp index 8fe2e3b5dc..ffad3b33e7 100644 --- a/src/Planners/tests/ContactPhaseListTest.cpp +++ b/src/Planners/tests/ContactPhaseListTest.cpp @@ -29,9 +29,10 @@ TEST_CASE("ContactPhaseList") phaseList.setLists(contactListMap); } - ContactList contactListLeft, contactListRight; + ContactList contactListLeft, contactListRight, contactListAdditional; contactListLeft.setDefaultName("left"); contactListRight.setDefaultName("right"); + contactListAdditional.setDefaultName("additional"); REQUIRE(contactListLeft.addContact(iDynTree::Transform::Identity(), 0.0, 1.0)); REQUIRE(contactListLeft.addContact(iDynTree::Transform::Identity(), 2.0, 5.0)); @@ -40,19 +41,25 @@ TEST_CASE("ContactPhaseList") REQUIRE(contactListRight.addContact(iDynTree::Transform::Identity(), 0.0, 3.0)); REQUIRE(contactListRight.addContact(iDynTree::Transform::Identity(), 4.0, 7.0)); - REQUIRE(phaseList.setLists({contactListLeft, contactListRight})); + REQUIRE(contactListAdditional.addContact(iDynTree::Transform::Identity(), 4.0, 5.0)); + REQUIRE(contactListAdditional.addContact(iDynTree::Transform::Identity(), 6.0, 7.5)); + + REQUIRE(phaseList.setLists({contactListAdditional, contactListLeft, contactListRight})); SECTION("Check phases") { - REQUIRE(phaseList.size() == 7); + REQUIRE(phaseList.size() == 8); const ContactListMap& contactListMap = phaseList.lists(); ContactList::const_iterator expectedLeft = contactListMap.at("left").begin(); ContactList::const_iterator expectedRight = contactListMap.at("right").begin(); + ContactList::const_iterator expectedAdditional = contactListMap.at("additional").begin(); + ContactPhaseList::const_iterator phase = phaseList.begin(); REQUIRE(phase->beginTime == 0.0); REQUIRE(phase->endTime == 1.0); + REQUIRE(phase->activeContacts.size() == 2); bool ok = phase->getContactGivenList("left")->contact_it == expectedLeft; REQUIRE(ok); ok = phase->getContactGivenList("right")->contact_it == expectedRight; @@ -63,6 +70,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 1.0); REQUIRE(phase->endTime == 2.0); + REQUIRE(phase->activeContacts.size() == 1); ok = phase->getContactGivenList("right")->contact_it == expectedRight; REQUIRE(ok); @@ -70,6 +78,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 2.0); REQUIRE(phase->endTime == 3.0); + REQUIRE(phase->activeContacts.size() == 2); ok = phase->getContactGivenList("left")->contact_it == expectedLeft; REQUIRE(ok); ok = phase->getContactGivenList("right")->contact_it == expectedRight; @@ -80,6 +89,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 3.0); REQUIRE(phase->endTime == 4.0); + REQUIRE(phase->activeContacts.size() == 1); ok = phase->getContactGivenList("left")->contact_it == expectedLeft; REQUIRE(ok); @@ -87,16 +97,21 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 4.0); REQUIRE(phase->endTime == 5.0); + REQUIRE(phase->activeContacts.size() == 3); ok = phase->getContactGivenList("left")->contact_it == expectedLeft; REQUIRE(ok); ok = phase->getContactGivenList("right")->contact_it == expectedRight; REQUIRE(ok); + ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + REQUIRE(ok); phase++; expectedLeft++; + expectedAdditional++; REQUIRE(phase->beginTime == 5.0); REQUIRE(phase->endTime == 6.0); + REQUIRE(phase->activeContacts.size() == 1); ok = phase->getContactGivenList("right")->contact_it == expectedRight; REQUIRE(ok); @@ -104,21 +119,35 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 6.0); REQUIRE(phase->endTime == 7.0); + REQUIRE(phase->activeContacts.size() == 3); ok = phase->getContactGivenList("left")->contact_it == expectedLeft; REQUIRE(ok); ok = phase->getContactGivenList("right")->contact_it == expectedRight; REQUIRE(ok); + ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + REQUIRE(ok); phase++; expectedLeft++; expectedRight++; + REQUIRE(phase->beginTime == 7.0); + REQUIRE(phase->endTime == 7.5); + REQUIRE(phase->activeContacts.size() == 1); + ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + REQUIRE(ok); + + phase++; + expectedAdditional++; + ok = phase == phaseList.end(); REQUIRE(ok); ok = expectedLeft == contactListMap.at("left").end(); REQUIRE(ok); ok = expectedRight == contactListMap.at("right").end(); REQUIRE(ok); + ok = expectedAdditional == contactListMap.at("additional").end(); + REQUIRE(ok); } } From 302ab345737dd3a21d5d5a9878ef5033d680bdb9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 10:29:47 +0200 Subject: [PATCH 05/10] Added documentaiton for ContactList and renamed some methods. --- .../BipedalLocomotion/Planners/ContactList.h | 146 +++++++++++++++--- src/Planners/src/ContactList.cpp | 19 ++- src/Planners/tests/ContactListTest.cpp | 16 +- 3 files changed, 148 insertions(+), 33 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h index 9a96eba75c..a362365064 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h @@ -20,69 +20,179 @@ namespace Planners { /** - * @brief Class containing a list of steps. - * The steps are added such that the activation time is strictly growing. + * @brief Class containing a list of contacts. + * The contact are added such that the activation time is strictly growing. In addition, contacts cannot be overlapping. * It represents a series of contact activations and deactivations of a single entity. - * Once items are inserted, items cannot change since they are ordered according to their timing. */ class ContactList { + /** + * @brief Struct used for inserting new contacts in the set. + */ struct ContactCompare { - bool operator()(const Contact& lhs, const Contact& rhs) const { - return lhs.deactivationTime < rhs.activationTime; - } + bool operator()(const Contact& lhs, const Contact& rhs) const; }; - std::set m_contacts; - std::string m_defaultName{"ContactList"}; - ContactType m_defaultContactType{ContactType::FULL}; + std::set m_contacts; /** Data structure for inserting and ordering contacts. **/ + std::string m_defaultName{"ContactList"}; /** Default name for the contact list. **/ + ContactType m_defaultContactType{ContactType::FULL}; /** Default contact type. **/ public: using const_iterator = std::set::const_iterator; using const_reverse_iterator = std::set::const_reverse_iterator; + /** + * @brief Set the default name. + * @param defaultName the default name. + */ void setDefaultName(const std::string& defaultName); + /** + * @brief Get the default name. + * @return the default name. + */ const std::string& defaultName() const; + /** + * @brief Set the default contact type. + * @param The default contact type. + */ void setDefaultContactType(const ContactType& type); + /** + * @brief Get the default contact type. + * @return the default contact type. + */ const ContactType& defaultContactType() const; + /** + * @brief Add a new contact to the list. + * @param newContact The new contact + * @return false if it was not possible to insert the contact. + * Possible failures: the activation time is greater than the deactivation time, or the new contact ovelaps with an existing contact. + */ bool addContact(const Contact& newContact); + /** + * @brief Add a new contact to the list. + * + * It uses the defaultName and the defaultContactType for the missing informations. + * @param newTransform The contact pose. + * @param activationTime The activation time. + * @param deactivationTime The deactivation time. + * @return false if it was not possible to insert the contact. + * Possible failures: the activation time is greater than the deactivation time, or the new contact ovelaps with an existing contact. + */ bool addContact(const iDynTree::Transform& newTransform, double activationTime, double deactivationTime); + /** + * @brief Erase a contact + * @param iterator to the contact to erase. + * @return an iterator to the contact that follows the one removed. + */ const_iterator erase(const_iterator iterator); + /** + * @brief Return a const iterator to the begin of the contacts. + */ const_iterator begin() const; + + /** + * @brief Return a const iterator to the begin of the contacts. + */ const_iterator cbegin() const; + /** + * @brief Return a const reverse iterator to the contacts (basically starting from the last contact going backward). + */ const_reverse_iterator rbegin() const; + + /** + * @brief Return a const reverse iterator to the contacts (basically starting from the last contact going backward). + */ const_reverse_iterator crbegin() const; + /** + * @brief Return a const iterator to the end of the list. + * + * This is only a placeholder, it does not reference any contact. + */ const_iterator end() const; + + /** + * @brief Return a const iterator to the end of the list. + * + * This is only a placeholder, it does not reference any contact. + */ const_iterator cend() const; + /** + * @brief Return a const reverse iterator to the end of the list. + * + * This is only a placeholder, it does not reference any contact. + */ const_reverse_iterator rend() const; + + /** + * @brief Return a const reverse iterator to the end of the list. + * + * This is only a placeholder, it does not reference any contact. + */ const_reverse_iterator crend() const; + /** + * @brief Get the size of the list. + * @return The number of contacts. + */ size_t size() const; - const_iterator firstStep() const; - - const_iterator lastStep() const; - + /** + * @brief Iterator pointing to the first contact. + */ + const_iterator firstContact() const; + + /** + * @brief Iterator pointing to the last contact. + */ + const_iterator lastContact() const; + + /** + * @brief Edit an existing contact. + * @param element Iterator to the element to edit. + * @param newContact The new contact + * @return false if the element is not valid or if the new contact timing would require a reordering of the list. + */ bool editContact(const_iterator element, const Contact& newContact); - const_iterator getPresentStep(double time) const; - - bool keepOnlyPresentStep(double time); - + /** + * @brief Get the contact given the time. + * + * It returns the contact with the highest activation time lower than time. + * 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. + * If no contact satisfy this condition, it returns a pointer to the end. + */ + const_iterator getPresentContact(double time) const; + + /** + * @brief Clear all the steps, except the one returned by getPresentContact + * @param time The present time. + * @return false if no contact is available at this time. + */ + bool keepOnlyPresentContact(double time); + + /** + * @brief Clear the contacts. + */ void clear(); - void removeLastStep(); + /** + * @brief Remove only the last contact. + */ + void removeLastContact(); }; diff --git a/src/Planners/src/ContactList.cpp b/src/Planners/src/ContactList.cpp index cec6b87dd8..80b4420567 100644 --- a/src/Planners/src/ContactList.cpp +++ b/src/Planners/src/ContactList.cpp @@ -10,6 +10,11 @@ using namespace BipedalLocomotion::Planners; +bool ContactList::ContactCompare::operator()(const Contact &lhs, const Contact &rhs) const +{ + return lhs.deactivationTime < rhs.activationTime; +} + void ContactList::setDefaultName(const std::string &defaultName) { m_defaultName = defaultName; @@ -116,12 +121,12 @@ size_t ContactList::size() const return m_contacts.size(); } -ContactList::const_iterator ContactList::firstStep() const +ContactList::const_iterator ContactList::firstContact() const { return begin(); } -ContactList::const_iterator ContactList::lastStep() const +ContactList::const_iterator ContactList::lastContact() const { return --end(); } @@ -162,7 +167,7 @@ bool ContactList::editContact(ContactList::const_iterator element, const Contact return true; } -ContactList::const_iterator ContactList::getPresentStep(double time) const +ContactList::const_iterator ContactList::getPresentContact(double time) const { // With the reverse iterator we find the last step such that the activation time is smaller that time ContactList::const_reverse_iterator presentReverse = std::find_if(rbegin(), rend(), @@ -176,9 +181,9 @@ ContactList::const_iterator ContactList::getPresentStep(double time) const return --(presentReverse.base()); //This is to convert a reverse iterator to a forward iterator. The -- is because base() returns a forward iterator to the next element. } -bool ContactList::keepOnlyPresentStep(double time) +bool ContactList::keepOnlyPresentContact(double time) { - ContactList::const_iterator dropPoint = getPresentStep(time); + ContactList::const_iterator dropPoint = getPresentContact(time); if (dropPoint == end()) { @@ -199,8 +204,8 @@ void ContactList::clear() m_contacts.clear(); } -void ContactList::removeLastStep() +void ContactList::removeLastContact() { - erase(lastStep()); + erase(lastContact()); } diff --git a/src/Planners/tests/ContactListTest.cpp b/src/Planners/tests/ContactListTest.cpp index 7bf39e4788..aa7316ce4a 100644 --- a/src/Planners/tests/ContactListTest.cpp +++ b/src/Planners/tests/ContactListTest.cpp @@ -46,8 +46,8 @@ TEST_CASE("ContactList") SECTION("Insertion order") { - REQUIRE(contactsAreEqual(p1, *list.firstStep())); - REQUIRE(contactsAreEqual(p2, *list.lastStep())); + REQUIRE(contactsAreEqual(p1, *list.firstContact())); + REQUIRE(contactsAreEqual(p2, *list.lastContact())); Contact p3; p3.activationTime = 0.6; @@ -78,16 +78,16 @@ TEST_CASE("ContactList") p2Modified = p2; p2Modified.type = ContactType::POINT; - REQUIRE(list.editContact(list.lastStep(), p2Modified)); - REQUIRE(contactsAreEqual(p2Modified, *list.lastStep())); + REQUIRE(list.editContact(list.lastContact(), p2Modified)); + REQUIRE(contactsAreEqual(p2Modified, *list.lastContact())); } SECTION("Present step") { - REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.2))); - REQUIRE(contactsAreEqual(p2, *list.getPresentStep(1.6))); - REQUIRE(contactsAreEqual(p1, *list.getPresentStep(0.6))); - bool same = list.getPresentStep(0.0) == list.end(); + REQUIRE(contactsAreEqual(p2, *list.getPresentContact(1.2))); + REQUIRE(contactsAreEqual(p2, *list.getPresentContact(1.6))); + REQUIRE(contactsAreEqual(p1, *list.getPresentContact(0.6))); + bool same = list.getPresentContact(0.0) == list.end(); REQUIRE(same); } From af8d3efd5ddf068e08a98244218e2057ad6a9db2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 10:58:45 +0200 Subject: [PATCH 06/10] Added documentation of ContactPhaseList. --- .../BipedalLocomotion/Planners/ContactList.h | 5 ++ .../BipedalLocomotion/Planners/ContactPhase.h | 31 ++++++- .../Planners/ContactPhaseList.h | 82 ++++++++++++++++++- 3 files changed, 112 insertions(+), 6 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h index a362365064..e981c5ff8b 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h @@ -8,8 +8,13 @@ #ifndef BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H #define BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H +// BipedalLocomotion #include + +//iDynTree #include + +//std #include #include #include diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h index 7b46d1be08..9f2a8cb170 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h @@ -18,22 +18,49 @@ namespace BipedalLocomotion namespace Planners { +/** + * @brief Utility struct to store a reference to a contact. + */ struct ContactReference { - std::string listLabel; - ContactList::const_iterator contact_it; + std::string listLabel; /** Label to indicate the corresponding list. **/ + ContactList::const_iterator contact_it; /** Const iterator to a contact. **/ }; +/** + * @brief Struct defining a contact phase. + * Each phase is characterized by a set of contacts which remain active for the entirety of the phase. + */ struct ContactPhase { + /** + * @brief The phase initial time. + **/ double beginTime {0.0}; + /** + * @brief The phase end time. + **/ double endTime {0.0}; + /** + * @brief The set of contacts active during the phase. + */ std::vector activeContacts; + /** + * @brief Utility function to check if a list is present amongst the active contacts. + * @param key The label of the list to be checked. + * @return True if key is present amongst the active contacts. + **/ bool isListIncluded(const std::string& key) const; + /** + * @brief Utility function to retrieve the an iterator to an active contact given the list label + * @param key The label of interest + * @return An iterator to the element inside activeContacts containing the desired key. + * If no contact with the desired label is present, it returns an iterator to the end of the vector. + */ std::vector::const_iterator getContactGivenList(const std::string& key) const; }; diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h index 54fb7ba1cd..aa5f857060 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h @@ -20,49 +20,123 @@ namespace BipedalLocomotion { namespace Planners { - +/** + * @brief Utility alias to a map of ContacLists. + */ using ContactListMap = std::unordered_map; +/** + * @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 { - ContactListMap m_contactLists; + ContactListMap m_contactLists; /** The input contact lists. **/ - std::vector m_phases; + std::vector m_phases; /** The computed phases. **/ - void createPhases(); + 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; + /** + * @brief Set the input lists + * @param contactLists The set of lists to be used for computing the phases. + */ void setLists(const ContactListMap& contactLists); + /** + * @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. + * @return False if some lists have the same defaultName. + */ bool setLists(const std::initializer_list& contactLists); + /** + * @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. + * @return A const reference to the input lists. + */ const ContactListMap& lists() const; + /** + * @brief Const iterator to the begin of the phases. + */ const_iterator begin() const; + + /** + * @brief Const iterator to the begin of the phases. + */ const_iterator cbegin() const; + /** + * @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). + */ const_reverse_iterator crbegin() const; + /** + * @brief Return a const iterator to the end of the list. + * + * This is only a placeholder, it does not reference any phase. + */ const_iterator end() const; + + /** + * @brief Return a const iterator to the end of the list. + * + * This is only a placeholder, it does not reference any phase. + */ const_iterator cend() const; + /** + * @brief Return a const reverse iterator to the end of the list. + * + * This is only a placeholder, it does not reference any phase. + */ const_reverse_iterator rend() const; + + /** + * @brief Return a const reverse iterator to the end of the list. + * + * This is only a placeholder, it does not reference any phase. + */ const_reverse_iterator crend() const; + /** + * @brief Access phases by index. + * @param index of the phase to be accessed. + * @return A const reference to the desired phase. + */ const ContactPhase& operator[](size_t index) const; + /** + * @brief A const iterator to the first phase. + */ const_iterator firstPhase() const; + /** + * @brief A const iterator to the last phase. + */ const_iterator lastPhase() const; + /** + * @brief Get the number of phases. + */ size_t size() const; + /** + * @brief Clear the phases and the stored lists. + */ void clear(); }; From b909bff08481e169d094bf0bfe1839240ced7657 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 11:06:49 +0200 Subject: [PATCH 07/10] Updated Changelog and add possibility not to compile Planners libraries. --- CHANGELOG.md | 3 ++- ...lLocomotionFrameworkFindDependencies.cmake | 4 ++++ src/Planners/CMakeLists.txt | 24 +++++++++++-------- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 19977613e4..6f2a15d03b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ All notable changes to this project are documented in this file. - Implement `ParametersHandler` library (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/13) - Implement `GenericContainer::Vector` (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/29) - Implement `Estimators` library (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/23) -- Renamed from ``bipedal-locomotion-controllers`` to ``bipedal-locomotion-framework``. +- Renamed from ``bipedal-locomotion-controllers`` to ``bipedal-locomotion-framework`` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/40). +- Implement `Contact` library. [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/ diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index d7543af92d..df896da1d2 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -139,3 +139,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_YarpImplementation framework_dependent_option(FRAMEWORK_COMPILE_Estimators "Compile Estimators library?" ON "FRAMEWORK_HAS_Eigen3" OFF) + +framework_dependent_option(FRAMEWORK_COMPILE_Planners + "Compile Planners libraries?" ON + "FRAMEWORK_HAS_Eigen3" OFF) diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 52e472b32e..b98c670526 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -2,13 +2,17 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -set(H_PREFIX include/BipedalLocomotion/Planners) - -add_bipedal_locomotion_library( - NAME Contact - PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h - SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp - PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core - INSTALLATION_FOLDER Planners - SUBDIRECTORIES tests - ) +if (FRAMEWORK_COMPILE_Planners) + + set(H_PREFIX include/BipedalLocomotion/Planners) + + add_bipedal_locomotion_library( + NAME Contact + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h + SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp + PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core + INSTALLATION_FOLDER Planners + SUBDIRECTORIES tests + ) + +endif() From 8cf6b976fc7f2ca33000c6fb16202c17ab530342 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 17:47:39 +0200 Subject: [PATCH 08/10] Added direct accessor to contacts the list. --- .../BipedalLocomotion/Planners/ContactList.h | 8 +++++++ src/Planners/src/ContactList.cpp | 21 +++++++++++++++++++ src/Planners/tests/ContactListTest.cpp | 19 +++++++++++++++++ 3 files changed, 48 insertions(+) diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h index e981c5ff8b..5cd82389df 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactList.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactList.h @@ -146,6 +146,14 @@ class ContactList */ const_reverse_iterator crend() const; + /** + * @brief Access contacts by index. + * @warning This method in a for loop is much less efficient than using iterators. + * @param index of the phase to be accessed. + * @return A const reference to the desired contact. + */ + const Contact& operator[](size_t index) const; + /** * @brief Get the size of the list. * @return The number of contacts. diff --git a/src/Planners/src/ContactList.cpp b/src/Planners/src/ContactList.cpp index 80b4420567..a4646e76d9 100644 --- a/src/Planners/src/ContactList.cpp +++ b/src/Planners/src/ContactList.cpp @@ -7,6 +7,9 @@ #include #include +#include +#include +#include using namespace BipedalLocomotion::Planners; @@ -116,6 +119,24 @@ ContactList::const_reverse_iterator ContactList::crend() const return m_contacts.crend(); } +const Contact &ContactList::operator[](size_t index) const +{ + assert(index < size()); + + if (index > std::ceil(size()/2)) + { + ContactList::const_reverse_iterator it = rbegin(); + std::advance(it, size() - index - 1); + return *(it); + } + else + { + ContactList::const_iterator it = begin(); + std::advance(it, index); + return *(it); + } +} + size_t ContactList::size() const { return m_contacts.size(); diff --git a/src/Planners/tests/ContactListTest.cpp b/src/Planners/tests/ContactListTest.cpp index aa7316ce4a..9d48b188fb 100644 --- a/src/Planners/tests/ContactListTest.cpp +++ b/src/Planners/tests/ContactListTest.cpp @@ -96,4 +96,23 @@ TEST_CASE("ContactList") list.clear(); REQUIRE(list.size() == 0); } + + SECTION("Accessor") + { + bool ok = true; + for (size_t i = 0; i < 50; ++i) + { + ok = ok && list.addContact(iDynTree::Transform::Identity(), 2.0 + i, 2.5 + i); + } + REQUIRE(ok); + REQUIRE(list.size() == 52); + + ContactList::const_iterator it = list.begin(); + for (size_t i = 0; i < list.size(); ++i) + { + ok = ok && contactsAreEqual(list[i], *it); + it++; + } + REQUIRE(ok); + } } From ab4e9d7a5c4a101ca8630b96af1052bf8a3c95e5 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 19 May 2020 18:08:07 +0200 Subject: [PATCH 09/10] Attempt workaround on ci. Using a fancy way to perform the ceiling of a division of integers. Adding the links to the PRs in the changelog. --- .github/workflows/ci.yml | 3 ++- CHANGELOG.md | 2 +- src/Planners/src/ContactList.cpp | 3 +-- src/Planners/tests/ContactListTest.cpp | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7a93490cca..d6f2399242 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -81,7 +81,8 @@ jobs: uses: actions/cache@v1 with: path: ${{ github.workspace }}/install/deps - key: source-deps-${{ runner.os }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }} + # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45 + key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }} - name: Source-based Dependencies [Windows] diff --git a/CHANGELOG.md b/CHANGELOG.md index 6f2a15d03b..9fa9baf7b2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,6 @@ All notable changes to this project are documented in this file. - Implement `GenericContainer::Vector` (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/29) - Implement `Estimators` library (https://github.com/dic-iit/bipedal-locomotion-controllers/pull/23) - Renamed from ``bipedal-locomotion-controllers`` to ``bipedal-locomotion-framework`` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/40). -- Implement `Contact` library. +- Implement `Contact` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/43 and https://github.com/dic-iit/bipedal-locomotion-framework/pull/45) [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/ diff --git a/src/Planners/src/ContactList.cpp b/src/Planners/src/ContactList.cpp index a4646e76d9..e30e8b4e07 100644 --- a/src/Planners/src/ContactList.cpp +++ b/src/Planners/src/ContactList.cpp @@ -9,7 +9,6 @@ #include #include #include -#include using namespace BipedalLocomotion::Planners; @@ -123,7 +122,7 @@ const Contact &ContactList::operator[](size_t index) const { assert(index < size()); - if (index > std::ceil(size()/2)) + if (index > (size() / 2 + (size() % 2 != 0))) //fancy way for doing std::ceil(size() / 2.0) { ContactList::const_reverse_iterator it = rbegin(); std::advance(it, size() - index - 1); diff --git a/src/Planners/tests/ContactListTest.cpp b/src/Planners/tests/ContactListTest.cpp index 9d48b188fb..bfadb3bcfd 100644 --- a/src/Planners/tests/ContactListTest.cpp +++ b/src/Planners/tests/ContactListTest.cpp @@ -100,12 +100,12 @@ TEST_CASE("ContactList") SECTION("Accessor") { bool ok = true; - for (size_t i = 0; i < 50; ++i) + for (size_t i = 0; i < 49; ++i) { ok = ok && list.addContact(iDynTree::Transform::Identity(), 2.0 + i, 2.5 + i); } REQUIRE(ok); - REQUIRE(list.size() == 52); + REQUIRE(list.size() == 51); ContactList::const_iterator it = list.begin(); for (size_t i = 0; i < list.size(); ++i) From 546052a21edd899518f127f34ece52495f75fd4e Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 21 May 2020 11:05:47 +0200 Subject: [PATCH 10/10] Using an unordered_map instead of vector in the ContactPhase. --- .../BipedalLocomotion/Planners/ContactPhase.h | 21 ++---------- src/Planners/src/ContactPhase.cpp | 9 +---- src/Planners/src/ContactPhaseList.cpp | 34 +++++-------------- src/Planners/tests/ContactPhaseListTest.cpp | 28 +++++++-------- 4 files changed, 25 insertions(+), 67 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h index 9f2a8cb170..557e9f0944 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h +++ b/src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h @@ -10,7 +10,7 @@ #include #include -#include +#include #include namespace BipedalLocomotion @@ -18,15 +18,6 @@ namespace BipedalLocomotion namespace Planners { -/** - * @brief Utility struct to store a reference to a contact. - */ -struct ContactReference -{ - std::string listLabel; /** Label to indicate the corresponding list. **/ - ContactList::const_iterator contact_it; /** Const iterator to a contact. **/ -}; - /** * @brief Struct defining a contact phase. * Each phase is characterized by a set of contacts which remain active for the entirety of the phase. @@ -46,7 +37,7 @@ struct ContactPhase /** * @brief The set of contacts active during the phase. */ - std::vector activeContacts; + std::unordered_map activeContacts; /** * @brief Utility function to check if a list is present amongst the active contacts. @@ -54,14 +45,6 @@ struct ContactPhase * @return True if key is present amongst the active contacts. **/ bool isListIncluded(const std::string& key) const; - - /** - * @brief Utility function to retrieve the an iterator to an active contact given the list label - * @param key The label of interest - * @return An iterator to the element inside activeContacts containing the desired key. - * If no contact with the desired label is present, it returns an iterator to the end of the vector. - */ - std::vector::const_iterator getContactGivenList(const std::string& key) const; }; } diff --git a/src/Planners/src/ContactPhase.cpp b/src/Planners/src/ContactPhase.cpp index 1c914ec6f2..8837b0c3cb 100644 --- a/src/Planners/src/ContactPhase.cpp +++ b/src/Planners/src/ContactPhase.cpp @@ -12,13 +12,6 @@ using namespace BipedalLocomotion::Planners; bool ContactPhase::isListIncluded(const std::string &key) const { - std::vector::const_iterator it = getContactGivenList(key); - - return it != activeContacts.end(); + return activeContacts.find(key) != activeContacts.end(); } -std::vector::const_iterator ContactPhase::getContactGivenList(const std::string &key) const -{ - return std::find_if(activeContacts.begin(), activeContacts.end(), - [key](const ContactReference& ref){return ref.listLabel == key;}); -} diff --git a/src/Planners/src/ContactPhaseList.cpp b/src/Planners/src/ContactPhaseList.cpp index 4bb0eb5505..44252dc505 100644 --- a/src/Planners/src/ContactPhaseList.cpp +++ b/src/Planners/src/ContactPhaseList.cpp @@ -10,7 +10,6 @@ #include #include #include -#include using namespace BipedalLocomotion::Planners; @@ -18,19 +17,15 @@ void BipedalLocomotion::Planners::ContactPhaseList::createPhases() { m_phases.clear(); - std::map> activations, deactivations; + std::map> activations, deactivations; for (ContactListMap::iterator list = m_contactLists.begin(); list != m_contactLists.end(); ++list) { const std::string& key = list->first; for (ContactList::const_iterator step = list->second.begin(); step != list->second.end(); ++step) { - ContactReference newReference; - newReference.listLabel = key; - newReference.contact_it = step; - - activations[step->activationTime].push_back(newReference); - deactivations[step->deactivationTime].push_back(newReference); + activations[step->activationTime][key] = step; //By construction, there is only a step given a key and activationTime + deactivations[step->deactivationTime][key] = step; } } @@ -54,29 +49,17 @@ void BipedalLocomotion::Planners::ContactPhaseList::createPhases() m_phases.push_back(currentPhase); currentPhase.beginTime = deactivations.begin()->first; - const std::vector& toBeRemoved = deactivations.begin()->second; - //The following lambda returns true if the label inside reference is contained in toBeRemoved - auto deleteLambda = [toBeRemoved](const ContactReference &reference) + for (auto& toBeRemoved : deactivations.begin()->second) { - const std::string& key = reference.listLabel; - std::vector::const_iterator it = std::find_if(toBeRemoved.begin(), toBeRemoved.end(), - [key](const ContactReference& ref){return ref.listLabel == key;}); - - return it != toBeRemoved.end(); - }; - - //Remove from activeContacts all the elements for which deleteLambda returns true. - currentPhase.activeContacts.erase(std::remove_if(currentPhase.activeContacts.begin(), - currentPhase.activeContacts.end(), - deleteLambda), currentPhase.activeContacts.end()); + currentPhase.activeContacts.erase(toBeRemoved.first); //Erase all the contacts which are deactivativated in this instant + } deactivations.erase(deactivations.begin()); if (activations.size() && (deactivations.begin()->first == activations.begin()->first)) { - currentPhase.activeContacts.insert(currentPhase.activeContacts.end(), - activations.begin()->second.begin(), + currentPhase.activeContacts.insert(activations.begin()->second.begin(), activations.begin()->second.end()); //Add the new contacts to the list. activations.erase(activations.begin()); @@ -88,8 +71,7 @@ void BipedalLocomotion::Planners::ContactPhaseList::createPhases() m_phases.push_back(currentPhase); currentPhase.beginTime = activations.begin()->first; - currentPhase.activeContacts.insert(currentPhase.activeContacts.end(), - activations.begin()->second.begin(), + currentPhase.activeContacts.insert(activations.begin()->second.begin(), activations.begin()->second.end()); //Add the new contacts to the list. activations.erase(activations.begin()); diff --git a/src/Planners/tests/ContactPhaseListTest.cpp b/src/Planners/tests/ContactPhaseListTest.cpp index ffad3b33e7..f624e54948 100644 --- a/src/Planners/tests/ContactPhaseListTest.cpp +++ b/src/Planners/tests/ContactPhaseListTest.cpp @@ -60,9 +60,9 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 0.0); REQUIRE(phase->endTime == 1.0); REQUIRE(phase->activeContacts.size() == 2); - bool ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + bool ok = phase->activeContacts.at("left") == expectedLeft; REQUIRE(ok); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); phase++; @@ -71,7 +71,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 1.0); REQUIRE(phase->endTime == 2.0); REQUIRE(phase->activeContacts.size() == 1); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); phase++; @@ -79,9 +79,9 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 2.0); REQUIRE(phase->endTime == 3.0); REQUIRE(phase->activeContacts.size() == 2); - ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + ok = phase->activeContacts.at("left") == expectedLeft; REQUIRE(ok); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); phase++; @@ -90,7 +90,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 3.0); REQUIRE(phase->endTime == 4.0); REQUIRE(phase->activeContacts.size() == 1); - ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + ok = phase->activeContacts.at("left") == expectedLeft; REQUIRE(ok); phase++; @@ -98,11 +98,11 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 4.0); REQUIRE(phase->endTime == 5.0); REQUIRE(phase->activeContacts.size() == 3); - ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + ok = phase->activeContacts.at("left") == expectedLeft; REQUIRE(ok); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); - ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + ok = phase->activeContacts.at("additional") == expectedAdditional; REQUIRE(ok); phase++; @@ -112,7 +112,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 5.0); REQUIRE(phase->endTime == 6.0); REQUIRE(phase->activeContacts.size() == 1); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); phase++; @@ -120,11 +120,11 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 6.0); REQUIRE(phase->endTime == 7.0); REQUIRE(phase->activeContacts.size() == 3); - ok = phase->getContactGivenList("left")->contact_it == expectedLeft; + ok = phase->activeContacts.at("left") == expectedLeft; REQUIRE(ok); - ok = phase->getContactGivenList("right")->contact_it == expectedRight; + ok = phase->activeContacts.at("right") == expectedRight; REQUIRE(ok); - ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + ok = phase->activeContacts.at("additional") == expectedAdditional; REQUIRE(ok); phase++; @@ -134,7 +134,7 @@ TEST_CASE("ContactPhaseList") REQUIRE(phase->beginTime == 7.0); REQUIRE(phase->endTime == 7.5); REQUIRE(phase->activeContacts.size() == 1); - ok = phase->getContactGivenList("additional")->contact_it == expectedAdditional; + ok = phase->activeContacts.at("additional") == expectedAdditional; REQUIRE(ok); phase++;