Skip to content

Commit

Permalink
Fix bug in calcJacobianTransformErrorDiff
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Feb 14, 2024
1 parent f78e2c9 commit 3110b4d
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 25 deletions.
11 changes: 4 additions & 7 deletions tesseract_common/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 jacobiancdifference
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);
Expand Down
37 changes: 19 additions & 18 deletions tesseract_common/test/tesseract_common_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2205,7 +2205,7 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
{ // 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 target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2217,7 +2217,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2229,7 +2229,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2241,20 +2241,21 @@ 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 target_tf_perturbed =
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf * Eigen::Translation3d(eps, -eps, eps);
;
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::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 target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2266,7 +2267,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2278,7 +2279,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2290,20 +2291,20 @@ 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 target_tf_perturbed =
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf * Eigen::Translation3d(-eps, eps, -eps);
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::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 target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2315,7 +2316,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -2327,7 +2328,7 @@ 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 target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
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::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
Expand All @@ -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 target_tf_perturbed =
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf * Eigen::Translation3d(-eps, -eps, eps);
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::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)));
}
}
Expand Down

0 comments on commit 3110b4d

Please sign in to comment.