TOTP hangs during construction

[dmcconachie-tri]

Occasionally when creating a TOTP object, the constructor appears to hang during a call to IntegrateForward. Eventually (4+ minutes after the constructor is called), the terminal reports Killed.

Some example inputs as logged by the following are 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 1

[2021-09-01 10:57:41.664] [console] [info] waypoints:
[2021-09-01 10:57:41.665] [console] [info]   1.79874   1.09534  -1.28107  0.459357 0.0742109 0.0291835  -2.40435     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info]   1.20785  0.929724 -0.728769 0.0518051    2.4434 -0.450822  0.565328     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info]  0.637736   1.26644 -0.164602  0.516272   2.50057 0.0666952   1.13145     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 10:57:41.665] [console] [info] acceleration_limits: 0.85 0.85 0.87 1.13 1.22 1.57 1.57    0    0
[2021-09-01 10:57:41.665] [console] [info] max_path_deviation: 0.01
[2021-09-01 10:57:41.665] [console] [info] timestep: 0.001
Killed

Example 2

[2021-09-01 14:14:28.574] [console] [info] waypoints:
[2021-09-01 14:14:28.574] [console] [info]  1.47535  1.07883 -1.86478  1.52119 0.200813 -0.64916  2.45783    -0.01     0.01
[2021-09-01 14:14:28.574] [console] [info]  -0.587795   0.306845   -1.71638 -0.0474613  -0.706651   0.187446    1.47559      -0.01       0.01
[2021-09-01 14:14:28.574] [console] [info]  -1.35365 -0.364784  -2.79961 -0.424282  -2.60627  0.413107   1.82557     -0.01      0.01
[2021-09-01 14:14:28.574] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 14:14:28.574] [console] [info] acceleration_limits: 2.125 2.125 2.175 2.825  3.05 3.925 3.925     0     0
[2021-09-01 14:14:28.574] [console] [info] max_path_deviation: 0.01
[2021-09-01 14:14:28.574] [console] [info] timestep: 0.001
Killed

Example 3

[2021-09-01 14:51:23.278] [console] [info] waypoints:
[2021-09-01 14:51:23.278] [console] [info]  -2.13759  0.987328  -1.98412 0.0896687  0.337348   1.42335   -1.3307     -0.01      0.01
[2021-09-01 14:51:23.278] [console] [info]  0.793133 0.0747637  0.413363 0.0644863   1.52315  0.290163  -1.17068     -0.01      0.01
[2021-09-01 14:51:23.278] [console] [info]  -0.169478   0.744316    1.16333  0.0502231    2.49467 -0.0181687    0.42362      -0.01       0.01
[2021-09-01 14:51:23.278] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 14:51:23.278] [console] [info] acceleration_limits: 2.125 2.125 2.175 2.825  3.05 3.925 3.925     0     0
[2021-09-01 14:51:23.278] [console] [info] max_path_deviation: 0.01
[2021-09-01 14:51:23.278] [console] [info] timestep: 0.001
Killed