New implementation for computeCartesianPath()

[rhaschke]

This is a port of moveit/moveit#3618

The Cartesian interpolator uses a JumpThreshold parameter to detect and reject a switch between IK solution branches leading to larger joint-space jumps. However, this approach is rather fragile: It is difficult to find a suitable threshold parameter and the approach requires a long-enough trajectory to work (at least 10 waypoints).
Particularly if the last requirement is not met, joint-space jumps can still occur unnoticed leading to a dangerous execution of an unchecked, large-volume trajectory.

This PR proposes a fundamentally different way to check for joint-space jumps, which works robustly and locally for any two adjacent waypoints: If the two waypoints are distant in joint-space, their half-way interpolation state will typically lead to a very strong deviation from the straight-line Cartesian path between them. This PR measures this deviation and rejects a trajectory if it exceeds a "precision" threshold.
Actually, the approach (recursively) introduces more waypoints in between to improve the precision of the linear Cartesian path. Thus, the method doesn't need to be provided with a good estimate of the MaxEEFStep parameter, it adjusts the step size dynamically to meet the given precision requirement. (This statement only holds if IK succeeds at all. In order to evaluate IK on a fine-grained grid and detect obstacles, one still needs the MaxEEFStep parameter.)

The following plots illustrate the new behavior. They all use the extremely coarse MaxEEFStep > 1m, resulting in a single initial step and then adjust the precision to introduce additional waypoints:

1 step, 0.3m precision	1 step, collision not detected!

1 + 1 steps, 0.1m precision	1 + 3 steps, 0.025m precision

I kept but deprecated the old JumpThreshold-based methods. I'm not sure how to best migrate the MoveGroup interface. To expose the new interface, we would need to introduce the CartesianPrecision argument as a message.
For now, I just dropped the JumpThreshold argument and resort to the default precision, which is 1mm. Given a reasonable MaxEEFStep, this should perfectly resolve the jump issues. I think, fiddling with the new parameter shouldn't be necessary in most cases.

[sea-bass]

Interesting! I like this promise of being able to run less IK solves with these (adjustable) checks. Only some minor comments.

[sjahr]

Cool, looks good thank you!

[DaniGarciaLopez]

Thanks for the contribution @rhaschke! Any plans to backport this to humble?

[rhaschke]

No. This PR involves a big API change.