SolveType::Manip2 solution close to joint limit

[Former user]

When I am calculating a IK for my robot with SolveType::Manip2 I frequently get a IK solution close or quite on the joint limit. I had a look at the code and I am not sure if I just misunderstood the concept or found a bug.

SolveType::Manip2 should find a IK solution with well-conditioned Jacobian and some distance to the joint limit. The optimality criterion is (trac_ik.cpp line 260)

penalty = manipPenalty(q_out);
err = penalty*TRAC_IK::ManipValue2(q_out)

with manipPenalty() -> 0 for q_out close to joint limit and ManipValue2() -> 0 for an ill-conditioned Jacobian. So, err -> 0 for a suboptimal solution and err -> large for an optimal solution.

All solutions are sorted in the end (trac_ik.cpp line 509).

std::sort(errors.rbegin(),errors.rend());

std::sort() returns the values ascending. Hence, the solution taken is the worst, instead of the best.

q_out = solutions[errors[0].second];

Can anyone confirm this issue or show me where I'm wrong?

[Patrick Beeson]

You are incorrect on the point of the sort function.

sort using rbegin/rend iterators actually reverse the sort, so the resulting list is is descending order.

https://stackoverflow.com/questions/9025084/sorting-a-vector-in-descending-order

[Patrick Beeson]

If you really want to try to push the joint away from the limits for your robot, you can try to change the -10 scalar in the mainPenalty function to -1 or some other negative value closer to zero. This should penalize things away from the midpoint of the joints more and more., though I've found anything greater than -10 seems to bias the answers to always be "stuck" near nominal.