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 @rjoomen
Levi-Armstrong authored and rjoomen committed Feb 17, 2024
1 parent a3244ec commit 838a368
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 50 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 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);
Expand Down
87 changes: 44 additions & 43 deletions tesseract_common/test/tesseract_common_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
Expand All @@ -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()));
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 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)));
}
}
Expand Down

0 comments on commit 838a368

Please sign in to comment.