TOTP negative path velocity assertion

[dmcconachie-tri]

Occasionally when creating a TOTP object, an assertion triggers. An example input as logged by the following is included below.

  drake::log()->info("waypoints:");
  for (const auto& waypoint : raw_waypoints){
    drake::log()->info("{}", waypoint.transpose());
  }
  drake::log()->info("velocity_limits: {}", velocity_limits.transpose());
  drake::log()->info("acceleration_limits: {}", acceleration_limits.transpose());
  drake::log()->info("max_path_deviation: {}", max_path_deviation);
  drake::log()->info("timestep: {}", timestep);
  drake::log()->flush();

  return common_robotics_utilities::time_optimal_trajectory_parametrization
      ::Trajectory(raw_waypoints, velocity_limits, acceleration_limits,
                   max_path_deviation, timestep);

Example:

[2021-09-01 10:53:58.618] [console] [info] waypoints:
[2021-09-01 10:53:58.618] [console] [info]   -2.6094   1.44521   -1.7293  -1.17439   -1.9229 -0.321022  -2.01043     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info]  -2.01408   1.32902  -1.73112  -1.27455   -1.8832 -0.132581  -1.89372     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info] -0.851836 -0.126547   0.76479  0.737985  0.286924 -0.779685  -1.50867     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info]  0.637736   1.26644 -0.164602  0.516272   2.50057 0.0666952   1.13145     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 10:53:58.618] [console] [info] acceleration_limits: 0.85 0.85 0.87 1.13 1.22 1.57 1.57    0    0
[2021-09-01 10:53:58.618] [console] [info] max_path_deviation: 0.01
[2021-09-01 10:53:58.618] [console] [info] timestep: 0.001
terminate called after throwing an instance of 'std::runtime_error'
  what():  Error while integrating backward: Negative path velocity
Aborted (core dumped)

Possibly related to #19.