Drake
Collaboration diagram for Collision Filter Groups:

The Principle

A collision filter group represents collision filtering by defining a collection of collision elements. That group then defines "ignore" relationships with other groups. A collision element's membership in a group does not guarantee inclusion in any filtered collision pairs. This group must be included in at least one group's ignore list; inclusion in its own ignore list would be sufficient.)

A collision-filter-group-based filtering system's unique benefit arises from thinking about classes of collision elements. It serves as a short hand for communicating that no collision element in one class can collide with elements belonging to another class. Each unique collision filter group defines such a class.

This can be used to indicate that two different models cannot collide. Simply create two collision filter groups, assigning all of the collision elements of one model to the first group, and all elements of the second model to the second group. Set either group to ignore the other (or both, redundancy doesn't hurt).

Declaring Collision Filter Groups

Declaring collision filter groups in URDF/SDF files

Collision filter groups can be instantiated by specifying them in URDF/SDF files.

Declaration

1 <collision_filter_group name="group1">
2  <member link="body1"/>
3  <member link="body2"/>
4  <ignored_collision_filter_group collision_filter_group="group2"/>
5  <ignored_collision_filter_group collision_filter_group="group3"/>
6 </collision_filter_group>

This XML-snippet illustrates the syntax for declaring a collision filter group. It declares a collision filter group named group1. It has two members, body1 and body2 (although it could have any number of links). This is short-hand for communicating that the collision elements of body1 and body2 belong to group1. Furthermore, group1 ignores two groups: group2 and group3. It is not considered an error to ignore a non-existing group, but it is considered an error to reference a link that hasn't been defined.

One possible use of collision filter groups is to create a set of collision elements which cannot collide with each other (i.e., no self-collisions in the group). This is achieved by having the group ignore itself. E.g.,

1 <collision_filter_group name="no_self_collision_group">
2  <member link="body1"/>
3  <member link="body2"/>
4  <ignored_collision_filter_group collision_filter_group="no_self_collision_group"/>
5 </collision_filter_group>

Urdf files support the definition of a single robot. As such, all <collision_filter_group> (<cfg> for brevity) tags are children of the <robot> tag.

Sdf files support the definition of multiple robots (aka "models"). As such, the <cfg> tags are children of each <model> tag. The <cfg> tags can only include links that are defined in that model in its membership lists. The implication of that is collision filter groups defined in sdf files contain bodies from a single model only. However, a collision filter group in one model can ignore a collision filter group in another model. See multi_model_groups.sdf for an example.

Declaring collision filter groups in code

In addition to parsing the collision filter groups from the urdf/sdf files, there is an API on the RigidBodyTree that allows manipulation. However, the API is constrained. The workflow is as follows:

The API only supports building up collision filter groups and adding bodies to groups. There is no API for removing bodies from previously declared groups or removing groups from another group's ignore list. The API is strictly additive.

Furthermore, once declared collision filter groups have been compiled (via invocation of RigidBodyTree::compile()) the names can no longer be referenced. This is what allows the same urdf/sdf file to be read multiple times but not have the named collision filter groups in the file end up spanning all of the models.

However, Drake does support some post-compile filtering modifications. These include:

In typical use, parsing a model from a file calls RigidBodyTree::compile() and any named collision filter groups in that file cannot be subsequently referenced. The parsing API provides a mechanism for suppressing the automatic tree compilation. There is a parallel set of parse functions which take an additional bool, indicating if the tree should be compiled (true) or not upon parse completion. By passing false, the parsing process will not compile the tree, but it becomes the caller's responsibility to make sure that RigidBodyTree::compile() is invoked before doing any work on the tree. For example:

const bool do_compile = false;
const std::string file_name = "some_file.urdf";
AddModelInstanceFromUrdfFileToWorld(file_name, kRollPitchYaw, do_compile, &tree);
tree.DefineCollisionFilterGroup("new_group"); // doesn't already exist.
tree.AddCollisionFilterGroupMember("new_group", "body1", 0); // Assumes known body name and model instance id.
tree.AddCollisionFilterIgnoreTarget("new_group", "group_from_urdf");
tree.compile();

In-code representation

Collision filter groups are implemented as a pair of fixed-width bitmasks, stored with each collision element. Each collision filter group corresponds to a single bit. One bitmask represents the set of groups a collision element belongs to. The second represents the groups that this collision element ignores. When a pair of collision elements are considered for collision, the membership mask of each is ANDed with the ignore mask of the other. If either operations produces a non-zero result, the pair is filtered. The cost of the comparison is constant, but related to the bitmask width.

The width is defined at compile time and is defined by drake::multibody::collision::kMaxNumCollisionFilterGroups. The fixed width is a performance optimization but it means that there is a finite number of collision filter groups available in the system. Each RigidBodyTree instance has its own space of collision filter group identifiers.

By default, all elements belong to a common, universal group which corresponds to bit zero. A collision element can be rendered "invisible" by assigning it to a group that ignores this universal group.

Next topic: collision_filter_mapping