From e4998e385cdfefe7b5134c4c11d64351eac09d86 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 13 Jul 2024 15:20:34 -0500 Subject: [PATCH] Handle edge case in calcJacobianTransformErrorDiff --- tesseract_common/src/utils.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp index b1cc3abd154..b68e832ebda 100644 --- a/tesseract_common/src/utils.cpp +++ b/tesseract_common/src/utils.cpp @@ -172,11 +172,12 @@ Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target, std::pair 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;