calcJacobianTransformErrorDiff sign change

[marrts]

I'm not entirely sure if this is a bug or expected behavior since I'm not super well versed in Jacobians, but I think this is a bug that isn't captured correctly.

In running trajopt I am getting a failure that looks like this: (also noticed opposite has a typo, but it made the error easy to track down lol)

calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!

This error is located here. Copied below for convenience.

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

Attaching a debugger I see my errors evaluate as:

// value = {axis, angle}
pose_rotation_error           = {[ 3.1016e-5,  1.896e-5, -0.9999999],  2.09439}
perturbed_pose_rotation_error = {[-3.5380e-5, -1.794e-5,  0.9999999], -2.09439}

These angles seem to be almost identical, just a singularity because of how the angle is represented.

Here are a couple of screenshots of the robot and the TCP at the approximate joint values where this calculation was being performed:

I know there was an effort to fix Jacobian related issues in #988 and I'm wondering if this scenario might have just been overlooked.

[marrts]

2.09439 radians

It's worth mentioning that this is an angle of 120 degrees, which will have some unique, weird properties

[Levi-Armstrong]

So this is a bug. The angle axis is pointing in opposite directions. Can you post the transform for the three inputs to create a unit test to track down the issue?

[Levi-Armstrong]

So I see I added the debug check but noticed there is an issue as the source and target converge but I didn't think it would cause issues with convergence but obviously I was wrong since you are seeing failures. I will post how to address this so you can test.

[marrts]

This seems to fix my issue, thanks!