Add the possibility to set the dafault contact in ContactList class

[GiulioRomualdi]

This PR introduces the possibility to set the default contact in the ContactList class. Furthermore it introduces the TextLogging in ContactList

[S-Dafarra]

I did not understand what " default index" meant. It became clearer once I read the docs. Probably "default frame index" may be more evident.

Moreover, the definition of frame index is strictly bound to the use of iDynTree models. In other cases, it is possible that is not used an int to define a frame, but a struct for example. In addition, these contacts may express different points fixed to the same frame. I am not fully convinced by this addition to be honest. What is your intended use case?

Can you detail your use case?

[GiulioRomualdi]

I did not understand what " default index" meant. It became clearer once I read the docs. Probably "default frame index" may be more evident.

I agree

Moreover, the definition of frame index is strictly bound to the use of iDynTree models. In other cases, it is possible that is not used as an int to define a frame, but a struct for example. In addition, these contacts may express different points fixed to the same frame. I am not fully convinced by this addition to be honest. What is your intended use case?

Can you detail your use case?

The Contact struct has already the index attribute. This information is used in the ContactDetectors class to associate a contact to a frame of the robot. (probably @prashanthr05 can explain better this part)

https://github.com/dic-iit/bipedal-locomotion-framework/blob/005684637a8aa1f360c3aee8edab3b26edf7a654/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h#L53-L56

In my specific case, I use Contact::index in this code (related to this PR #284)

https://github.com/dic-iit/bipedal-locomotion-framework/blob/005684637a8aa1f360c3aee8edab3b26edf7a654/src/Contacts/src/FixedFootDetector.cpp#L199

The aim of the method was to simplify the possibility to add steps in a ContactList in this way:

    manif::SE3d rightTransform(Eigen::Vector3d::Zero(), manif::SO3d::Identity());
    contactListMap["right_foot"].setDefaultIndex(
        m_kinDynWithMeasured.kindyn->model().getFrameIndex("r_sole"));
    contactListMap["right_foot"].addContact(rightTransform, 0.0, 3.0);
    contactListMap["right_foot"].addContact(rightTransform,4.0, 5.0);

Otherwise this is required

    manif::SE3d rightTransform(Eigen::Vector3d::Zero(), manif::SO3d::Identity());
   
   PlannedContact contact1;
   contact1.activationTime = 0.0;
   contact1.deactivationTime = 3.0;
   contact1.pose = rightTransform; 
   contact1.index =  m_kinDynWithMeasured.kindyn->model().getFrameIndex("r_sole");
    contactListMap["right_foot"].setDefaultIndex(
        m_kinDynWithMeasured.kindyn->model().getFrameIndex("r_sole"));
    contactListMap["right_foot"].addContact(contact1);

Another possibility is to remove

https://github.com/dic-iit/bipedal-locomotion-framework/blob/005684637a8aa1f360c3aee8edab3b26edf7a654/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h#L53-L56

from contact and keep it only in the EstimatedContact class. However in this case we would find a way to reimplement
https://github.com/dic-iit/bipedal-locomotion-framework/blob/005684637a8aa1f360c3aee8edab3b26edf7a654/src/Contacts/src/FixedFootDetector.cpp#L164
to pass the index related to the contact.

Just to conclude I'm using the index to reset the legged odometry in this way.

m_floatingBaseEstimator.changeFixedFrame(m_fixedFootDetector.getFixedFoot().index,
    m_fixedFootDetector.getFixedFoot().pose.quat(),
    m_fixedFootDetector.getFixedFoot().pose.translation());

Notice that here m_fixedFootDetector.getFixedFoot() returns a
https://github.com/dic-iit/bipedal-locomotion-framework/blob/005684637a8aa1f360c3aee8edab3b26edf7a654/src/Contacts/include/BipedalLocomotion/Contacts/Contact.h#L91
so index is used to understand if the fixed foot is the left or the right.

Let me know if you have any idea

[S-Dafarra]

We (@GiulioRomualdi , @prashanthr05 and I) discussed F2F. Ideally, it would be better to avoid using indexes since:

• different models may use different ways to define a frame
• it may not be safe, especially if we add new links to the model
• there might be different models

In addition, if the contact is a point, we can define only its position, but then we cannot use that to define the frame pose.

Hence, we agreed on the following.

Instead of specifying the index, we may use a struct with the following fields:

• the name of the model associated to the contact
• the name of the frame to which the contact is rigidly attached
• the relative pose between this frame and the contact.

In any case, this is something that can be addressed in future PRs.