[ur_moveit_plugin] Use full range in IK if joints support it without solution explosion

[hartmanndennis]

The solver of ur_kinematics returns solutions in range [0,2PI]. #184 fixed this for the limited version by in effect mapping this to [-PI;PI] range. But this didn't address the issue for the full range version.
#305 fixes this by generatic the extra solutions in the full range but this adds quite a lot of solutions.
This PR extends #184 by checking every joint if it is more than 180° away from the seed. If it is, rotating 360° will be beneficial. If the joint limits allow it, the rotation is applied. This always returns the best solutions in the full range of the joint limits.

This, together with #302 and changing longest_valid_segment_fraction to something lower make ur_kinematics usable with full joint range.

[gavanderhoorn]

Same as for #239 and #305: this should be reviewed together with the changes in those PRs.

I won't do that before ROSCon however, and in any case would like some additional eyes on this.

[arunavanag591]

#include <moveit_msgs/GetKinematicSolverInfo.h>
needs to be replaced with
#include <moveit_msgs/KinematicSolverInfo.h>

in universal_robot/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h

when building in kinetic for #239

[gavanderhoorn]

Tagging this PR with wrid18 as this is a PR that would be nice to review.

#239 and #305 should be taken into account / considered as well.

[mxgrey]

I recommend closing this PR in favor of #358 which combines #239 and #305. While this approach is more efficient than the implementation in #305, it is also less generalized, and does not compute all possible joint configurations, which is needed by #239.

If it can be shown by benchmarks that this approach offers a considerable performance improvement over #358, then it may be worth using this approach in the single-solution implementation of getPositionIK, but the method of #305 would still needed for the multi-solution approach.

[hartmanndennis]

This PR changes only searchPositionIK, which is expected to return only one solution, ideally the closest solution to the seed. Moveit uses this function almost exclusively for planning and not getPositionIK.
This implementation does give the closest possible solution to the seed without enumerating every possible but worse extra solutions. By enumerating you are generating 2^6*8 - 8 = 504 extra solutions and sort them without having any benefit for this function.

[mxgrey]

Your points are well-taken.

I was curious about investigating the significance of the performance difference, so I created a simple performance test for the function.

I've created a branch that merges 316 into the rest (merge_239_305_316) and compared the test results to merge_239_305 which is the branch for the PR.

On my machine, for both implementations, the average run time per function call varied from 32-35us. The 239_305_316 typically outperformed 239_305, but usually by less than 1 us.

I would encourage others to try running the test and see what kind of results you get. I feel that the difference in performance seems negligible and is mostly washed out by the rest of the computations being performed. However, it is better, if only by a small percentage, and maybe some would find that valuable.

[hartmanndennis]

Thanks for writing the test but there are some mistakes which make your result not correct.
In the launch file change <arg name="limited" value="true"/> to false because the whole point of this PR is to use the unlimited version.
You should also check that your generated poses have a valid inverse. Testing it here, almost no poses do.
I changed your test so it does the inverse of a fixed pose with a valid inverse and the result of 100000 iterations for this PR are Average time per query: 4.33714 us and for #358 Average time per query: 28.865 us.

[mxgrey]

Thanks for inspecting the test.

I've made the changes you suggested (the repo has also been updated), and I've seen the same trend that you're reporting. When limited is false and the requested poses are consistently in a solvable region, your approach performs several times faster than the one from #305 .

I've gone ahead and merged your approach into #358 . The only caveat (as I understand it) is this approach seems to make some assumptions about the outer limits of what the joint position limits can be. If the joint limits are ever extended past those assumptions, then someone will have to remember to tweak this implementation (although please do correct me if I'm misunderstanding that).

[gavanderhoorn]

@hartmanndennis: thanks for your work as well. 👍 🍻