Fix numerical issue in inverse function

[Levi-Armstrong]

The issue is a result of the derived equation break down when joint 5 is zero because there are infinite combinations of joint 4 and joint 6 which satisfy the pose. The changes here add a check for the condition when joint 5 is zero and I have derived an alternative method for calculating joint 6 rotation for joint 4 set to zero based on the equations in the paper.

In addition, we confirmed that the author's Matlab code has the same issue.

Also, update the inverse function to run forward kin check if built-in debug to find issues where the inverse returns incorrect solutions.

[Levi-Armstrong]

This addresses issue #61

[Levi-Armstrong]

@marip8 are you able to review?

[Levi-Armstrong]

This seems to be a bigger issue than I originally thought. I add the check to the inverse kinematics to compare forward kinematics when building in debug and it is failing all over the place. I added a new unit test which shows that the recent change fixed one of the tests but still fails for the new one. I think there may be issues with the implementation based on the paper. I was able to find Matlab code from the original author which we are going to compare. There is some difference between the two implementations around where the // ??? are in this code which indicates that something may not have been clear in the paper.

[Levi-Armstrong]

The function atan2 is undefined if both a and b are zero. In the unit test cases, there are calls to atan2 where both a and b are below the epsilon for a double which is 2.22045e-16 indicating that it is operating in the undefined region of atan2. It appears that the garage solutions only appear if there is a situation where atan2 was called where both numbers are very small. Both of the poses in the unit test were generated with joints 4, 5, and 6 all being zero which means for the IK solution there is an infinite combination of 4 and 6 that satisfy the pose. I wonder if this is what is causing the breakdown in the current implementation which you would think we could check for this condition somehow if it turns out to be the case.

[Jmeyer1292]

The changes here add a check for the condition when joint 5 is zero and I have derived an alternative method for calculating joint 6 rotation for joint 4 set to zero based on the equations in the paper.

Can you describe the intended behavior a little bit? How much of a change in behavior does this represent? With the acknowledgement that it was giving incorrect answers (by how much?) will a planner sampling along a line experience a sharp discontinuity for a single pose if it happens to land in the joint 5 ~ 0 region? Perhaps this was already the behavior.

Also curious what the IKFast model does.

[Levi-Armstrong]

Can you describe the intended behavior a little bit? How much of a change in behavior does this represent? With the acknowledgement that it was giving incorrect answers (by how much?) will a planner sampling along a line experience a sharp discontinuity for a single pose if it happens to land in the joint 5 ~ 0 region? Perhaps this was already the behavior.

Comment added to the code below. This is the singularity issue that industrial controllers have. This is a discontinuity and could be sharp +/- PI. Though since the controller knows the previous joint state it most likely sets joint 4 to the previous state value and calculates a new joint 6. Though in the case of motion planning we do not have this luxury.

  // The derived equations in the paper are geometric and break down when joint 5 is equal to zero.
  // When joint 5 is equal to zeros the values passed to std::atan2 are both zero which is undefined.
  // This can result in significant rotation error up to PI between joint 4 and joint 6 each.
  // In the paper it defines an equation for Rc which is the rotation matrix of joint 5 relative to the base.
  // If you set joint 4 to zero and joint 5 is zero it results in the matrix below where the z-axis is the
  // same as the provided pose. Next, it takes the poses x-axis and projects it onto Rc and then leverage
  // std::atan2 to calculate the joint 6 angle.

[Jmeyer1292]

Thanks for responding @Levi-Armstrong. I obviously defer to your judgement and expertise, so please move forward.

It does make me thing aloud about how to correctly integrate information like this into a sampling planner.

1. We could have special case logic in the IK solver or in a wrapper function (ala the harmonize functions) that indicates to the user that additional solutions are available and helps them sample them.
2. We could incorporate smarts into graph search edge finders such that they detect these degenerate cases and can smartly compute a cost and a motion that is feasible.
3. ???

[Levi-Armstrong]

It does make me thing aloud about how to correctly integrate information like this into a sampling planner.

1. We could have special case logic in the IK solver or in a wrapper function (ala the harmonize functions) that indicates to the user that additional solutions are available and helps them sample them.

2. We could incorporate smarts into graph search edge finders such that they detect these degenerate cases and can smartly compute a cost and a motion that is feasible.

3. ???

You are certainly correct that this would cause undesirable behavior with sample-based planners especially Descartes if it were to pass through a singularity. It would be good to implement samplers for this kinematic arrangement which detect Joint 5 at zeros and generate additional solutions so you would have a smoother transition.