Add jacobian transform error calculation function (#971)

tesseract-robotics · Dec 11, 2023 · ff72c05 · ff72c05
1 parent 985e4c0
commit ff72c05
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 1 deletion.
diff --git a/tesseract_common/include/tesseract_common/utils.h b/tesseract_common/include/tesseract_common/utils.h
@@ -138,12 +138,22 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R)
 
 /**
  * @brief Calculate error between two transforms expressed in t1 coordinate system
+ * @warning This should not be used to calculate numerical jacobian
  * @param t1 Target Transform
  * @param t2 Current Transform
- * @return error [Position, Rotational(Angle Axis)]
+ * @return error [Position, calcRotationalError(Angle Axis)]
  */
 Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);
 
+/**
+ * @brief Calculate error between two transforms expressed in t1 coordinate system
+ * @warning This should only be used to calculate numerical jacobian
+ * @param t1 Target Transform
+ * @param t2 Current Transform
+ * @return error [Position, calcRotationalError2(Angle Axis)]
+ */
+Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);
+
 /**
  * @brief This computes a random color RGBA [0, 1] with alpha set to 1
  * @return A random RGBA color

diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp
@@ -177,6 +177,12 @@ Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Iso
   return concat(pose_err.translation(), calcRotationalError(pose_err.rotation()));
 }
 
+Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2)
+{
+  Eigen::Isometry3d pose_err = t1.inverse() * t2;
+  return concat(pose_err.translation(), calcRotationalError2(pose_err.rotation()));
+}
+
 Eigen::Vector4d computeRandomColor()
 {
   Eigen::Vector4d c;

diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp
@@ -2144,6 +2144,40 @@ TEST(TesseractCommonUnit, calcTransformError)  // NOLINT
   }
 }
 
+/** @brief Tests calcTransformErrorJac */
+TEST(TesseractCommonUnit, calcTransformErrorJac)  // NOLINT
+{
+  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
+
+  {  // X-Axis
+    Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX());
+    Eigen::VectorXd err = tesseract_common::calcTransformErrorJac(identity, pi_rot);
+    EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(M_PI_2, 0, 0)));
+  }
+
+  {  // Y-Axis
+    Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY());
+    Eigen::VectorXd err = tesseract_common::calcTransformErrorJac(identity, pi_rot);
+    EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, M_PI_2, 0)));
+  }
+
+  {  // Z-Axis
+    Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
+    Eigen::VectorXd err = tesseract_common::calcTransformErrorJac(identity, pi_rot);
+    EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, 0, M_PI_2)));
+  }
+
+  {  // Translation
+    Eigen::Isometry3d pi_rot = identity * Eigen::Translation3d(1, 2, 3);
+    Eigen::VectorXd err = tesseract_common::calcTransformErrorJac(identity, pi_rot);
+    EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d(1, 2, 3)));
+    EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d::Zero()));
+  }
+}
+
 /** @brief Tests calcTransformError */
 TEST(TesseractCommonUnit, computeRandomColor)  // NOLINT
 {