Search for cheapest IK solution

- searchPositionIK() returned the first collision-free solution within joint limits it finds. (The free joint is moved as little as possible.) This approach regularly leads to large and ugly motions in joint space even for small displacements of the EE, see video [1]. Instead, the solution with the smallest costs should be returned. Here, the largest motion of any joint is taken as the cost term. Trajectories become much smoother, see video [2].
- Due to the speed of IK fast, it is ok to search the entire search space. However, if callbacks for collision checking are used, a coarse-to-fine search scheme might be needed.
- getCount() was called with a positive third argument. Thus, no search in negative direction was performed.

[1] https://www.youtube.com/watch?v=-VX709r0N2Y
[2] https://www.youtube.com/watch?v=BFpG8TY6aqk