From 316f010224a1d08880c20707c6d2084bb0958b0c Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 13 Feb 2024 22:19:24 -0600 Subject: [PATCH] Fix bug in calcJacobianTransformErrorDiff --- tesseract_common/src/utils.cpp | 11 +-- .../test/tesseract_common_unit.cpp | 87 ++++++++++--------- 2 files changed, 48 insertions(+), 50 deletions(-) diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp index 1f0f89d7399..dd6293461e8 100644 --- a/tesseract_common/src/utils.cpp +++ b/tesseract_common/src/utils.cpp @@ -211,19 +211,16 @@ Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target, throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!"); #endif - Eigen::Isometry3d ttp = target.inverse() * target_perturbed; - // The reason for premultiplying translation with ttp.rotation() is because the translation error needs to be in the - // target frame coordinates Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian - // difference + // Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference Eigen::VectorXd perturbed_err; if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2) - perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(), + perturbed_err = concat(perturbed_pose_err.translation(), perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - 2 * M_PI)); else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2) - perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(), + perturbed_err = concat(perturbed_pose_err.translation(), perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI)); else - perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(), + perturbed_err = concat(perturbed_pose_err.translation(), perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second); return (perturbed_err - err); diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index cbf7d507b36..c867c29cf1d 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -2198,16 +2198,16 @@ void runCalcJacobianTransformErrorDiffTest(double anlge) } } -void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) +void runCalcJacobianTransformErrorDiffDynamicTargetTest(double angle) { Eigen::Isometry3d identity = Eigen::Isometry3d::Identity(); const double eps = 0.001; { // X-Axis positive Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2217,9 +2217,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // X-Axis negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2229,9 +2229,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // X-Axis positive and negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2241,22 +2241,23 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // X-Axis translation Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX()); - source_tf_perturbed.translation() += Eigen::Vector3d(eps, -eps, eps); + Eigen::Isometry3d target_tf_perturbed = + Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf * Eigen::Translation3d(eps, -eps, eps); + ; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); - EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps))); + EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps))); EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0))); } { // Y-Axis positive Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2266,9 +2267,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Y-Axis negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2278,9 +2279,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Y-Axis positive and negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2290,22 +2291,22 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Y-Axis translation Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY()); - source_tf_perturbed.translation() += Eigen::Vector3d(-eps, eps, -eps); + Eigen::Isometry3d target_tf_perturbed = + Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf * Eigen::Translation3d(-eps, eps, -eps); + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); - EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps))); + EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps))); EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0))); } { // Z-Axis positive Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2315,9 +2316,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Z-Axis negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2327,9 +2328,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Z-Axis positive and negative Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf; + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero())); @@ -2339,13 +2340,13 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge) { // Z-Axis translation Eigen::Isometry3d target_tf{ identity }; target_tf.translation() = Eigen::Vector3d(1, 2, 3); - Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ()); - Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ()); - source_tf_perturbed.translation() += Eigen::Vector3d(-eps, -eps, eps); + Eigen::Isometry3d target_tf_perturbed = + Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf * Eigen::Translation3d(-eps, -eps, eps); + Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ()); Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff( target_tf, target_tf_perturbed, source_tf, source_tf_perturbed); - EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, -eps, eps))); + EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, eps, -eps))); EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps))); } }