Is your feature request related to a problem? Please describe.
Drake's proximity engine maintains two separate FCL trees for collision geometries: static and dynamic. Currently, the only geometries which end up in the static tree are those which are attached directly to the world frame. This is a subset of the geometries for which plant.IsAnchored(geom.parent) == true. Anchored geometries are also effectively fixed in the world frame, so they are good candidates for being added to the static tree.
For scenes with many static models which are anchored but not directly attached to the world body (as is the case if those static models are loaded via the Parser from URDF/SDF files), this leads to a large dynamic tree, and therefore spurious anchored<->anchored collision checks. Even though these anchored<->anchored collision pairs can be filtered via the collision filtering API, that does not eliminate them from the broadphase which still takes a significant amount of time.
For particularly large static scenes (thousands of anchored geoms), we have noticed a 10x simulation performance improvement by relocating the anchored geometries from separate bodies to be attached directly to the world body.
Describe the solution you'd like
Geometries which are anchored per the MbP definition land in the static tree for collision checking, in order to improve simulation performance for scenes with many anchored geometries.
Describe alternatives you've considered
At the moment, we are experimenting with a post-processing step before plant finalization which relocates anchored geometries up into the world body. This is not ideal because it involves loading all of the geometries a second time. It's also unclear leaving the old (now empty) geometries lying around the scene graph is a good idea.
Removing the anchored<->anchored collision pairs via collision filters also does not improve the performance significantly (see above).
Additional context
As noted by @SeanCurtis-TRI , the SceneGraph cannot trivially query plant.IsAnchored, so it will need to be informed of the body/geometry tree structure in some other way.
Is your feature request related to a problem? Please describe.
Drake's proximity engine maintains two separate FCL trees for collision geometries: static and dynamic. Currently, the only geometries which end up in the static tree are those which are attached directly to the world frame. This is a subset of the geometries for which
plant.IsAnchored(geom.parent) == true. Anchored geometries are also effectively fixed in the world frame, so they are good candidates for being added to the static tree.For scenes with many static models which are anchored but not directly attached to the world body (as is the case if those static models are loaded via the
Parserfrom URDF/SDF files), this leads to a large dynamic tree, and therefore spurious anchored<->anchored collision checks. Even though these anchored<->anchored collision pairs can be filtered via the collision filtering API, that does not eliminate them from the broadphase which still takes a significant amount of time.For particularly large static scenes (thousands of anchored geoms), we have noticed a 10x simulation performance improvement by relocating the anchored geometries from separate bodies to be attached directly to the world body.
Describe the solution you'd like
Geometries which are anchored per the MbP definition land in the static tree for collision checking, in order to improve simulation performance for scenes with many anchored geometries.
Describe alternatives you've considered
At the moment, we are experimenting with a post-processing step before plant finalization which relocates anchored geometries up into the world body. This is not ideal because it involves loading all of the geometries a second time. It's also unclear leaving the old (now empty) geometries lying around the scene graph is a good idea.
Removing the anchored<->anchored collision pairs via collision filters also does not improve the performance significantly (see above).
Additional context
As noted by @SeanCurtis-TRI , the
SceneGraphcannot trivially queryplant.IsAnchored, so it will need to be informed of the body/geometry tree structure in some other way.