New path interpolation method to sample footsteps from a path of poses/waypoints

[SimoneMic]

PR for discussion of:

• Previous core methods/function have not been modified, all the new addiction is related to the navigation side and are not being used in the manual mode.
• Created a new module PosesPairInterpolator that interpolates a linear path from two poses, from a starting time instant for each time step (the concept is the same of a temporal integrator). This is similar to the getIntegratorSolution(t, unicycleState)
• This new module is used in a new method interpolateNewStepsFromPath in UnicyclePlanner.cpp that is analogue to computeNewSteps the logic structure is the same and keeps a lot of analogies.
• From the previous point, the Footstep evaluation and sampling is the same, but now a new function checks the constraints checkConstraints for simplicity.
• New struct for selecting the configuration mode of the planner NavigationSetup: the two main modes are Manual (default) and Navigation, a third and not used one is NotConfigured (if could be useful in the future) - in UnicycleGenerator.cpp
• Based on this config, in UnicycleGenerator.cpp each call to generate new footsteps now checks for the NavigationSetup AND the controller type. Calling interpolateNewStepsFromPath if in navigation mode and direct control, ComputeNewSteps otherwise.
• The input path is set externally via setInputPath (for interpolateNewStepsFromPath only) in UnicyclePlanner.cpp
• setLateralVelocityConservaiveFactor in UnicyclePlanner.cpp allows to set a different conservative factor for interpolateNewStepsFromPath only

[S-Dafarra]

Hi @SimoneMic. please let me know when the PR is ready to be reviewed. I quickly checked the code. Here some very preliminary comments:

• There is a typo: setLateralVelocityConservaiveFactor -> setLateralVelocityConservativeFactor
• Please avid to duplicate the logic of computeNewSteps. It is the most complex bit of the library, and having two copies of them is not desirable.

Maybe can you explain better what you are trying to achieve. Why do you need the additional methods?

[SimoneMic]

Hi @SimoneMic. please let me know when the PR is ready to be reviewed. I quickly checked the code. Here some very preliminary comments:

* There is a typo: `setLateralVelocityConservaiveFactor` -> `setLateralVelocityConservativeFactor`

* Please avid to duplicate the logic of `computeNewSteps`. It is the most complex bit of the library, and having two copies of them is not desirable.

Maybe can you explain better what you are trying to achieve. Why do you need the additional methods?

• for setLateralVelocityConservaiveFactor I haven't seen that it was already existing, the reason is caution: since the robot will be sidestepping autonomously I wanted it to be really slow, because it will be moving in its blind spot. But I can create a new method or use the actual corrective factor. For sure I will remove the duplicate.
• in computeNewSteps I have changed quite a bit of stuff, and to avoid breaking stuff I thought to make a separate method. Since it's a crucial part I avoided to touch that, also it will be easier to test in this way, being able to simply switch a function call.

For what regards the purpose, I need to actually follow a path of X, Y, and Theta poses, which is not supported by the actual method. Also, I need to be able to rotate the robot in place if I have a too big yaw difference between two poses, which is not possible to do with the actual method. Furthermore, I don't have any error due to a virtual controller, this means that the footsteps will be sample from a path which is exactly following the given waypoints. In this way, we don't have a unique very complex method, but two (with some redundancies) less complicated.

Under an architecture point of view, the evaluation of the path could be put in another function and shared. But it is more about how do you want to structure the whole module overall. Moreover we could imagine a unique computeNewSteps method divided into four sections: init, integration/interpolation, evaluation, footsteps finalization. But I think that this is more a topic for a separate discussion

[SimoneMic]

Closing in favor of #62