Jmeyer1292
commented
8 years ago This change allows us to actually take advantage of FCL's broad phase collision detection. We no longer pay a significant price for complex models that are not near collision during runtime. Profiling of our applications has shown many spend 99% of their time in collision checking. I encourage you to run any of your moveit applications that may take into account collisions under something like valgrind --tool=callgrind and see for yourself.
It would be great if the community at large could inspect this change, test it out, and lend their brains to the implications to related factors such as thread-safety.
I'm tagging maintainers: @isucan @mikeferguson @acornacorn and frequent contributor: @davetcoleman This may be relevant to your work as well.
davetcoleman
Levi-Armstrong
Every time a collision or distance request is made all of the fcl::CollisionObjects are recreated shown here. During the creation of the fcl::CollisionObject it computes the AABB of the passed geometry as shown here. This calculation is very expensive because it has to walk the mesh twice as shown here. The calculation of the geometries AABB is only required once and then it can easily be update by providing the TF using CollisionObject::setTransform followed by calling the CollisionObject::updateAABB which simply re-sizes the AABB based on the transform as shown here. It is also seen here that I am creating a copy of the cached fcl::CollisionObject which is to maintain thread safe.
I performed some baseline testing. The testing was performed using a joint interpolated planner and performing 100 planning requests using the same start and goal then taking the average of the time it took to calculate the trajectory where each performed a 100 collision checks. I performed this using both methods where I changes the meshing of a single object in the urdf to where the total triangle count was 10K, 500K and 10,000K. The performance improvements are shown below and they are significant.