SolveType::Manip2 solution close to joint limit
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?
Comments (3)
-
Account Deactivated -
Account Deactivated 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.
-
Account Deactivated - changed status to closed
- Log in to comment
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