Fix segfault in totg

[Dale-Koenig]

Description

(At least partially) fixes #1805. Calls to acos can result in nan values if not careful due to numerical floating point errors. If there is a possibility of calling acos on values that are approximately -1, then it will result in nan, which leads to a segfault later in totg. Also removes an incorrect comment that implies a section would avoid this error, but it actually handles a different case.

[codecov-io]

Codecov Report

Merging #1861 into master will increase coverage by 1.67%.
The diff coverage is 100%.

@@            Coverage Diff             @@
##           master    #1861      +/-   ##
==========================================
+ Coverage   48.46%   50.14%   +1.67%     
==========================================
  Files         298      313      +15     
  Lines       23305    24639    +1334     
==========================================
+ Hits        11295    12355    +1060     
- Misses      12010    12284     +274

Impacted Files	Coverage Δ
...cessing/src/time_optimal_trajectory_generation.cpp	76.2% <100%> (ø)	⬆️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	64.68% <0%> (-1.87%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	55.38% <0%> (-1.26%)	⬇️
...ccupancy_map_monitor/src/occupancy_map_monitor.cpp	29.65% <0%> (-0.69%)	⬇️
...rimental/moveit_jog_arm/src/jog_interface_base.cpp	83.33% <0%> (-0.19%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	43.9% <0%> (-0.12%)	⬇️
...pabilities/get_planning_scene_service_capability.h	100% <0%> (ø)	⬆️
...it/planning_scene_monitor/planning_scene_monitor.h	82.6% <0%> (ø)	⬆️
...veit_jog_arm/include/moveit_jog_arm/jog_arm_data.h	100% <0%> (ø)	⬆️
...eit_experimental/moveit_jog_arm/src/jog_server.cpp	100% <0%> (ø)	⬆️
... and 25 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 546d6e0...b978138. Read the comment docs.

[henningkayser]

Thanks for fixing this! Shouldn't we clip the upper limit as well? Also, did you rule out that the dot product produces nan values?

[Dale-Koenig]

Thanks for fixing this! Shouldn't we clip the upper limit as well? Also, did you rule out that the dot product produces nan values?

The check if ((start_direction - end_direction).norm() < 0.000001) does special handling in the case where the dot product would be 1, so there should be no need to clip. As for the dot product I don't think it can produce nan values unless the inputs are already nan, since it is just multiplication and addition. For the errors I encountered the acos was at fault.

[rhaschke]

If these vectors are close to zero, normalization will result in NaNs or Infs. Not sure though, that this can happen.

[Dale-Koenig]

@rhaschke True, I suppose there is division in there. I believe the following loop, which is done before the constructor for Path, ensures that the vectors are large enough that we do not have to worry about failure when taking the dot product. On the other hand, I think once it gets into the heart of the algorithm there are various parts that are not robust to very small values, where for example a 180 degree turn, which this prevents from throwing an exception, might still result in very small time steps between switching points, causing failure during one of the integration methods.

for (size_t j = 0; j < num_joints; j++)
{
  new_point[j] = waypoint->getVariablePosition(idx[j]);
  if (p > 0 && std::abs(new_point[j] - points.back()[j]) > 0.001)
    diverse_point = true;
}

[mlautman]

@rhaschke and @Dale-Koenig Any reason to not merge this?