Handle edge case in calcJacobianTransformErrorDiff

tesseract_common/src/utils.cpp

@@ -172,11 +172,12 @@ Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
   std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
       calcRotationalErrorDecomposed(perturbed_pose_err.rotation());
 
-  // They should always pointing in the same direction
-#ifndef NDEBUG
-  if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
-    throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
-#endif
+  // They should always pointing in the same direction, but when error is close to zero this can flip so we will correct
+  if (perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
+  {
+    perturbed_pose_rotation_err.first *= -1;
+    perturbed_pose_rotation_err.second *= -1;
+  }
 
   // Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference
   Eigen::VectorXd perturbed_err;