Template OMPL Interface

[gstavrinos]

Minimal implementation of an OMPL optimizer. The optimizer will only return straight paths, solely for the purpose of testing integration.

[gstavrinos]

Note: Creating a complete planner should not be necessary with a complete quadcopter problem definition on #2 and #3. This is an interesting ompl demo for a car. This could also prove useful.

[gstavrinos]

Studying material:

• https://github.com/ompl/ompl/blob/main/demos/RigidBodyPlanningWithODESolverAndControls.cpp
• https://github.com/ompl/omplapp/blob/main/demos/SE3RigidBodyPlanning/SE3RigidBodyPlanningWithOptimization.cpp
• https://ompl.kavrakilab.org/classompl_1_1control_1_1ODEBasicSolver.html
• https://github.com/ompl/omplapp/blob/main/src/omplapp/apps/QuadrotorPlanning.h
• https://github.com/ompl/omplapp/blob/main/demos/SE3RigidBodyPlanning/QuadrotorPlanning.cpp
• https://github.com/ompl/ompl/blob/main/src/ompl/control/SimpleSetup.h
• https://ompl.kavrakilab.org/classompl_1_1app_1_1QuadrotorPlanning.html
• https://ompl.kavrakilab.org/classompl_1_1control_1_1SimpleSetup.html
• https://ompl.kavrakilab.org/optimalPlanning.html
• https://ompl.kavrakilab.org/spaces.html

[gstavrinos]

This issue should be now implemented based on the test commit bd7e915. It will be closed as soon as it is tested enough.