Inconsistency between collision checking processes

[mpowelson]

This issue is to document discussions with @Levi-Armstrong about inconsistencies between processes in a motion planning pipeline with regards to collision checking. I'm documenting them here for others to find with similar problems.

From #435:

What prompted this was me chasing an issue with inconsistency between FixStateCollisionProcessGenerator and DiscreteContactCheckProcessGenerator. I'm getting plans where FixStateCollisionProcessGenerator with distance set to 0.005 will say it is not in collision, but then a DiscreteContactCheckProcessGenerator set to 0.004 right after that says it has a distance of 0.003907 and will fail because it is in collision.

There are a couple of things in play here.

• Bullet's collision checking algorithm is non-deterministic
• Bullet relies on a settable threshold to stop the distance calculation.
• Bullet's collision checking algorithm with the current settings (compiled with double precision) is accurate to ~1mm
• FCL is thought to be a bit more accurate
• FCL used to be slower for distance calculations, but @Levi-Armstrong did some work a few months ago to fix that in Tesseract. Apparently there is an FCL PR to push the changes back, but it so far has not been accepted.

Action items

• Add a tolerance to the collision checking config. If distance is 5mm and the tolerance is 2mm, allow a distance of 3.1mm to be considered not in collision
• Add collision checking plugin to the collision checking config. That way, you can set it in these processes without modifying the default.
• See what the accuracy actually is. Move 2 boxes slowly towards each other with multiple checks per location and see when it is in collision. Is it a distribution or a specific distance?