Add ContactList and utilities for defining contact phases.

.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]

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. (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/

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)

src/Planners/CMakeLists.txt

@@ -2,10 +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.
 
-add_bipedal_locomotion_library(
-  NAME                  Contact
-  PUBLIC_HEADERS        include/BipedalLocomotion/Planners/Contact.h
-  PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core
-  IS_INTERFACE
-  INSTALLATION_FOLDER   Planners
-  )
+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()

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

src/Planners/include/BipedalLocomotion/Planners/ContactList.h

@@ -0,0 +1,217 @@
+/**
+ * @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
+
+// BipedalLocomotion
+#include <BipedalLocomotion/Planners/Contact.h>
+
+//iDynTree
+#include <iDynTree/Core/Transform.h>
+
+//std
+#include <set>
+#include <string>
+#include <functional>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+
+/**
+ * @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.
+ */
+class ContactList
+{
+    /**
+     * @brief Struct used for inserting new contacts in the set.
+     */
+    struct ContactCompare {
+        bool operator()(const Contact& lhs, const Contact& rhs) const;
+    };
+
+    std::set<Contact, ContactCompare> 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<Contact, ContactCompare>::const_iterator;
+    using const_reverse_iterator = std::set<Contact, ContactCompare>::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 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.
+     */
+    size_t size() 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);
+
+    /**
+     * @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();
+
+    /**
+     * @brief Remove only the last contact.
+     */
+    void removeLastContact();
+
+};
+
+} //namespace Planners
+} //namespace BipedalLocomotion
+
+
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONTACTLIST_H

src/Planners/include/BipedalLocomotion/Planners/ContactPhase.h

@@ -0,0 +1,53 @@
+/**
+ * @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 <BipedalLocomotion/Planners/Contact.h>
+#include <BipedalLocomotion/Planners/ContactList.h>
+#include <unordered_map>
+#include <string>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+
+/**
+ * @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::unordered_map<std::string, ContactList::const_iterator> 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;
+};
+
+}
+}
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_CONTACT_PHASE_H