diff --git a/tesseract_common/include/tesseract_common/utils.h b/tesseract_common/include/tesseract_common/utils.h
index dd107d0255a..d118fe2a375 100644
--- a/tesseract_common/include/tesseract_common/utils.h
+++ b/tesseract_common/include/tesseract_common/utils.h
@@ -114,31 +114,9 @@ Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b);
  */
 Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R);
 
-/**
- * @brief Calculate the rotation error vector given a rotation error matrix where the angle is between [0, 2 * pi]
- * @details This function does not break down when the angle is near zero or 2pi when calculating the numerical
- * jacobian. This is because when using Eigen's angle axis it converts the angle to be between [0, PI] where internally
- * if the angle is between [-PI, 0] it flips the sign of the axis. Both this function and calcRotationalError both check
- * for this flip and reverts it. Since the angle is always between [-PI, PI], switching the range to [0, PI] will
- * never be close to 2PI. In the case of zero, it also does not break down because we are making sure that the angle
- * axis aligns with the quaternion axis eliminating this issue. As you can see the quaternion keeps the angle small but
- * flips the axis so the correct delta rotation is calculated.
- *
- * Angle: 0.001 results in an axis: [0, 0, 1]
- * Angle: -0.001 results in and axis: [0, 0, -1]
- * e1 = angle * axis = [0, 0, 0.001]
- * e2 = angle * axis = [0, 0, -0.001]
- * delta = e2 - e1 = [0, 0, 0.002]
- *
- * @details This should be used when numerically calculating rotation jacobians
- * @param R rotation error matrix
- * @return Rotation error vector = Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()
- */
-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
+ * @warning This should not be used to calculate numerical jacobian, see calcJacobianTransformErrorDiff
  * @param t1 Target Transform
  * @param t2 Current Transform
  * @return error [Position, calcRotationalError(Angle Axis)]
@@ -146,13 +124,30 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R)
 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)]
+ * @brief Calculate jacobian transform error difference expressed in the target frame coordinate system
+ * @details This is used when the target is a fixed frame in the environment
+ * @param target Target The desired transform to express the transform error difference in
+ * @param source The current location of the source transform
+ * @param source_perturbed The perturbed location of the source transform
+ * @return The change in error represented in the target frame
+ */
+Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
+                                               const Eigen::Isometry3d& source,
+                                               const Eigen::Isometry3d& source_perturbed);
+
+/**
+ * @brief Calculate jacobian transform error difference expressed in the target frame coordinate system
+ * @details This is used when the target and source are both dynamic links
+ * @param target Target The desired transform to express the transform error difference in
+ * @param target_perturbed The perturbed location of the target transform
+ * @param source The current location of the source transform
+ * @param source_perturbed The perturbed location of the source transform
+ * @return The change in error represented in the target frame
  */
-Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);
+Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
+                                               const Eigen::Isometry3d& target_perturbed,
+                                               const Eigen::Isometry3d& source,
+                                               const Eigen::Isometry3d& source_perturbed);
 
 /**
  * @brief This computes a random color RGBA [0, 1] with alpha set to 1
diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp
index 91bad6fec16..1f0f89d7399 100644
--- a/tesseract_common/src/utils.cpp
+++ b/tesseract_common/src/utils.cpp
@@ -123,7 +123,7 @@ Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b)
   return out;
 }
 
-Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
+std::pair<Eigen::Vector3d, double> calcRotationalErrorDecomposed(const Eigen::Ref<const Eigen::Matrix3d>& R)
 {
   Eigen::Quaterniond q(R);
   Eigen::AngleAxisd r12(q);
@@ -144,31 +144,13 @@ Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
 
   assert(std::abs(angle) <= M_PI);
 
-  return axis * angle;
+  return { axis, angle };
 }
 
-Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R)
+Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
 {
-  Eigen::Quaterniond q(R);
-  Eigen::AngleAxisd r12(q);
-
-  // Eigen angle axis flips the sign of axis so rotation is always positive which is
-  // not ideal for numerical differentiation.
-  int s = (q.vec().dot(r12.axis()) < 0) ? -1 : 1;
-
-  // Make sure that the angle is on [0, 2 * pi]
-  const static double two_pi = 2.0 * M_PI;
-  double angle = s * r12.angle();
-  Eigen::Vector3d axis{ s * r12.axis() };
-  angle = copysign(fmod(fabs(angle), two_pi), angle);
-  if (angle < 0)
-    angle += two_pi;
-  else if (angle > two_pi)
-    angle -= two_pi;
-
-  assert(angle <= two_pi && angle >= 0);
-
-  return axis * angle;
+  std::pair<Eigen::Vector3d, double> data = calcRotationalErrorDecomposed(R);
+  return data.first * data.second;
 }
 
 Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2)
@@ -177,10 +159,74 @@ 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::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
+                                               const Eigen::Isometry3d& source,
+                                               const Eigen::Isometry3d& source_perturbed)
+{
+  Eigen::Isometry3d pose_err = target.inverse() * source;
+  std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
+  Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);
+
+  Eigen::Isometry3d perturbed_pose_err = target.inverse() * source_perturbed;
+  std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
+      calcRotationalErrorDecomposed(perturbed_pose_err.rotation());
+
+  // They should always pointing in the same direction
+#ifndef NDEBUG
+  if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
+    throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
+#endif
+
+  // 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(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(perturbed_pose_err.translation(),
+                           perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI));
+  else
+    perturbed_err = concat(perturbed_pose_err.translation(),
+                           perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);
+
+  return (perturbed_err - err);
+}
+
+Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
+                                               const Eigen::Isometry3d& target_perturbed,
+                                               const Eigen::Isometry3d& source,
+                                               const Eigen::Isometry3d& source_perturbed)
+{
+  Eigen::Isometry3d pose_err = target.inverse() * source;
+  std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
+  Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);
+
+  Eigen::Isometry3d perturbed_pose_err = target_perturbed.inverse() * source_perturbed;
+  std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
+      calcRotationalErrorDecomposed(perturbed_pose_err.rotation());
+
+  // They should always pointing in the same direction
+#ifndef NDEBUG
+  if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
+    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
+  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_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_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI));
+  else
+    perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
+                           perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);
+
+  return (perturbed_err - err);
 }
 
 Eigen::Vector4d computeRandomColor()
diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp
index be53a0884f3..cbf7d507b36 100644
--- a/tesseract_common/test/tesseract_common_unit.cpp
+++ b/tesseract_common/test/tesseract_common_unit.cpp
@@ -2036,80 +2036,6 @@ TEST(TesseractCommonUnit, calcRotationalError)  // NOLINT
   }
 }
 
-/** @brief Tests calcRotationalError2 which return angle between [0, 2 * PI]*/
-TEST(TesseractCommonUnit, calcRotationalError2)  // NOLINT
-{
-  auto check_axis = [](const Eigen::Vector3d& axis) {
-    return (axis.normalized().isApprox(Eigen::Vector3d::UnitZ(), 1e-6) ||
-            axis.normalized().isApprox(-Eigen::Vector3d::UnitZ(), 1e-6));
-  };
-  Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
-  Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(3 * M_PI_2, Eigen::Vector3d::UnitZ());
-  Eigen::Vector3d rot_err = tesseract_common::calcRotationalError2(pi_rot.rotation());
-  EXPECT_NEAR(rot_err.norm(), M_PI_2, 1e-6);
-  EXPECT_TRUE(check_axis(rot_err.normalized()));
-
-  pi_rot = identity * Eigen::AngleAxisd(0.0001, Eigen::Vector3d::UnitZ());
-  rot_err = tesseract_common::calcRotationalError2(pi_rot.rotation());
-  EXPECT_NEAR(rot_err.norm(), 0.0001, 1e-6);
-  EXPECT_TRUE(check_axis(rot_err.normalized()));
-
-  // Test greater than 2 * PI
-  pi_rot = identity * Eigen::AngleAxisd(3 * M_PI, Eigen::Vector3d::UnitZ());
-  rot_err = tesseract_common::calcRotationalError2(pi_rot.rotation());
-  EXPECT_NEAR(rot_err.norm(), M_PI, 1e-6);
-  EXPECT_TRUE(check_axis(rot_err.normalized()));
-
-  // Test lessthan than 0
-  pi_rot = identity * Eigen::AngleAxisd(-M_PI, Eigen::Vector3d::UnitZ());
-  rot_err = tesseract_common::calcRotationalError2(pi_rot.rotation());
-  EXPECT_NEAR(rot_err.norm(), M_PI, 1e-6);
-  EXPECT_TRUE(check_axis(rot_err.normalized()));
-
-  // Test for angle between [0, 2 * PI]
-  Eigen::Isometry3d pi_rot_plus = identity * Eigen::AngleAxisd(M_PI + 0.001, Eigen::Vector3d::UnitZ());
-  Eigen::Isometry3d pi_rot_minus = identity * Eigen::AngleAxisd(M_PI - 0.001, Eigen::Vector3d::UnitZ());
-  Eigen::Vector3d pi_rot_delta = tesseract_common::calcRotationalError2(pi_rot_plus.rotation()) -
-                                 tesseract_common::calcRotationalError2(pi_rot_minus.rotation());
-  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
-
-  // Test for angle at 0
-  pi_rot_plus = identity * Eigen::AngleAxisd(0.001, Eigen::Vector3d::UnitZ());
-  pi_rot_minus = identity * Eigen::AngleAxisd(-0.001, Eigen::Vector3d::UnitZ());
-  pi_rot_delta = tesseract_common::calcRotationalError2(pi_rot_plus.rotation()) -
-                 tesseract_common::calcRotationalError2(pi_rot_minus.rotation());
-  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
-
-  // Test for angle at 2 * PI
-  pi_rot_plus = identity * Eigen::AngleAxisd((2 * M_PI) + 0.001, Eigen::Vector3d::UnitZ());
-  pi_rot_minus = identity * Eigen::AngleAxisd((2 * M_PI) - 0.001, Eigen::Vector3d::UnitZ());
-  pi_rot_delta = tesseract_common::calcRotationalError2(pi_rot_plus.rotation()) -
-                 tesseract_common::calcRotationalError2(pi_rot_minus.rotation());
-  EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
-
-  // Test random axis
-  for (int i = 0; i < 100; i++)
-  {
-    Eigen::Vector3d axis = Eigen::Vector3d::Random().normalized();
-
-    // Avoid M_PI angle because this breaks down
-    Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(1000, -5 * M_PI, 5 * M_PI);
-    for (Eigen::Index j = 0; j < angles.rows(); j++)
-    {
-      pi_rot_plus = identity * Eigen::AngleAxisd(angles(j) + 0.001, axis);
-      pi_rot_minus = identity * Eigen::AngleAxisd(angles(j) - 0.001, axis);
-      Eigen::Vector3d e1 = tesseract_common::calcRotationalError2(pi_rot_plus.rotation());
-      Eigen::Vector3d e2 = tesseract_common::calcRotationalError2(pi_rot_minus.rotation());
-      EXPECT_FALSE((e1.norm() < 0));
-      EXPECT_FALSE((e1.norm() > 2 * M_PI));
-      EXPECT_FALSE((e2.norm() < 0));
-      EXPECT_FALSE((e2.norm() > 2 * M_PI));
-      pi_rot_delta = e1 - e2;
-      EXPECT_NEAR(pi_rot_delta.norm(), 0.002, 1e-6);
-    }
-  }
-}
-
 /** @brief Tests calcTransformError */
 TEST(TesseractCommonUnit, calcTransformError)  // NOLINT
 {
@@ -2144,38 +2070,300 @@ TEST(TesseractCommonUnit, calcTransformError)  // NOLINT
   }
 }
 
