Initial JointConfiguration not found when calculating FK and IK

[GuyWithCookies]

Summary

Calculating the forward and inverse kinematics of an UR5 doesn't return the initial joint configuration.

Expected Results

Calculating the forward kinematic of a given joint configuration (e.g. {M_PI, M_PI, M_PI, M_PI, M_PI, M_PI}) and using the solution to calculate the inverse kinematics should give back the input joint configuration (along with other possible solutions).

JointConfiguration -- forward --> CartesianPose -- inverse --> JointConfiguration

Actual Results

The inverse returns (viable) solutions but doesn't contain the input joint configuration.
(Note: For some rare cases, e.g. {0, 0, 0, 0, 0, 0} the initial joint configuration is obtained. However for many JointConfiguration (as for example {M_PI, M_PI, M_PI, M_PI, M_PI, M_PI}) the inital JointConfiguration is not obtained)

Steps to reproduce

The example code in ur_kinematics/src/ur_kin.cpp was used to test this issue.
The main function was adapted to use different input joint configuration:

Code:

int main(int argc, char* argv[])
{
  // Input JointConfiguraition
  double q[6] = {M_PI, M_PI, M_PI, M_PI, M_PI, M_PI};
  double* T = new double[16];
  forward(q, T);
  for(int i=0;i<4;i++) {
    for(int j=i*4;j<(i+1)*4;j++)
      printf("%1.3f ", T[j]);
    printf("\n");
  }
  double q_sols[8*6];
  int num_sols;
  num_sols = inverse(T, q_sols);
  for(int i=0;i<num_sols;i++) 
    printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n", 
       q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
  for(int i=0;i<=4;i++)
    printf("%f ", PI/2.0*i);
  printf("\n");
  return 0;
}

Output:

Forward Solution:
0.000 1.000 -0.000 0.033
1.000 -0.000 0.000 -0.027
-0.000 -0.000 -1.000 0.184
0.000 0.000 0.000 1.000

Inverse Solutions:
3.724590 0.000000 3.141593 0.000000 2.558595 3.141593
3.724590 0.000000 3.141593 0.000000 2.558595 3.141593
3.724590 3.713447 2.673703 6.179221 3.724590 0.000000
3.724590 6.053951 3.609482 2.902937 3.724590 0.000000
3.141593 3.370827 2.673703 0.238656 3.141593 0.000000
3.141593 5.711331 3.609482 3.245557 3.141593 0.000000
3.141593 3.370827 2.673703 0.238656 3.141593 0.000000
3.141593 5.711331 3.609482 3.245557 3.141593 0.000000

// expected output would contain (or a 2PI multiple of it):
3.141593 3.141593 3.141593 3.141593 3.141593 3.141593 

---

More Info

I'm aware of the issues (e.g.#442) & PR (e.g. #453) which are already targeting missing IK solutions. These issues discuss missing solutions which are multiples of 2PI (a solution for this issue is already suggested in PR #453). However as far as I can see the issue I describe above does not fall into this category due the fact that the returned IK solutions don't contain a solution which is a multiple of the input joint configuration ({M_PI, M_PI, M_PI, M_PI, M_PI, M_PI}).

[GuyWithCookies]

The problem was an issue in the ForwardKinematic when I ported the calculations to my project.
Closing this as it is not an issue of this project