Finish Tool Path Constraint Implementation

[sea-bass]

In #377, there was some work done to create a nearest-neighbors data structure to allow a trajectory to effectively be "projected", or constrained, to a predefined path.

In favor of closing old PRs, I am creating this issue to continue that work, noting the following plan of action to get it done:

1. Rebase on main
2. Investigate whether instead of copy-pasting and slightly modifying the 3 OMPL headers for the nearest neighbor calculation, can we directly use those headers and/or make the necessary changes to directly OMPL to facilitate this.
3. A lot of the distance functions used in that PR are already available in Eigen, and since we're converting anyway we may as well just use Eigen instead of geometry_msgs.
4. The pose interpolation function does a lot of superfluous conversions from geometry_msgs to Eigen types, but combined with the above point, this could be easily addressed and made more efficient
5. Create a simple, but compelling demo that maybe sets e.g., a circular path and allows users to execute a trajectory within that constraint.

[mamoll]

Some context for this issue: the goal is to transform a tool path (workspace) trajectory, defined in terms of a polyline, into a continuous configuration space trajectory. This is different from repeatedly calling IK on a bunch of waypoints, since that doesn't ensure a continuous trajectory. We can treat a workspace path as a low-dimensional manifold embedded in config. space, where the start and goal are two points on that manifold. OMPL's constrained planning capabilities are a probabilistically complete solution to find a feasible (and even optimal) solution.

2. definitely check out OMPL's main branch. I think there have been some changes since the last OMPL release. We are about to cut a new OMPL release (1.6.0). A lot of tuning went into the nearest neighbor data structures, since they have a significant impact on planning time.
3. and 4. definitely avoid converting things back and forth.
4. use a simple example where a naive IK-based approach would likely fail would be best. Pick a 7-dof arm and add some simple obstacles. OMPL should exploit the kinematic redundancy to track the path, avoid collisions, and produce a continuous trajectory

[mamoll]

(Side-note: there is also some related copying from OMPL in moveit_kinematics/cached_ik_kinematics_plugin/*.)

[sea-bass]

I've started implementing this in #1946.

Got a working draft version of Boston's original PR with all the review comments -- specifically no copy-pasting of OMPL source and no unnecessary conversions between Eigen and ROS Pose messages... but I still need to come up with a demo and test more than just "the code built".

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.