Draft: OMPL Planner Refactor

[marip8]

This PR introduces new interfaces for waypoint, composite, and planner profiles to support a refactor of the motion planners. The OMPL motion planner was also updated to utilize the new profile definitions.

This PR implements a refactor of the OMPL motion planner based on changes from #156 and #157

To-do

• Understand why JointStartCartesianGoal unit test fails (currently produces only 1 valid OMPL goal state)
  • The IK solution for the goal state seemed to be too close to the collision wall separating the start/end states such that the solution itself was not in collision but it prevented valid samples from being generated. The IK solution was not equivalent to the joint state used to create the Cartesian pose because the KDL IK solver resolved the 7th joint to a different value. Spacing the nominal start/goal poses further away from the collision wall solved the issue
• Cast data object to ompl::base::SimpleSetup if it is not a nullptr
• Cast data object to ompl::base::PlannerData if it is not nullptr; save response.data as ompl::base::PlannerData for use with future planners or external graph search algorithms
• Decide which instructions to extract planner and composite profiles from
• Consolidate return instruction to be a single composite where the start move is in the start instruction of the composite
• Accept input composite instructions with more than one waypoint and plan trajectories in series
  • Need to decide how parameters like maximum planning time apply for this strategy
• Implement waypoint profiles for generating the start and goal states
• Test with demos

To-do in future PRs

• Rename from OMPL planner "configurator" to "factory" or "allocator"
• Add an override method for construction OMPL planners with ompl::base::PlannerData
  • This is only valid for multi-query planners like the PRM variants
  • If a planner doesn't support construction from ompl::base::PlannerData itself, it also has a pointer to the SpaceInformation which could be used to construct the planner
• Add a pointer to an ompl::base::PlannerData (or a file from which to load one using ompl::base::PlannerDataStorage) to the multi-query planner factories
• Consider creating an OMPL WaypointProfile that leverages OMPL goal sampling

[marip8]

@Levi-Armstrong I think this is ready for review. The unit tests pass (locally), and I'm working on visualizing the freespace example to confirm proper behavior

[Levi-Armstrong]

It looks like several of the process planner units are now failing.

[marip8]

I think the process planner unit tests are failing because no default profiles were added to the new profile dictionary maps for the OMPL planner. I'll look into it. Strange that the Windows build succeeds. Does it not run unit tests?

[Levi-Armstrong]

Yea it does not run the unit test on the windows build. It just makes sure everything build. A few were segfaulting which I believe it's due to trying to write to temp directory.

[codecov]

Codecov Report

Merging #138 (e7900e7) into master (31afa29) will decrease coverage by 4.97%.
The diff coverage is 83.53%.

@@            Coverage Diff             @@
##           master     #138      +/-   ##
==========================================
- Coverage   66.30%   61.32%   -4.98%     
==========================================
  Files         199      200       +1     
  Lines        9407     9102     -305     
==========================================
- Hits         6237     5582     -655     
- Misses       3170     3520     +350     

Impacted Files	Coverage Δ
...tesseract_command_language/composite_instruction.h	100.00% <ø> (ø)
...act_command_language/utils/get_instruction_utils.h	51.28% <ø> (ø)
...act_command_language/src/composite_instruction.cpp	52.12% <0.00%> (+5.38%)	⬆️
...planners/ompl/weighted_real_vector_state_sampler.h	100.00% <ø> (ø)
tesseract_motion_planners/ompl/src/utils.cpp	58.62% <ø> (-3.88%)	⬇️
...ct_process_managers/core/process_planning_server.h	100.00% <ø> (ø)
...de/tesseract_command_language/profile_dictionary.h	85.89% <58.33%> (-12.26%)	⬇️
...mmand_language/src/utils/get_instruction_utils.cpp	42.20% <66.66%> (+42.20%)	⬆️
...s/ompl/src/profile/ompl_composite_profile_rvss.cpp	72.22% <72.22%> (ø)
...eract_command_language/src/utils/flatten_utils.cpp	55.71% <75.00%> (ø)
... and 27 more

[marip8]

I resolved the issue with the unit tests, and I was able to successfully run the examples in this repository (and tesseract_ros_examples with a few minor changes). It looks like code coverage fails, presumably because of the few functions that I added to the profile dictionary to support this. I could add a unit test for this capability in this PR, but I think the idea is that we would eventually replace the existing profile dictionary with the new implementation once all the planners are updated. I would propose holding off on making those unit tests until we are ready to finish the profile dictionary