Slow Collision Computation

[lukas-ramlab]

For motion planning in robotics I'm using pinocchio with coal.
In some scenarios, the algorithm requires the distance (not just the collision check) to make more informed decision where to move. The distance computation is much slower than just collision computation (~ 100x) which is expected.

Since I only require the distance when being close to collision, I now want to accelerate the computation by first checking if two bodies are within range by setting collision_data.collisionRequests[it].security_margin = XXX , and then checking for collision. Only they collide with margin, I will proceed to compute the collision.

However, what I notice now is that this collision check which used to take ~1ms, becomes very slow for certain objects pairs >1s, when setting relatively high security_margin=0.5.

Looking at the meshes shows that the collision model has many of BVs: approximately 4 BVs per vertex.

First object:

(Pdb) collision_model.geometryObjects[pair.first].geometry
<coal.coal_pywrap.BVHModelOBBRSS object at 0x7f37e1b9c660>
(Pdb) collision_model.geometryObjects[pair.first].geometry.getNumBVs()
1199
(Pdb) collision_model.geometryObjects[pair.first].geometry.vertices().shape
(310, 3)

Second object:

(Pdb) collision_model.geometryObjects[pair.second].geometry
<coal.coal_pywrap.BVHModelOBBRSS object at 0x7f37e1b9c660>
(Pdb) collision_model.geometryObjects[pair.second].geometry.getNumBVs()
491139
(Pdb) collision_model.geometryObjects[pair.second].geometry.vertices().shape
(122787, 3)

Both input objects are triangle meshes. I use the python binding for faster prototyping, but since it's a single function call this should have a negligible on the computation time.

Is this large increase in computational time expected? And is there a specific way of constructing the BV-objects that can improve the computational speed?

And lastly, is there a better option to speed up distance-computation, if I only need the exact distance if it is below a certain margin?

Thanks for any input!

[lukas-ramlab]

Some more information to reproduce the issue, attached a minimal example python script.
Where the output is:

>>> python3 scripts/minimal_example.py 
Collision computation took 0.000016s.
Collision computation with margin=1.0 took 0.147513s.
Distance computation took 0.007977s.

From the example (and output) it becomes clear that the collision computation is much slower with an safety_margin. This is expected, since the two parts are closer to collision. However, the collision computation is also much slower than the actual distance computation. This is unexpected, especially the slowdown of a factor 20x.
The slowdown is not always the case, but I observe it for mesh-pairs which are close to collision.

Files:
minimal_example.py
parts.zip

[hazidh]

#751
Hello, I also encountered a similar problem before. When adding the safety distance, the calculation speed would become very slow. This was the link at that time. It seems that there wasn't a very good solution. If you have found any good solutions, please feel free to share them.

[lukas-ramlab]

Thanks for the info!

I indeed need the large margin, since I want to use it to speed up distance computation (only compute the actual distance below a specific margin).

I will try to use the AABB-representation for an estimate if I'm within proximity of collision, and see if that sufficiently speeds up the simulation.