Bug: Search for cheapest IK solution about moveit_ikfast

[gavanderhoorn]

Hm, in the small amount of testing I did with two 6dof industrial manipulators, I did not notice a significant increase in time taken to find a solution. Nevertheless, I think what you propose makes sense: stop searching based on a convergence criterion, not (only) after reaching an iteration count.

Perhaps it is time we introduce some additional fields in KinematicsQueryOptions? @nalt also suggested this (to indicate the desired search strategy), but it seems a termination condition selector is also needed.

That would allow for runtime decisions on time vs quality of solutions fi.

Even with these changes response time is slower for Baxter, though the solutions are much smoother/better

Can you comment on how much slower things are? Intuitively I'd say something like time_of(single_solution) * iteration_count?

[nalt]

I measured a runtime of 20µs for each call to ComputeIK() on an i7. @davetcoleman, could you check how often searchPositionIK is called? Does your code use solution_callback? I guess with more complex planners, searchPositionIK() is called very often, and the 10ms runtime (@500 iterations) are too much.

ComputeIK() gives us 8 solutions each time, some of which are rejected by the joint limits. When iterating over the the free parameter, we get 8 distinct curves in the joint space, with some gaps in between due to the joint limits. This structure should be considered for the stop criterion: We do not know when we jump to another curve in the solution space. Thus, the check for the solution improvement will often result in a local minimum. For small motions, we expect that also the free joint does not move much. This is why we should mostly see the best solution within the first 30 iterations.

A save stop criterion could be

best_costs < fabs(search_discretization_*counter)

This stops the search when the motion of the free joint becomes larger than the best found solution. It needs to be checked if this is effective in practice, but it ensures that we never miss a better solution.

The number of iterations could be greatly reduced with setSearchDiscretization(). Maybe a discretization of 5° is enough? We might also need a hierarchic search: First use a stepping of, say, 10°, then refine.

[davetcoleman]

I'm only testing with interactive markers in Rviz right now. When clicking the arrow once (which results in 1 call to searchPositionIK()) it takes maybe 5 seconds to find a result. But if dragging the arrow it is rather unusable.

Perhaps it is time we introduce some additional fields in KinematicsQueryOptions?

Makes sense

Does your code use solution_callback?

its using the default Rviz plugin behavior, not sure

[nalt]

So the question is why searchPositionIK() takes 5s for you, but 10ms for me with the same number of iterations. Collision checking could be the reason, since it may take a long time - especially if the meshes are complex. If it is the reason, the search could be disabled whenever a collision callback is provided.
Have you seen the same behavior in the ClamArm environment?

[davetcoleman]

I'm testing with Baxter, using a very simplified geometric model. I don't think that is the reason, but rather IKFast generates custom header files specific to the geometry of each robot. Depending on your robot's axel alignment and other singularities, it will have different performance for different robots. I believe my generated IKFast header happens to output many more potentail solutions. Is your test robot 7dof like mine is?