Automatically adapt closing fingers posture to the SQ

[pattacini]

As of now, the closing posture of the fingers is hard-coded, hence we rely on the compliance of the control that is intrinsically provided by Gazebo (torque control is used underneath).

Anyway, given the knowledge of the SQ, it'd be nice to find out the fingers configuration that best adapts with the SQ surface. We could achieve this using e.g. CGAL (already included in the docker image) or FCL:

• Compute meshes of the SQ for fast collision detection with the segments representing the fingers.
• For each finger, simulate:
  • Start closing the proximal joint until collisions are detected (cycle with steps of e.g. 1 deg).
  • Then, start closing the distal joints until collisions are detected (cycle with steps of e.g. 1 deg).
• Use a suitable closing posture of the fingers derived from that obtained above: e.g. with joints more rotated of a tiny amount wrt the outcome of the simulation.

🏆 As a bonus, we may want to display the skeleton of the final hand posture in VTK.

Resources

• 📖 https://arxiv.org/abs/1802.05176
• ⚙️ https://doc.cgal.org/latest/Surface_mesher/index.html
• ⚙️ https://doc.cgal.org/latest/AABB_tree/index.html