ChainIkSolverPos_TL can be dramatically improved

[Davide Faconti]

Hi,

I found out that there is a much better strategy than the one used by ChainIkSolverPos_TL (random seed) to improve over the original chainiksolverpos_nr_jl.

Instead of clamping or wrapping the Q using the joint limits and hope for the best, you can move inside the nullspace to escape the joint limit.

In the benchmark ik_test.cpp I achieved these result with the PR2 arm:

1. original KDL using KDL::ChainIkSolverVel_pinv: 80%, 1.45 ms
2. original KDL using KDL::ChainIkSolverVel_pinv_givens: 86%, 1.15 ms
3. mine (joint limit avoid with nullspace, NO random seed): 97%, 0.45 ms

I will be happy to create a pull request... but it would be more convenient if the other one is accepted first ;) https://bitbucket.org/traclabs/trac_ik/pull-requests/12/cleanup

[Patrick Beeson]

Since I assume this is only going to affect the KDL cpp file, is it possible to make a PR for this fix?

[Davide Faconti]

yes! I will do it today if I have time or tomoroow