Fix fcl implementation so the fcl::CollisionObject's are not created every time a request is made.#295
Conversation
Conflicts: collision_detection_fcl/src/collision_robot_fcl.cpp
|
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 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 |
| // Need to store the FCL object so the AABB does not get recreated every time. | ||
| // Every time this object is created, g->computeLocalAABB() is called which is | ||
| // very expensive and should only be calculated once. To update the AABB, use the | ||
| // collObj->setTsetTransform and then call collObj->computeAABB() to transform the AABB. |
There was a problem hiding this comment.
Misspelling setTsetTransform?
There was a problem hiding this comment.
Sorry, I have fixed the misspelling, but I have not pushed the changes yet. I was waiting to see if there were any addition modification required.
|
This is a great find! I will begin to test this on my end. FYI the following link is not viewable for me: |
The link has been fixed. |
|
I just ran this using the default Hilgendorf (two UR5's and a base stand) with a simple wall obstacle and floor obstacle around it. I randomly sampled 10,000 joint configurations and asked MoveIt! if the state was collision free. I unfortunately only saw a 12.6% improvement in the speed of this task: Before this commit: 21.0 sec Is my significantly test different than yours? Note I'm compiled in debug mode and using FCL through OMPL.
|
|
The reason may be that the performance is driven by the number of triangles in the urdf collision geometries so the triangle count may be significantly lower and 10,000. You may try adding an object with around 500,000 triangles and you should see a big difference. |
|
@davetcoleman I would also add that:
|
|
@davetcoleman, I made the requested changes. I can also fixup this change so there is only one commit if you like. |
|
Since two of you are in agreement on the PR and since I have been running it successfully for a week I'll give it the green light, thanks! Do you by chance have a re-usable benchmark setup for collision checking that we should save for future use? Not sure where the best place to put it would be but seems like a useful tool to have. Also, do you mind sharing a summary of this great improvement on the mailing list to help us drum up support for MoveIt! development? |
There will be one available today or tomorrow as part of the industrial_moveit repository. This is where we have been make enhancements to moveit so we have a benchmarking package. |
|
@davetcoleman Should I create a pull request against jade-devel or will you cherry pick it? |
|
I would welcome you to create a quick PR, I'm actively working on Docker images to make it easier for me to do things like cherry-pick |
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.