Add ContactList and utilities for defining contact phases. #45

S-Dafarra · 2020-05-19T09:08:29Z

No description provided.

Trying to fix WIndows failure.

GiulioRomualdi · 2020-05-19T14:50:29Z

On macOS is failing. Do you have any hints? I think it is not related to this PR.

S-Dafarra · 2020-05-19T15:51:12Z

On macOS is failing. Do you have any hints? I think it is not related to this PR.

To be honest I don't know. It seems the same cache problem we had back in time after renaming the project. The weird thing is that it was not failing on my fork https://github.com/S-Dafarra/bipedal-locomotion-framework/runs/688385965

Rather than trying to run again the jobs, I added a method on the ContactList and pushed a new commit.

traversaro · 2020-05-19T15:55:30Z

On macOS is failing. Do you have any hints? I think it is not related to this PR.

To be honest I don't know. It seems the same cache problem we had back in time after renaming the project. The weird thing is that it was not failing on my fork https://github.com/S-Dafarra/bipedal-locomotion-framework/runs/688385965

Rather than trying to run again the jobs, I added a method on the ContactList and pushed a new commit.

It is actualy iDynTree's fault, that has bugged CMake config ^^'' . See robotology/whole-body-estimators#60 for the issue and a workaround.

Using a fancy way to perform the ceiling of a division of integers. Adding the links to the PRs in the changelog.

GiulioRomualdi

Really nice machinery! I add some minor comments. Fell free to merge it

GiulioRomualdi · 2020-05-21T07:27:44Z

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

+        bool operator()(const Contact& lhs, const Contact& rhs) const;
+    };
+
+    std::set<Contact, ContactCompare> m_contacts; /** Data structure for inserting and ordering contacts. **/


Correct me if I'm mistaken. You are using an std::set because you are sure that the Contact key is unique.

Yes, indeed I make sure of that with ContactCompare. I basically needed a container to store all the contacts in a custom order. In addition, the insertion of a new contact fails if the new one is "intersecting" some contact already added. For example, see here for an attempt of invalid insertion. p3 is "intersecting" p2.

GiulioRomualdi · 2020-05-21T07:34:59Z

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

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


Why do you need this?
In the end, the Contact class contains

/** * Type of contact. */ ContactType type {ContactType::FULL};

Is it only an utility required to avoid passing the ContactType for each new contact that you define?

Yes, it is used here: https://github.com/S-Dafarra/bipedal-locomotion-framework/blob/ab4e9d7a5c4a101ca8630b96af1052bf8a3c95e5/src/Planners/include/BipedalLocomotion/Planners/ContactList.h#L92

In addition, the defaultContactName is used as a key in the ContactListMap when using this method: https://github.com/S-Dafarra/bipedal-locomotion-framework/blob/ab4e9d7a5c4a101ca8630b96af1052bf8a3c95e5/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h#L58

GiulioRomualdi · 2020-05-21T07:36:58Z

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

+     * @brief Set the default name.
+     * @param defaultName the default name.
+     */
+    void setDefaultName(const std::string& defaultName);


Only to have symmetry with the const std::string& defaultName() const you may change the signature of the function in

std::string& defaultName();

Actually, I would avoid this to prevent stupid mistakes like

if (list.defaultName() = "a")

In addition, even if it is not the case now, in the future I may need to do some changes when this name or the contact type change.

GiulioRomualdi · 2020-05-21T07:38:00Z

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

+     * @brief Set the default contact type.
+     * @param The default contact type.
+     */
+    void setDefaultContactType(const ContactType& type);


The same of const std::string& defaultName() const

As above, I would avoid this.

GiulioRomualdi · 2020-05-21T07:42:47Z

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

+struct ContactReference
+{
+    std::string listLabel; /** Label to indicate the corresponding list. **/
+    ContactList::const_iterator contact_it; /** Const iterator to a contact. **/
+};


Is there a reason why you prefer using a struct rather than an unordered_map? i
It may also simplify the implementation of

std::vector<ContactReference>::const_iterator getContactGivenList(const std::string& key) const;

Indeed, it was easier as you suggested. I modified the container in the ContactPhase to use an unordered_map in 546052a, thanks!

GiulioRomualdi · 2020-05-21T07:45:05Z

src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h

+/**
+ * @brief Utility alias to a map of ContacLists.
+ */
+using ContactListMap = std::unordered_map<std::string, ContactList>;


Could you please give me an example of key? Thanks 😄

It is an identifier of the ContactList, like "left_foot", "right hand"..

GiulioRomualdi · 2020-05-21T07:46:22Z

src/Planners/src/ContactList.cpp

