ur::kinematics.inverse() issue for some homogeneous matrix configurations (when coming from UR5 negative joint values)

[zenone87]

Dear all,

I am experiencing some issues in using the ur::kinematics.inverse(const double* T, double* q_sols, double q6_des) function.

It seems to work only for certain configurations of the input homogeneous matrix.

To test it, I define a set of fixed joint values, get the homogeneous matrix using the forward kinematics, and retrive again the joint values using the inverse kinematics.

For example, if I run

double JointValues[6] = {0.17462, 0.18904, 0.17868, 0.00054, 1.74530, 0.00004};
ur_kinematics::forward(JointValues, (double*)homo_ik_pose);
int num_sols = ur_kinematics::inverse((double*)homo_ik_pose, (double*)q_ik_sols, JointValues[5]);

I get

End-effector position (x,y,z): 0.796035 : 0.240161 : -0.247539
Joint values (rad): 0.17462, 0.18904, 0.17868, 0.00054, 1.74530, 0.00004

Number of valid solutions: 4
Solution (rad) 1: 0.17868, 0.18904, 0.17462, 0.00054, 1.74530, 0.00004
Solution (rad) 2: 0.17868, 0.35664, 6.10856, 0.18218, 1.74530, 0.00004
Solution (rad) 3: 3.60919, 2.57530, 0.12640, 0.04114, 1.12785, 3.25420
Solution (rad) 4: 3.60919, 2.69663, 6.15678, 0.17262, 1.12785, 3.25420

Best solution (rad): 0.17868, 0.18904, 0.17462, 0.00054, 1.74530, 0.00004

as expected.

On the other hand, if one of the input joint values is negative...

double JointValues[6] = {-1.48344, 0.18904, 0.17868, 0.00054, 1.74530, 0.00004};
ur_kinematics::forward(JointValues, (double*)homo_ik_pose);
int num_sols = ur_kinematics::inverse((double*)homo_ik_pose, (double*)q_ik_sols, JointValues[5]);

I get

End-effector position (x,y,z): 0.610661 : 0.206683 : 0.438742
Joint values (rad): -1.48344, 0.18904, 0.17868, 0.00054, 1.74530, 0.00004

Number of valid solutions: 8
Solution (rad) 1: 0.17868, 5.06218, 1.48344, 4.72688, 1.74530, 0.00004
Solution (rad) 2: 0.17868, 0.18904, 4.79974, 0.00054, 1.74530, 0.00004
Solution (rad) 3: 0.17868, 5.55134, 0.82953, 1.75004, 4.53789, 3.14164
Solution (rad) 4: 0.17868, 0.06241, 5.45365, 2.61486, 4.53789, 3.14164
Solution (rad) 5: 3.67128, 3.09871, 0.80537, 0.60719, 1.31231, 2.79247
Solution (rad) 6: 3.67128, 3.86994, 5.47782, 1.44669, 1.31231, 2.79247
Solution (rad) 7: 3.67128, 2.93564, 1.50112, 3.21610, 4.97087, 5.93406
Solution (rad) 8: 3.67128, 4.36204, 4.78206, 4.79193, 4.97087, 5.93406

Best solution (rad): 0.17868, 0.18904, 4.79974, 0.00054, 1.74530, 0.00004

which is not what I expected.

Are there some constraints on the homogeneous matrix that I should consider? From the homogeneous matrix, how can I be sure that the solutions provided by the IK solver are correct?

Moreover, it is not clear to me how I should set the last input parameter q6_des given a desired pose of the end-effector (via the homogeneous matrix).

Thank you very very much!

[zenone87]

The position of the end-effector was actually always correct, and ur_kinematics::inverse always outputs joint angles between [0, 2*pi).