-/** @brief Tests calcTransformErrorJac */
-TEST(TesseractCommonUnit, calcTransformErrorJac)  // NOLINT
+void runCalcJacobianTransformErrorDiffTest(double anlge)
 {
   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)));
+  const double eps = 0.001;
+  {  // X-Axis positive
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(eps, 0, 0)));
+  }
+
+  {  // X-Axis negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-eps, 0, 0)));
+  }
+
+  {  // X-Axis positive and negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
+  }
+
+  {  // X-Axis translation
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(eps, -eps, eps)));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-eps, 0, 0)));
+  }
+
+  {  // Y-Axis positive
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, eps, 0)));
+  }
+
+  {  // Y-Axis negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -eps, 0)));
+  }
+
+  {  // Y-Axis positive and negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
+  }
+
+  {  // Y-Axis translation
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, eps, -eps)));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -eps, 0)));
+  }
+
+  {  // Z-Axis positive
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, eps)));
+  }
+
+  {  // Z-Axis negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -eps)));
+  }
+
+  {  // Z-Axis positive and negative
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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(target_tf, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d::Zero()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
+  }
+
+  {  // Z-Axis translation
+    Eigen::Isometry3d target_tf{ identity };
+    target_tf.translation() = Eigen::Vector3d(1, 2, 3);
+    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, source_tf, source_tf_perturbed);
+    EXPECT_TRUE(diff.head(3).isApprox(Eigen::Vector3d(-eps, -eps, eps)));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -eps)));
   }
+}
 
-  {  // 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)));
+void runCalcJacobianTransformErrorDiffDynamicTargetTest(double anlge)
+{
+  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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(2 * eps, 0, 0)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-2 * eps, 0, 0)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(-3 * eps, 0, 0)));
+  }
+
+  {  // 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::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.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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 2 * eps, 0)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -2 * eps, 0)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, -3 * eps, 0)));
+  }
+
+  {  // 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::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.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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, 2 * eps)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
+  }
+
+  {  // 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::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()));
+    EXPECT_TRUE(diff.tail(3).isApprox(Eigen::Vector3d(0, 0, -3 * eps)));
+  }
+
+  {  // 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::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.tail(3).isApprox(Eigen::Vector3d(0, 0, -2 * eps)));
   }
+}
 
-  {  // 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 calcJacobianTransformErrorDiff */
+TEST(TesseractCommonUnit, calcJacobianTransformErrorDiff)  // NOLINT
+{
+  runCalcJacobianTransformErrorDiffTest(0);
+  runCalcJacobianTransformErrorDiffTest(M_PI_2);
+  runCalcJacobianTransformErrorDiffTest(M_PI);
+  runCalcJacobianTransformErrorDiffTest(3 * M_PI_2);
+  runCalcJacobianTransformErrorDiffTest(2 * M_PI);
+
+  runCalcJacobianTransformErrorDiffDynamicTargetTest(0);
+  runCalcJacobianTransformErrorDiffDynamicTargetTest(M_PI_2);
+  runCalcJacobianTransformErrorDiffDynamicTargetTest(M_PI);
+  runCalcJacobianTransformErrorDiffDynamicTargetTest(3 * M_PI_2);
+  runCalcJacobianTransformErrorDiffDynamicTargetTest(2 * M_PI);
 }
 
 /** @brief Tests calcTransformError */