contact variation should consider difference of rotation for 6D contacts

[pFernbach]

Hello, I had some issue where the method to get the contact variations between two states do not return the expected contact change because it does not consider the rotation between two contacts but only the translation.

I added this commit : pFernbach@1c01e81 which work fine for 6D contacts, but it should be disabled when considering 3D contacts.
However we don't have access to the contactType in rbprm-state, and I'm not sure about the best way to get access to this info.

One way should be to add a map <limbId,contactType> in rbprm-state, but we should correctly update it every-time we change a contact.

[stonneau]

ok. Just to be clear: in what context does this happen ? I mean the position remains the same but the orientation changes ?

[pFernbach]

It happen for example in the scenarios where the robot turn in place. Between two adjacent states the position of the contact can be really close (under the threshold in contactVariation) but the orientation change a lot.

[stonneau]

I have an issue here because in 3D if you replace the contact on the same location you will end up with the same issue, so this fix is not the good one.
Between the 1cm margin arbitrarily fixed here and the error in the projection there might be some margin, even though the projection error concerns all the constraints. In my memory experimenting did not lead to such great results. Have you tried yourself ?

I am afraid that in any case this requires some extended thinking.

[stonneau]

We need to think of our uses cases to make sure this is the case, but if we assume that there is a contact switch between two states, we could find which one has been replaced based on the minimal error. With a parameter option, we could return the most probable contact switch. This might be enough.

What do you think ?

[pFernbach]

Yes this solution seems the best. I will revert my commit.

I leave the issue open until it's implemented.