+{
+    assert(index < size());
+
+    if (index > (size() / 2 + (size() % 2 != 0))) //fancy way for doing std::ceil(size() / 2.0)


Premature optimization 😁 I needed the ceil, but the division between integers gives the floor so I add one in case the remainder is not zero. The reason is that I wanted to avoid casting this division with a double.
This if was meant to make this call slightly faster when used in a for loop. In fact, an std::set does not provide a random access operator. Hence, I used std::advance, which basically increase by one the iterator for index times. I thought that if index is greater than half of the size, it is faster to reach the desired position by starting from the end. But this advantage would have been eaten up by the division itself.

GiulioRomualdi · 2020-05-21T07:55:18Z

src/Planners/src/ContactList.cpp

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


This comment is just for me to recap. Before editing the contact you have to check:

the previous contact has an activation time smaller than the new one;

the next contact has an activation time greater than the new one.

Almost

the previous contact has a deactivation time smaller than the new activation time;

the next contact has an activation time greater than the new deactivation time.

It is done to make sure that the new step remains in the same position.

GiulioRomualdi · 2020-05-21T08:01:36Z

src/Planners/src/ContactPhaseList.cpp

+
+using namespace BipedalLocomotion::Planners;
+
+void BipedalLocomotion::Planners::ContactPhaseList::createPhases()


It reminds me the unicycle-planner 👍

This method is a bit obscure (I think I have to go deeper into the Planners library before understanding the details).

Could you give me a short recap of what the function is aim to do? Thanks

Actually, after your comment on using the unordered_map instead of the vector it got simplified. It works as follows. It is used to determine the phases given the contact lists. A phase is characterized by a set of contacts which remains constant (the set) for the entirety of its duration.

To compute them, I use two auxiliary maps. The items are ordered according to the time (hence the key is the time). I have one for the activations and one for the deactivations. Basically, an element contains all the contacts that got activated or deactivated for a given instant (the key). Then I start iterating.

The first phase will have the activation time equal to the first key in the activations map. The active contacts correspond to the first element. Then I remove the first element in the activations map. Subsequently, I check if the new first key on the activations map is smaller than the first key in the deactivations map. If this is the case, I have found the end of the previous phase and the begin of the new phase. The new active contacts are the previous ones plus those in the new first element in the activations map. Otherwise, if the first element of the deactivations map was occurring before, I have found again the phase end time and begin time of the new phase. The active contacts instead are the previous ones, minus those contained in the first deactivation element. After this passage, I remove the first deactivation element and I iterate until the two maps are empty.

GiulioRomualdi · 2020-05-21T08:08:40Z

src/Planners/tests/ContactPhaseListTest.cpp

+    SECTION("Set from map")
+    {
+        ContactListMap contactListMap;
+        REQUIRE(contactListMap["left"].addContact(iDynTree::Transform::Identity(), 0.0, 1.0));


Ok, I think I found an example of key

S-Dafarra

Answers to @GiulioRomualdi questions

S-Dafarra · 2020-05-21T08:20:12Z

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

+        bool operator()(const Contact& lhs, const Contact& rhs) const;
+    };
+
+    std::set<Contact, ContactCompare> m_contacts; /** Data structure for inserting and ordering contacts. **/


Yes, indeed I make sure of that with ContactCompare. I basically needed a container to store all the contacts in a custom order. In addition, the insertion of a new contact fails if the new one is "intersecting" some contact already added. For example, see here for an attempt of invalid insertion. p3 is "intersecting" p2.

S-Dafarra · 2020-05-21T08:23:09Z

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

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


Yes, it is used here: https://github.com/S-Dafarra/bipedal-locomotion-framework/blob/ab4e9d7a5c4a101ca8630b96af1052bf8a3c95e5/src/Planners/include/BipedalLocomotion/Planners/ContactList.h#L92

In addition, the defaultContactName is used as a key in the ContactListMap when using this method: https://github.com/S-Dafarra/bipedal-locomotion-framework/blob/ab4e9d7a5c4a101ca8630b96af1052bf8a3c95e5/src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h#L58

S-Dafarra · 2020-05-21T09:08:26Z

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

+     * @brief Set the default name.
+     * @param defaultName the default name.
+     */
+    void setDefaultName(const std::string& defaultName);


Actually, I would avoid this to prevent stupid mistakes like

if (list.defaultName() = "a")

In addition, even if it is not the case now, in the future I may need to do some changes when this name or the contact type change.

S-Dafarra · 2020-05-21T09:09:15Z

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

+     * @brief Set the default contact type.
+     * @param The default contact type.
+     */
+    void setDefaultContactType(const ContactType& type);


As above, I would avoid this.

S-Dafarra · 2020-05-21T09:10:07Z

src/Planners/include/BipedalLocomotion/Planners/ContactPhaseList.h

+/**
+ * @brief Utility alias to a map of ContacLists.
+ */
+using ContactListMap = std::unordered_map<std::string, ContactList>;


It is an identifier of the ContactList, like "left_foot", "right hand"..

S-Dafarra · 2020-05-21T09:15:51Z

src/Planners/src/ContactList.cpp

+{
+    assert(index < size());
+
+    if (index > (size() / 2 + (size() % 2 != 0))) //fancy way for doing std::ceil(size() / 2.0)


Premature optimization 😁 I needed the ceil, but the division between integers gives the floor so I add one in case the remainder is not zero. The reason is that I wanted to avoid casting this division with a double.
This if was meant to make this call slightly faster when used in a for loop. In fact, an std::set does not provide a random access operator. Hence, I used std::advance, which basically increase by one the iterator for index times. I thought that if index is greater than half of the size, it is faster to reach the desired position by starting from the end. But this advantage would have been eaten up by the division itself.

S-Dafarra · 2020-05-21T09:18:39Z

src/Planners/src/ContactList.cpp

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


Almost

the previous contact has a deactivation time smaller than the new activation time;

the next contact has an activation time greater than the new deactivation time.

It is done to make sure that the new step remains in the same position.

S-Dafarra · 2020-05-21T09:30:54Z

src/Planners/src/ContactPhaseList.cpp

+
+using namespace BipedalLocomotion::Planners;
+
+void BipedalLocomotion::Planners::ContactPhaseList::createPhases()


Actually, after your comment on using the unordered_map instead of the vector it got simplified. It works as follows. It is used to determine the phases given the contact lists. A phase is characterized by a set of contacts which remains constant (the set) for the entirety of its duration.

To compute them, I use two auxiliary maps. The items are ordered according to the time (hence the key is the time). I have one for the activations and one for the deactivations. Basically, an element contains all the contacts that got activated or deactivated for a given instant (the key). Then I start iterating.

The first phase will have the activation time equal to the first key in the activations map. The active contacts correspond to the first element. Then I remove the first element in the activations map. Subsequently, I check if the new first key on the activations map is smaller than the first key in the deactivations map. If this is the case, I have found the end of the previous phase and the begin of the new phase. The new active contacts are the previous ones plus those in the new first element in the activations map. Otherwise, if the first element of the deactivations map was occurring before, I have found again the phase end time and begin time of the new phase. The active contacts instead are the previous ones, minus those contained in the first deactivation element. After this passage, I remove the first deactivation element and I iterate until the two maps are empty.

S-Dafarra · 2020-05-21T09:45:21Z

@GiulioRomualdi let me know if I answered your questions and I can merge 😁

GiulioRomualdi · 2020-05-22T00:18:03Z

@S-Dafarra thanks for the explanation. You can merge it

S-Dafarra added 7 commits May 17, 2020 16:13

Added ContactList.

79ac4fc

Added ContactPhase.h

ac15e3e

Trying to fix WIndows failure.

Added definition of phases and a way to compute them.

65508eb

Fixed while loop condition when computing the phases.

d7f7d9c

Added documentaiton for ContactList and renamed some methods.

302ab34

Added documentation of ContactPhaseList.

af8d3ef

Updated Changelog and add possibility not to compile Planners libraries.

b909bff

S-Dafarra changed the title ~~Add ContactList and C~~ Add ContactList and utilities for defining contact phases. May 19, 2020

S-Dafarra requested a review from GiulioRomualdi May 19, 2020 09:09

S-Dafarra self-assigned this May 19, 2020

S-Dafarra added this to the Iteration 43 - End of Research Increment milestone May 19, 2020

Added direct accessor to contacts the list.

8cf6b97

Attempt workaround on ci.

ab4e9d7

Using a fancy way to perform the ceiling of a division of integers. Adding the links to the PRs in the changelog.

GiulioRomualdi reviewed May 21, 2020

View reviewed changes

Using an unordered_map instead of vector in the ContactPhase.

546052a

S-Dafarra commented May 21, 2020

View reviewed changes

S-Dafarra merged commit a89f6b4 into ami-iit:master May 22, 2020

prashanthr05 mentioned this pull request Nov 12, 2020

Make compilation of Contact library independent from Planners library #141

Merged


		using namespace BipedalLocomotion::Planners;

		void BipedalLocomotion::Planners::ContactPhaseList::createPhases()

Add ContactList and utilities for defining contact phases. #45

Add ContactList and utilities for defining contact phases. #45

Conversation

S-Dafarra commented May 19, 2020

GiulioRomualdi commented May 19, 2020

S-Dafarra commented May 19, 2020

traversaro commented May 19, 2020

GiulioRomualdi left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra commented May 21, 2020

GiulioRomualdi commented May 22, 2020