GraspFilter::filterGraspsHelper removes all poses due to timeout when checking a lot of grasps

[Levaru]

I have the problem that when filtering a huge amount of grasps (over 800), the GraspFilter::filterGraspsHelper will remove all of them due to the IK-solver reaching it's timeout limit. Most of the time this will happen after executing the filter a second time and will happen everytime after that.

I suspect that the problem is somehow related to the OpenMP for-loop. If the IK-solver also uses multithreading (I'm not sure) then I believe that when the for-loop uses all available threads, the IK-solvers are starved for CPU resources and won't be able to find a valid solution before reaching the timeout.

Is there any way to solve this problem?

I also noticed that when executing the GraspFilter::filterGraspsHelper multiple times, the omp_get_max_threads() function will always return the lowest amount that omp_set_num_threads(num_threads) was ever set to.

Example:
Run 1.
10 grasp_candidates
omp_get_max_threads() returns 12 threads
since num_threads > grasp_candidates.size() returns true, num_threads is set to 10

Run 2.
20 grasp_candidates
omp_get_max_threads() returns 10 threads

I thought that omp_get_max_threads() will return the maximum amount of usable threads, does it not? If yes, then why are the threads not freed afterwards?

[Levaru]

Nevermind, my problem was that I had unnormalized quaternions in my grasp poses which the IK solvers (KDL&LMA) apparently do not normalize before searching for a solution.