QP-IK [Distance and Gravity tasks] fails for some seed

[traversaro]

This problem appeared several times in the past in different forms (#714 (comment), conda-forge/bipedal-locomotion-framework-feedstock#29) but with #984 we can reproduce it in a reliable way.

I suspect the root cause is the heuristic in https://github.com/ami-iit/bipedal-locomotion-framework/blob/36fc886e6d8ec1b31bd5678f22d57cb6caf87471/src/IK/tests/QPInverseKinematicsTest.cpp#L246-L256 :

    // Find a frame sufficiently far away from those used for the distance and SE3 task
    do
    {
        out.targetFrameGravity = iDynTree::getRandomLinkOfModel(kinDyn->model());
        out.targetFrameDistance = iDynTree::getRandomLinkOfModel(kinDyn->model());
        indexGravity = kinDyn->model().getFrameIndex(out.targetFrameGravity);
        indexDistance = kinDyn->model().getFrameIndex(out.targetFrameDistance);
    } while (std::abs(indexGravity - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5
             || std::abs(indexDistance - kinDyn->model().getFrameIndex(out.endEffectorFrame)) < 5
             || std::abs(indexGravity - indexDistance) < 5 || indexGravity < 3
             || indexDistance < 3);

as the model is generated as in https://github.com/ami-iit/bipedal-locomotion-framework/blob/36fc886e6d8ec1b31bd5678f22d57cb6caf87471/src/IK/tests/QPInverseKinematicsTest.cpp#L325-L348 the fact that two links have an index with a difference higher then 5 does not say anything on the degrees of freedom of the joints between the two links.

[S-Dafarra]

The point is that, independently from the frame, we get the target for the task using random joint values
https://github.com/ami-iit/bipedal-locomotion-framework/blob/36fc886e6d8ec1b31bd5678f22d57cb6caf87471/src/IK/tests/QPInverseKinematicsTest.cpp#L221-L224

Hence, a solution does exist. The problem is that the IK does not converge to it when initialized from a set of joints that is perturbed from those used to compute the targets:
https://github.com/ami-iit/bipedal-locomotion-framework/blob/36fc886e6d8ec1b31bd5678f22d57cb6caf87471/src/IK/tests/QPInverseKinematicsTest.cpp#L288-L292

There are many random things here. In my opinion, here we are affected by the IK nonlinearities and the corresponding local minima