Refactoring collision detection using Geometry primitives (such as capsule)

[YoavFekete]

Today Collision detection in moveit is still based on user defined mesh's.
Even with FCL and all the mesh optimization it is still very expansive computationally to check collision .
A simple solution exists. Use primitives' collision objects instead of meshs.
The combination of capsules and obb and spheres yields very fast computation time and very good approximation,
This can potential yeild few order of magnitude in run time.
(i was working in a company were we developed our own collision detection system, and their we did this move, the results were more then 100 time fold for a single quarry (for a large cell).
Further more it is quit easy to automatically generate a primitive geo representation from the mesh , their are vey stable alg from gaming industry for that, and

The challenge would be to implement the capsule obb collision tests as in the current collision detection libs such as Bullet and FCL use very inefficient algorithms. I have recently wrote a new alg for that that might be much faster, But also the existing "slow" methods are much faster then mesh methods.

More for the future:

Improve the moveit setup assist to :

1. generate the primitive collision object representation, and save that in a config file, validate the result
2. while testing for the self collision matrix , use the data to generate a calculate the
   probability of a to collision objects in the cell to collides, with this a collision collision (order of checking the collision pairs) can be generated. changing the order of checking the individual quires, dosent change the alg correctness, but it will change the avg run time on colliding states , as pairs that have heiger probability to collide will be evaluated first.

Multiple collision queries:
Write a version that checks the collision state of more then one state at onces,
this open an entire world of optimizations, that can yield much faster run time then a signale query (on average)

GPU implementation:
with the collision system is based only on OBB (and aabb), capsule and sphere ,
its fairly easy to implement the collison test for that on GPU (as an option)
Today open source very fast code for capsule capsule collision detection exists for GPU,
they are uncomparble to the run time on CPU, so extending it to the other needed collision primitives should be feasible, and not to hard, test wise as we have the old mesh implementation, and the CPU new one it will be easy to unit test the GPU version.

Continues Qury:
given the next state and current state check that no collision is happening while moving from one to the other.
under the assumption of a linear move between states (which works while the time difference between the states is small )

[davetcoleman]

I agree supporting OBB Capsule collision detection! The first blocker is URDFdom, and there are a bunch of PRs aiming to address this but for version control reasons they have not been merged yet.

@YoavFekete if we can get those changes to URDF it seems to me that would unblock you to make PRs to MoveIt, yes?

[v4hn]

Better support for generating primitive representation from meshes would be appreciated, with or without capsules. Even for sphere approximations I'm not aware of good tools in our ecosystem beyond bounding spheres in geometric_shapes. Nonetheless, the fact that URDFdom was set in stone a very long time ago and many useful representations were not merged in "because we can't change" hampers great potential contributions. If upstream maintains the same arguments in urdfdom for ROS2, it definitely has to be forked for MoveIt and others (esp. tesseract) to progress.

[YoavFekete]

Yes if we could get this changes to URDF, it would be very natural add the improvements i suggest.
On my part i ma currently comparing my new Capsule AABB collision function with that of FCL,
results seem promising, currently working on the unit tests.
But soon i will be kind of blocked if the change does not propagate to the URDF.