Skip to content

Commit 838a368

Browse files
Levi-Armstrongrjoomen
Levi-Armstrong
authored and
rjoomen
committed
Fix bug in calcJacobianTransformErrorDiff
1 parent a3244ec commit 838a368

File tree

2 files changed

+48
-50
lines changed

2 files changed

+48
-50
lines changed

‎tesseract_common/src/utils.cpp‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -211,19 +211,16 @@ Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
211211
throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
212212
#endif
213213

214-
Eigen::Isometry3d ttp = target.inverse() * target_perturbed;
215-
// The reason for premultiplying translation with ttp.rotation() is because the translation error needs to be in the
216-
// target frame coordinates Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian
217-
// difference
214+
// Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference
218215
Eigen::VectorXd perturbed_err;
219216
if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2)
220-
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
217+
perturbed_err = concat(perturbed_pose_err.translation(),
221218
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - 2 * M_PI));
222219
else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2)
223-
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
220+
perturbed_err = concat(perturbed_pose_err.translation(),
224221
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI));
225222
else
226-
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
223+
perturbed_err = concat(perturbed_pose_err.translation(),
227224
perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);
228225

229226
return (perturbed_err - err);

‎tesseract_common/test/tesseract_common_unit.cpp‎

Lines changed: 44 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -2198,16 +2198,16 @@ void runCalcJacobianTransformErrorDiffTest(double anlge)
21982198
}
21992199
}
22002200

2201-
void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
2201+
void runCalcJacobianTransformErrorDiffDynamicTargetTest(double angle)
22022202
{
22032203
Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
22042204
const double eps = 0.001;
22052205
{ // X-Axis positive
22062206
Eigen::Isometry3d target_tf{ identity };
22072207
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2208-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX());
2209-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2210-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX());
2208+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitX()) * target_tf;
2209+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2210+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX());
22112211
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22122212
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22132213
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2217,9 +2217,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22172217
{ // X-Axis negative
22182218
Eigen::Isometry3d target_tf{ identity };
22192219
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2220-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX());
2221-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2222-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2220+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
2221+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2222+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
22232223
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22242224
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22252225
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2229,9 +2229,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22292229
{ // X-Axis positive and negative
22302230
Eigen::Isometry3d target_tf{ identity };
22312231
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2232-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX());
2233-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitX());
2234-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2232+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf;
2233+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitX());
2234+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
22352235
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22362236
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22372237
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2241,22 +2241,23 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22412241
{ // X-Axis translation
22422242
Eigen::Isometry3d target_tf{ identity };
22432243
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2244-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX());
2245-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitX());
2246-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitX());
2247-
source_tf_perturbed.translation() += Eigen::Vector3d(eps, -eps, eps);
2244+
Eigen::Isometry3d target_tf_perturbed =
2245+
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitX()) * target_tf * Eigen::Translation3d(eps, -eps, eps);
2246+
;
2247+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
2248+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitX());
22482249
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22492250
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2250-
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps)));
2251+
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps)));
22512252
EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
22522253
}
22532254

22542255
{ // Y-Axis positive
22552256
Eigen::Isometry3d target_tf{ identity };
22562257
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2257-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY());
2258-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2259-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY());
2258+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitY()) * target_tf;
2259+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2260+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY());
22602261
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22612262
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22622263
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2266,9 +2267,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22662267
{ // Y-Axis negative
22672268
Eigen::Isometry3d target_tf{ identity };
22682269
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2269-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY());
2270-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2271-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2270+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
2271+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2272+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
22722273
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22732274
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22742275
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2278,9 +2279,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22782279
{ // Y-Axis positive and negative
22792280
Eigen::Isometry3d target_tf{ identity };
22802281
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2281-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY());
2282-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitY());
2283-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2282+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf;
2283+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitY());
2284+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
22842285
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22852286
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
22862287
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2290,22 +2291,22 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
22902291
{ // Y-Axis translation
22912292
Eigen::Isometry3d target_tf{ identity };
22922293
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2293-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY());
2294-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitY());
2295-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitY());
2296-
source_tf_perturbed.translation() += Eigen::Vector3d(-eps, eps, -eps);
2294+
Eigen::Isometry3d target_tf_perturbed =
2295+
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitY()) * target_tf * Eigen::Translation3d(-eps, eps, -eps);
2296+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
2297+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitY());
22972298
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
22982299
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2299-
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps)));
2300+
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps)));
23002301
EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
23012302
}
23022303

23032304
{ // Z-Axis positive
23042305
Eigen::Isometry3d target_tf{ identity };
23052306
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2306-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ());
2307-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2308-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ());
2307+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(-eps, Eigen::Vector3d::UnitZ()) * target_tf;
2308+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2309+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ());
23092310
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
23102311
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
23112312
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2315,9 +2316,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
23152316
{ // Z-Axis negative
23162317
Eigen::Isometry3d target_tf{ identity };
23172318
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2318-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ());
2319-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2320-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2319+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
2320+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2321+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
23212322
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
23222323
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
23232324
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2327,9 +2328,9 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
23272328
{ // Z-Axis positive and negative
23282329
Eigen::Isometry3d target_tf{ identity };
23292330
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2330-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ());
2331-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge + eps, Eigen::Vector3d::UnitZ());
2332-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2331+
Eigen::Isometry3d target_tf_perturbed = Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf;
2332+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle + eps, Eigen::Vector3d::UnitZ());
2333+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
23332334
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
23342335
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
23352336
EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(diff.head(3), Eigen::Vector3d::Zero()));
@@ -2339,13 +2340,13 @@ void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
23392340
{ // Z-Axis translation
23402341
Eigen::Isometry3d target_tf{ identity };
23412342
target_tf.translation() = Eigen::Vector3d(1, 2, 3);
2342-
Eigen::Isometry3d target_tf_perturbed = target_tf * Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ());
2343-
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(anlge, Eigen::Vector3d::UnitZ());
2344-
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(anlge - eps, Eigen::Vector3d::UnitZ());
2345-
source_tf_perturbed.translation() += Eigen::Vector3d(-eps, -eps, eps);
2343+
Eigen::Isometry3d target_tf_perturbed =
2344+
Eigen::AngleAxisd(eps, Eigen::Vector3d::UnitZ()) * target_tf * Eigen::Translation3d(-eps, -eps, eps);
2345+
Eigen::Isometry3d source_tf = identity * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
2346+
Eigen::Isometry3d source_tf_perturbed = identity * Eigen::AngleAxisd(angle - eps, Eigen::Vector3d::UnitZ());
23462347
Eigen::VectorXd diff = tesseract_common::calcJacobianTransformErrorDiff(
23472348
target_tf, target_tf_perturbed, source_tf, source_tf_perturbed);
2348-
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, -eps, eps)));
2349+
EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, eps, -eps)));
23492350
EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
23502351
}
23512352
}

0 commit comments

Comments
 (0)