search

Jmeyer1292 / opw_kinematics

Closed form IK for parallel base, spherical wrist industrial manipulators
Apache License 2.0
69 stars 26 forks source link

Numerical issues with numbers close to zero in pose rotation matrix #60

Closed Levi-Armstrong closed 2 years ago

Levi-Armstrong commented 2 years ago

I have been running into issues where OPW kinematics produces incorrect IK solutions and have tracked it down where the matrix has very small numbers which are not zero. As shown below if you replace the 0 with 4.9065389333868E-018 you get a significant error in the solutions returned. I currently looking into how best to resolve this issue, but if anyone has suggestions please let me know.

  Transform<double> pose2{Transform<double>::Identity()};
  pose2(0,0) = -0.707106781186547;
  pose2(1,0) = 0;
  pose2(2,0) =-0.707106781186548;
  pose2(0,1) = 0; //4.9065389333868E-018;
  pose2(1,1) = 1;
  pose2(2,1) = 0; //-4.9065389333868E-018;
  pose2(0,2) = 0.707106781186548;
  pose2(1,2) = 0;
  pose2(2,2) =-0.707106781186547;
  pose2(0,3) = 1.3485459941112;
  pose2(1,3) = 0;
  pose2(2,3) =0.399038059222874;
Levi-Armstrong commented 2 years ago

@marip8 I found the issue.

The issue is that the algorithm uses std::atan2 which has numerical issues when both values are close to zero. For example std::atan2(6.938893903907231e-18, 7.216449660063518e-16) = 0.00961509 but should be zero because both values are zero.

Levi-Armstrong commented 2 years ago

Addressed #61