Fix segfault in totg (moveit#1861)

Ensure acos argument in valid range
v4hn · Mar 31, 2020 · ac84701 · ac84701
1 parent 6b80d7a
commit ac84701
Showing 1 changed file with 1 addition and 2 deletions.
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -110,7 +110,6 @@ class CircularPathSegment : public PathSegment
     const Eigen::VectorXd start_direction = (intersection - start).normalized();
     const Eigen::VectorXd end_direction = (end - intersection).normalized();
 
-    // check if directions are divergent
     if ((start_direction - end_direction).norm() < 0.000001)
     {
       length_ = 0.0;
@@ -122,7 +121,7 @@ class CircularPathSegment : public PathSegment
     }
 
     // directions must be different at this point so angle is always non-zero
-    const double angle = acos(start_direction.dot(end_direction));
+    const double angle = acos(std::max(-1.0, start_direction.dot(end_direction)));
     const double start_distance = (start - intersection).norm();
     const double end_distance = (end - intersection).norm();