Add the possibility to control a subset of coordinates in IK::SE3Task

[GiulioRomualdi]

This PR adds the possibility to set a custom mask for the IK::SE3. Thanks to the mask a subset of coordinates will be controlled by the task. For instance, the following mask

auto mask = std::vector<bool>{true, false, true, false, false, false};

enable the control of the x and z coordinate only.

I also update the SE3Task test considering the new feature implemented.

cc @paolo-viceconte

[traversaro]

How is this working at the angular level? At the translation level it is clear to me, as thanks to the use of mixed velocity we have that each one of the translation line correspond to a derivative of a position quantity (this is not true for other velocity representations). Instead, at the angular level this is not true, so I wonder how deal with that.

[S-Dafarra]

From the code point of view, it looks good to me! I share the concerns of @traversaro. I think that for the angular part, it is important to define what the user would need. For example, the user may want to disable the orientation control along one axis, or control only along an axis. We could interpret those booleans as the activation (and deactivation) of the angular control along the 3 inertial axis. For example, [true, false, false] would mean that we control the rotation of the body only around the x axis (of the inertial frame). On the other hand, [true, true, false] would mean that we do not control any rotation around the z axis. I wonder if the current implementation would reflect this behavior.

[traversaro]

I am probably missing something, but I am not sure what " control the rotation of the body only around the x axis (of the inertial frame)" means in rotational context. From my point of view, without going too much in detail, the most conservative choise is for now to only allow that all the orientational axis are enabled, or all orientational axis are disabled. This should be well posed, and clear to explain to users.

[S-Dafarra]

I am probably missing something, but I am not sure what " control the rotation of the body only around the x axis (of the inertial frame)" means in rotational context.

Right, thinking about it, the "control around one axis" makes sense only if the desired and measured axes are aligned, otherwise the definition of the error is not clear.

From my point of view, without going too much in detail, the most conservative choise is for now to only allow that all the orientational axis are enabled, or all orientational axis are disabled. This should be well posed, and clear to explain to users.

I agree then. By the way, how to define a position-only task?

[traversaro]

I agree then. By the way, how to define a position-only task?

With all the orientational axis disabled, i.e. :

auto mask = std::vector<bool>{true, true, true, false, false, false};

[S-Dafarra]

I agree then. By the way, how to define a position-only task?

With all the orientational axis disabled, i.e. :

auto mask = std::vector<bool>{true, true, true, false, false, false};

Ok but that's not very clean. Why asking the user to set either zero or three booleans to false? Since there is a SO3 task, I was wondering if there were also a position task (even before this PR). Then, this masking mechanism could have applied only to the position part of the SE3 task

[GiulioRomualdi]

I agree with @traversaro, disabling only part of the rotational task may not be theoretically sound.
A more detailed explanation follows:
The SE3Task considers the spatial velocity written in MIXED representation. So the angular velocity is in the inertial frame.

The desired angular velocity is defined by

where X is the SO(3) object. Applying the mask to the angular velocities does not mean that we are considering only roll, pitch, or yaw. Indeed if the mask M is applied to the effect of it in the log operator is not trivial:

here is the rotation error and the log operator definition has been used.

To conclude, applying a mask to the angular velocity means set to zero the angular velocity along a specific axis but this may not guarantee the convergence of X to the desired value

[GiulioRomualdi]

If you agree I can add #356 (comment) in the documentation so the user knows what is happening

[traversaro]

If you agree I can add #356 (comment) in the documentation so the user knows what is happening

Unless we have a strict reason for allowing mixed rotationals masks, I would explicitly reject them in https://github.com/dic-iit/bipedal-locomotion-framework/pull/356/files#diff-0f6c9c6bb1c2932e10165029e222cc13ee96f1a90a63fbdc6e4eb1fdd74a6754R80 . Users never read the docs.

[GiulioRomualdi]

Ok so we can still have a 6d boolean vector but the last three elements must be equal right?

[traversaro]

Ok so we can still have a 6d boolean vector but the last three elements must be equal right?

Yes, that was my suggestion. @S-Dafarra did not however liked it in #356 (comment) .

[GiulioRomualdi]

Ok so we can still have a 6d boolean vector but the last three elements must be equal right?

Yes, that was my suggestion. @S-Dafarra did not however liked it in #356 (comment) .

Ok but that's not very clean. Why asking the user to set either zero or three booleans to false? Since there is a SO3 task, I was wondering if there were also a position task (even before this PR). Then, this masking mechanism could have applied only to the position part of the SE3 task

At the current stage, there is no R3 task. We can implement that and change the mask for the SE3Task for only the positional part

[S-Dafarra]

At the current stage, there is no R3 task. We can implement that and change the mask for the SE3Task for only the positional part

I don't see the purpose of having three booleans that have to be equal. AN alternative could be to define two different masks. One with three elements, and another one with only one maybe?

[GiulioRomualdi]

Or in the se3task we give the opportunity to apply the mask on the translation (i.e. the rotation is always enabled) and then we implement an R3 task that contains only the translation and there we add the mask so the user can do the following:

1. use so3task for rotation only purpose
2. use r3task for translation only (with mask)
3. use se3task for rotation + translation (with mask only for translation(

[S-Dafarra]

Or in the se3task we give the opportunity to apply the mask on the translation (i.e. the rotation is always enabled) and then we implement an R3 task that contains only the translation and there we add the mask so the user can do the following:

1. use so3task for rotation only purpose
2. use r3task for translation only (with mask)
3. use se3task for rotation + translation (with mask only for translation(

Looks good to me!

[GiulioRomualdi]

Thanks to 8583f7c the mask can be set only for the linear part of the SE3 object