Navigate to maximize observability of body key points during TUG

[pattacini]

Following up the analysis in #182, we need to design a good algorithm that will let R1 navigate along an ideal straight track located next to the TUG area so that we will maximize the observability of body keypoints.

[pattacini]

I would need to:

• Understand how to control navigation using:
  • odometry as the main sensor;
  • LIDAR only for obstacle detection.
• Estimate how far we can navigate in open-loop before being forced to realign the robot on the track.
• Conceive a simple formulation to maximize key points observability.
• Represent the track.
• Design the API.

[pattacini]

219 implements a simple navigation system to be considered only as a first step.

Obstacles avoidance is not accounted for yet, whereas as of now we only keep a given distance from a specified skeleton.