Bug: Search for cheapest IK solution

[davetcoleman]

I finally got around to updating Baxter's ikfast plugins using the new #37 but unfortuantly I have found that the change has caused the IK solver to be incredibly slow. I suspect the robot the PR was tested on had much less redudancy, and so no performance difference was experienced. With the 7dof Baxter arms I am finding that ikfast will loop through ~530 ik solutions each request, requiring the cost of each solution to be calculated. I dug deeper into the code and I found that the best "cheapest" solution is almost always found between 1 and 30 iterations - meaning the remaining 500 checks are a waste of time.

The MoveIt Motion Planning Plugin using IKFast is now almost unusable.

These are preliminary ideas, but with Baxter I have added the following logic:

• If a solution has been found after 30 iterations, stop and use that one
• If the difference in costs between best solution improvements drops below 0.01, return that solution

You can see a messy hack for this in this commit:
davetcoleman/baxter_cpp@407d72d

Note that I also had to reduce the kinematic.yaml settings for better performance.

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

@nalt

[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?