Undesirable behavior of ResamplePath and TOTP for zero-length paths

[calderpg]

ResamplePath will shorten path segments with zero distance, i.e. [[1, 2], [1, 2]] becomes [[1, 2]]. This is normally desirable, but TOTP requires paths with at least two waypoints, and will segfault when given a path with only one waypoint. Additionally, TOTP appears to produce nonsense trajectories for zero-length paths.

Action items:

• Decide if ResamplePath should ever resample a path to a single waypoint, and correct if not.
• Add safety check in TOTP parametrization for single-waypoint paths (and/or replace offending operator [] with .at() to get such bounds checks).
• Assess TOTP trajectories for zero-length paths and see if fixes are necessary.

[calderpg]

For the near term, TOTP has been modified to throw if paths with less than two waypoints are provided.