Revision history [back]

Patrick,

This is a shortcoming of the current plugin being used in arm_kinematics_constraint_aware. As it's a numerical optimization solver, the KDL IK is very sensitive to the seed state at which the search begins. It will generally find a good solution quickly when the seed state is close to the goal state - this is why the mode that you use in the planning_components_visualizer works so well, as we constantly update the seed state such that each new pose is only slightly different than the old pose.

When you are invoking move_arm this will be set by default to the current state of the robot's arm - when you are dragging the arm further and further away from the desired pose the seed state gets worse and worse, and the numerical optimization may get stuck in a local minima.

There are a couple parameters you can tune in the IK plugin that will make it try harder to find solutions. The first is the 'max_solver_iterations' - it's set to 500 by default, but you may get better performance increasing it to 1000 or more. The second is 'max_search_iterations' - currently set to 3 by default. If KDL fails to converge by the requested number of iterations starting with the supplied seed state, it will randomly reseed the joints in between the limits and try again, repeating this process up to the requested number of search iterations.

The side-effect of increasing these parameters is that when KDL doesn't find a solution it can take a long time to return. On my machine the current time to return with no solution given the default parameters is about 10 milliseconds, which doesn't seem like a long time, but the PR2 analytic solver will return a failure in less than 10 microseconds.

The last option is to explore OpenRAVE's ikfast, which should provide analytical solutions quickly but is not currently integrated into the wizard pipeline. I'll be looking into integrating it but it probably won't make it into a release until F-turtle.