Semantics of collision filter groups (custom URDF tag)

[SeanCurtis-TRI]

Collsion Filter Groups

Drake has introduced a custom tag into the URDF file: collision_filter_group. The tag takes the following form:

   <collision_filter_group name="this group">
      <member link="link1"/>
      ...
     <member link="linkN"/>
     <ignored_collision_filter_group collision_filter_group="other group1"/>
     ...
     <ignored_collision_filter_group collision_filter_group="other groupN"/>
   </colliion_filter_group>

This is a compact way of representing classes of links and the relationships between them. I.e., I can define two groups, assign large sets of links to them and say, nothing in one group collides with the other group. It can also be used to avoid self-collision in a set (by having a group ignore collisions with itself.)

Semantics

At first glance the semantics are clear and straight-forward. However, upon deeper reflection, there are subtle nuances that aren't obvious:

1. What is the scope of a collision filter group declaration? If a single URDF is parsed multiple times into a single RigidBodyTree, we will encounter the same names for collision filter groups but in the context of a different model instance. Should those names be considered global such that the corresponding parts of each model instance are in the same group? Or are they purely local and there would be two "parallel" sets of collision filter groups?
2. What is the lifespan of a collision filter group declaration? Should I be able to refer to a group by name, outside of the parsing code? (Based on the answer to the previous question, this might require a (name, model_instance_id) pair.
3. Should I be able to create collision filter groups outside of parsing? The ability to add bodies from arbitrary sources (e.g., ground plain or terrain) and then using a collision filter group to exclude most of the bodies in a robot from considering contact with the ground.

Generally, we could assume the most generous answers to the questions that would lead to the most flexible system, but that leads to engineering costs we would not want to pay if we don't have to.

So, this is a call to solicit thoughts, opinions, examples.

For the record, this is strongly related to #1793.

[liangfok]

The following is my humble opinion.

What is the scope of a collision filter group declaration?

+1 for containing it within a single model instance.

If a single URDF is parsed multiple times into a single RigidBodyTree, we will encounter the same names for collision filter groups but in the context of a different model instance. Should those names be considered global such that the corresponding parts of each model instance are in the same group?

No.

Or are they purely local and there would be two "parallel" sets of collision filter groups?

Yes, multiple "parallel" groups, some of which could then be merged via post-processing either programmatically or via another configuration file like one written in YAML.

What is the lifespan of a collision filter group declaration? Should I be able to refer to a group by name, outside of the parsing code? (Based on the answer to the previous question, this might require a (name, model_instance_id) pair.

The lifetime should be equal to the lifetime of the model instance.

Should I be able to create collision filter groups outside of parsing?

Yes, either programmatically or via another configuration file like YAML.

The ability to add bodies from arbitrary sources (e.g., ground plain or terrain) and then using a collision filter group to exclude most of the bodies in a robot from considering contact with the ground.

I think the above should be done either programmatically or via a separate config file like YAML.

[sherm1]

If every model has a name, and every model instance has a unique index, then we could use a pathname to uniquely identify every collision group, like atlas_14/left_leg. Maybe we should plan for that in MultibodyTree, @amcastro-tri ?

[SeanCurtis-TRI]

This is exactly why I started investigating this topic before GeometryWorld. It exposes design questions that we can consider going into MultiBodyTree and GeometryWorld. So, huzzah!

[siyuanfeng-tri]

I agree with Liang. Being able to modify the groups after parsing is useful. I am trying to add another conceptually similar abstraction for grouping bodies / joints through a YAML config file.

[amcastro-tri]

I am still at the basic functional stage of the MBT design. I do agree however that we should start discussing this topic early especially if that would have a certain impact on API and internal structure. Questions I personally have are:

• How should we represent model instances? simply as id's as RBT does?
• Would that be all we need or would it be useful to have a separate abstraction or bookkeeping class?
• What about composing several MultibodyPlant's into a single world? will that be allowed? imagine having several instances of the same robot in the same world and possibly interacting by collision?

[liangfok]

I think having a Model abstraction that encapsulates a set of bodies / joints would be useful. Note that in my mind a particular body or joint can be part of numerous Model abstractions, which is analogous to @siyuanfeng-tri's group alias concept in #4638.

[amcastro-tri]

@liangfok, what do you mean with "Model abstractions"? is that different from "model instance"?

[SeanCurtis-TRI]

How should we represent model instances? simply as id's as RBT does?

Does RBT currently do this? I certainly wouldn't want an "id" that is a literal position in a vector such that it would change as the composition of the tree changed. I'd want it to be a fixed value that has the same span as the body itself.

Would that be all we need or would it be useful to have a separate abstraction or bookkeeping class?

As long as we have some reliable, stable handle to bodies, I'd be content. Then any operations, aggregations, etc., on bodies could fundamentally operate on handles (which would need to be resolved to take any action on the underlying data.)

What about composing several MultibodyPlant's into a single world? will that be allowed? imagine having several instances of the same robot in the same world and possibly interacting by collision?

I'd assume this wouldn't be possible. Multiple MultiBodyPlant instances implies multiple MultiBodyTree instances. We'd have to provide a mechanism where all the underlying trees would be aggregated into a single system of equations to be solved. If that's the intent, why not simply populate a single MBT with all of the dynamic bodies?

[SeanCurtis-TRI]

And FTR, it seems that a discussion of how bodies are represented lies outside the scope of this issue. Perhaps create its own issue? Or redirect to a pre-existing issue?

[liangfok]

How should we represent model instances? simply as id's as RBT does?

Does RBT currently do this? I certainly wouldn't want an "id" that is a literal position in a vector such that it would change as the composition of the tree changed. I'd want it to be a fixed value that has the same span as the body itself.

Yes, RBT currently has a "model_instance_id" to represent model instances within the tree. See RigidBodyTree::add_model_instance(), which returns a unique ID. Note that there's currently no way to remove model instances from the tree.

[liangfok]

@liangfok, what do you mean with "Model abstractions"? is that different from "model instance"?

By "model abstraction", I basically mean a "view" into the RBT. It differs from "model instance" in that model abstractions may overlap each other.

[siyuanfeng-tri]

More to Liang's last comment, Imaging you have a humanoid robot, and someone (like me) are very interested in a subset of the joints. I'd like to have a group of leg joints [left_hip_x, left_hip_y, left_knee, left_ankle, right_hip_x, right_hip_y, right_knee, right_ankle], a group of knee joints [left_knee, right_knee], and more. I can then do special logic for different groups. These groups are all referring to the same model instance, with potentially overlapping elements in it.

[SeanCurtis-TRI]

Again...please redirect to another issue. What I really want to see is use cases to alluded by @patmarion.

[liangfok]

How about this?

1. I instantiate a RBT, which represents the world.
2. I add a ground to the world and add it to the "world" collision group.
3. I add a building to the world and add it to the "world" collision group.
4. I add a car to the world and define a collision group called "car".
5. I add an airplane to the world, and define a collision group called "airplane".
6. I add a human and define a collision group called "human".
7. After the simulation advances a few minutes, the human gets into the car. The human is now added to the "car" collision group (the human is also in the "human" collision group).
8. After the simulation advances a few more minutes, the human gets out of the car and is removed from the "car" collision group (the human is now only in the "human" collision group).
9. I add a hamburger to the world and add it to the "hamburger" collision group.
10. After the simulation advances a few minutes, the human grabs the hamburger. While the human maintains a rigid grasp of the hamburger, the "hamburger" collision group is merged into the "human" collision group.
11. After the simulation advanced a few minutes, the human eats the hamburger. The hamburger and its collision group are now removed from the simulation.
12. After the simulation advanced a few minutes, the human boards the airplane. The "human" and "airplane" collision groups are now merged.
13. The car's transmission is put into "park" and its emergency brake is activated. The "car" and "world" collision groups are now combined.

[sherm1]

Let's stick to "groups" for discussing arbitrary collections of things. "model abstraction" is too close to "model" and "model instance" IMO, which have well-defined narrow meanings.

What about composing several MultibodyPlant's into a single world? will that be allowed? imagine having several instances of the same robot in the same world and possibly interacting by collision?

No worries having multiple MBPlants in the same Diagram since they'll be able to share the same GeometryWorld. Gentle interaction is also fine, like robot1 senses robot2 and then reacts. But anything requiring the equivalent of direct feedthrough (simultaneous solution) won't work; for that the two robots would have to live in the same MBPlant (& MBTree). I think that restriction is unlikely to cause any big problems.

[sherm1]

How about this?

I don't understand the example. The purpose of a "collision group" is to define things that don't interact. A "car" group presumably would be there because we don't want components of the car bumping into one another. A human getting into a car needs to interact with the door and with the seat so why would they become part of the car group?

Making changes like that seems coupled with making other substantial changes in the System structure, such as adding and removing whole physical models. I would envision that as requiring Simulator detection of a "structural change event" during simulation, which then requires user code intervention to make suitable changes, including transferring still-meaningful parts of the state from the old to the new system. Redefining groups could be done at the same time. I don't think we need to define a separate group-editing ability at this point -- it doesn't seem necessary or sufficient.

[liangfok]

A human getting into a car needs to interact with the door and with the seat so why would they become part of the car group?

I am thinking about the time after the human is inside the car. Yes, I agree they should be part of different collision groups during the process of getting in the car.

[liangfok]

I don't think we need to define a separate group-editing ability at this point -- it doesn't seem necessary or sufficient.

Perhaps as a first pass we can omit the ability to change the world min-simulation. I think it'll be something nice to support eventually though. One use case is to randomly introduce and remove cars from a traffic simulation.

[SeanCurtis-TRI]

@liangfok An evocative example.

Like @sherm1, I recognize that there are huge obstacles above and beyond collision filters between now and running that scenario. However, I do appreciate the discussion as a sense of where we might be headed and making sure we don't paint ourselves into a corner.

If we put the "dynamic" nature of your example aside, there's still some more immediate ideas to discuss. In your particular example, it seems to feature only groups created outside of those defined inside a URDF. It's a very clean paradigm.

Do you also imagine wanting to do things that are more esoteric? Such as relating programmatically generated groups with parsed groups?

Example:

• Car.urdf defines several collision filter groups: wheels, exterior, and interior. Each of these groups ignore themselves (to preclude self-intersections testing) and may ignore each other to some extent.
• I load this car k times. I now have k wheels, exterior, and interior groups. Or, notationally, I have wheels_1, wheels_2, ..., wheels_k, etc.
• When I add a ground plane, I would like it to ignore groups: exterior_i, and interior_i (i.e., only perform collisions with the wheels...which would, of course, create interesting phenomena if the car flips upside down.) So, I add a group "ground", add the ground plane, and set it to ignore interior_i and exterior_i for all i which are model instance ids of loaded cars.

This illustrates that the parsed information (e.g., group names) should persist and should be distinguishable by model instance id (in both reference and construction.)

[patmarion]

use case 1: use filter groups to prevent collision between two groups of bodies on the same robot. The filter groups are defined in urdf. I can also programmatically add new groups, or access/modify the existing groups that were defined by the urdf and initialized by the parser.

use case 2: i can programmatically add bodies or load additional urdf files into the RBT, and programmatically create collision filter groups that include these new bodies and the prior existing bodies. for example, load two robot arms (i.e. load the same urdf twice), they each have their own filter groups as defined in urdf, and these are not shared (even though they have the same names because they come from the same urdf). Then I programmatically create groups between them.

use case 3: I want my code to be able to access collision filter group at runtime, and look up groups by name. I want to be able to programmatically inspect and add/modify these groups at runtime. since collision filter group names might not be unique, i may have to search within model_instance_ids to find the filter groups if I am doing a look up by name.

implementation detail, straw man: perhaps a collision filter group is an object (not a std::map or whatever mapping is currently used) and stores a list of body pointers. it has a user readable/settable name string, which doesn't have to be unique. Group names within a urdf are required to be unique, however. After adding a urdf to an RBT, I can get the list of collision filter group objects associated with that urdf (model_instance_id).

[SeanCurtis-TRI]

So, putting aside implementation discussion for a second, you're looking at the following functionality.

1) Load all collision filter groups (CFG) defined in URDF (referred to as URDF-CFG). 2) Create runtime CFG (programmatically) that can ignore other runtime CFG or URDF-CFG. 3) A single URDF with k URDF-CFG, read m times would end introducing km URDF-CFG in the set. 4) Given a URDF-CFG and runtime CFG have some unified "identifier". 5) Given a CFG identifier (of either type) get list of bodies which are members of the CFG. 6) Given a body, find out which CFG it belongs to which CFG it ignores. 7) Given a body, change which CFG it belongs to (add or subtract). 8) Given a CFG change what it ignores (add or subtract).

Everything in the list can now be thought of in two different contexts: during construction and during simulation. It might make sense to allow some of these to to be available in both contexts and some only in one.

Is that complete?

[patmarion]

Looks pretty good.

But, let's not even distinguish between CFG and URDF-CFG. There is only CFG. As a result of loading a URDF, one or more new CFGs may be created. If I just loaded foo.urdf, and that urdf defined a group named "bar", I want the parser to output some information so that my code can locate the CFG named "bar" that was constructed during the nth loading of foo.urdf. Code example:

modelId1 = rbt.loadUrdf("foo.urdf")
modelId2 = rbt.loadUrdf("foo.urdf")

cfg1 = rbt.findCFG(modelId1, "bar")
cfg2 = rbt.findCFG(modelId2, "bar")
cfg1.ignores(cfg2)

Also, for many of my use cases, I am querying for body collisions from inside a motion planner, i'm not actually using a simulator. So I am just interacting with the API of a single RBT object.

[patmarion]

In other words- the parser should do some bookkeeping so that I can find named CFGs initialized while loading the URDF. But the bookkeeping data need not be stored on/inside the CFG itself, it can be some mapping returned by the parser (probably stored on the RBT for convenience).

[SeanCurtis-TRI]

The distinction exists to acknowledge they come from different origins. Point 4 was supposed to communicate that from the outside, there is a common interface that hides that distinction come program manipulation (although, when acquiring a URDF-CFG, you would have to specify name and model instance id as you pointed out.)

Short point: it seems we're 100% on the same page.

[sherm1]

Probably not relevant for this discussion but I want to remind everyone that the basic unit of contact is "collision geometry", not "body". A body is a natural CFG of non-interacting collision geometry objects fixed to that body.

[liangfok]

Can we push the notion of a CFG "identifier" to the user and thus not have to maintain it inside of Drake proper? Drake can simply provide a CFG class that contains references to collision geometries that are part of the group. The user can then attach higher-level semantics like "model instance id" or "robot limb name" to the groups, and modify / destroy / create groups in a manner that the user finds convenient.

Let's also not forget about how collision filter groups can be supported from models specified by SDF Here's SDF's collision spec:

http://sdformat.org/spec?ver=1.6&elem=collision

It should be possible to automatically derive, at a coarse granularity, some collision filter groups based on kinematic analysis. It'll be kinda like image segmentation in computer vision. Has this possibility been considered? Is it already considered a "solved problem"?

[sherm1]

Here's SDF's collision spec

BTW, I see sdf's "collide_without_contact" and some bitmasks there. I know it also has some implicit groups (at least when used by Gazebo) -- by default all the collision geometry in a given model is in a self-ignore group. There is some way to override that but I'm not sure how it's done.

[SeanCurtis-TRI]

@liangfok I haven't thought through all the full implications of this CFG discussion and your statement, but my initial, gut reaction is that, no, we can't push it off on the user.

Several (pseudo-)random thoughts:

1. Drake providing and tracking CFG identifiers is what would allow us to blur the distinction between a CFG instantiated in a URDF file and associated with a particular model instance, and those instantiated programmatically.

2. The existence of the identifier in no way implies "semantics". Drake would have no idea why a group exists. Only where it came from and its state. Things like "model instance id" and "robot limb name" are not part of the semantics. They are the most direct way to successfully identify a particular CFG which was generated from parsing a URDF. However, using those two pieces of information would provide a drake-defined generic CFG id.

3. CFG have strong implications on the underlying collision engine. Conceptual changes on the user's part must lead to triggering of multiple mechanisms under the hood. One example is that, as @sherm1 pointed out, putting two bodies in mutually exclusive CFGs is, in practice, defining relationships between collision elements in the collision engine. In the case of bullet, this requires invalidating cached structures and rebuilding. So, when it comes to allowing user to "modify / destroy/ create groups ina manner that the user finds convenient", there must be constraints in place so that the system's underlying implementation has a chance to reflect those desires.

4. Finally, CFG (as presented implicitly defined in URDF) are body-centric concepts. They are an abstraction convenience so that the user doesn't have to think about what the collision representation of a body may be. I think there is real value in that abstraction (allowing us to hide collision implementation details and simplifying the API). So, for the sake of pedantic syntax, I'd propose that CFGs be discussed only in terms of bodies.

[liangfok]

Alright, I think we're on the same page -- the value of the CFG's identifier doesn't convey any additional semantics that are useful to CollisionWorld. It is useful purely to the user of CollisionWorld. Correct?

Regarding:

So, for the sake of pedantic syntax, I'd propose that CFGs be discussed only in terms of bodies.

I assume you're referring to informal discussion and not actual implementation right? Isn't is true that a body could have a super-complex collision specification consisting of thousands of collision geometries? In that context, wouldn't it be best to only include a small portion of the body's collision geometries in a CFG (i.e., the part that is closest to a mobile robot)? (I'm assuming all geometries inside a CFG need to be checked against all geometries in another CFG.)

[SeanCurtis-TRI]

As for CFGs discussed in terms of bodies, I meant in actual implementation. The primary motivation for that is that it is how they are defined in the URDF.

It is true that a body can be represented in collision with a union of arbitrary collision elements. ANd this is where I no longer quite follow your point.

I'm assuming all geometries inside a CFG need to be checked against all geometries in another CFG.

This seems to miss the point of CFG (aka a collision filter group). These filters only come up in collision detection when the question is asked: "hey, I have a pair of collision elements that seems to be a viable pair for doing a detailed collision test, should I do that?" That's when the CFG kicks in. If either element belongs to a group that ignores a group the other element belongs to (with the understanding that collision elements "inherit" their parent body's CFG semantics), then the detailed test will be skipped. The assumption here is that you've already performed spatial culling to determine if there are viable candidate pairs, and the existence of filter functionality provides the additional semantics that spatial overlap is an insufficient criteria for a pair to be meaningful.

That said, is possible, let me offer an alternative use case:

Currently, when we have two adjacent links, we dismiss possible collision between them. This is primarily because we assume that the collision geometry is an imperfect representation of the actual objects such that apparent collisions would not represent physically meaningful collisions. But imagine that one of those links is a complex object that has been decomposed into a union of collision elements. It is possible, in this case, for us to know that only one element of the union would provide the meaningless collisions mentioned above. Furthermore, if the two adjacent links are non-convex and there are valid configurations which are not constrained by joint limits but would be constrained by contact, it would be useful to define filters with finer granularity than body level. (i.e., disallow contact detection on those elements which lie "at the joint", and allow collision between elements "away from the joint." This type of logic is not supported -- not for any representation of collision filtering that we have.

[sherm1]

This type of logic is not supported -- not for any representation of collision filtering that we have.

Should be though IMO. Certainly "world" is an example of a body with lots of independent collision geometry. Are you saying we won't have collision geometry as the underlying unit of collision? That seems odd.

[SeanCurtis-TRI]

Not at all. Two things:

1) This isn't about whether collision elements are visible to the programmer. The intent of this issue was to strictly discuss one facet of collision: collision filter groups. It doesn't even span the whole subject of collision filtering. 2) Furthermore, I just made a statement of fact. We currently have no interfaces for picking and choosing filters programmatically in any meaningful way. I was not advocating that we shouldn't do that. Perhaps the missing word is "currently", as in, "This type of logic is currently not supported..."

Finally, what I would like to achieve (and we've discussed this in the past), is to mask how collision filtering is achieved and, instead, expose a straight forward API where I can specify things I don't want/want to collide, e.g.,

• don't collide this collision element with anything in this sub-tree.
• Don't allow this sub-tree to self-collide.
• Despite current state, allow these two elements to collide.

For me, the key is to focus on the abstraction appropriate to the problem. I.e., people shouldn't have to focus on collision elements if all they care about is the body level, or even the model (aka subtree) level.

[SeanCurtis-TRI]

After doing some tinkering with the matlab code, I've concluded two things:

1. Matlab precludes adding a URDF with collision filter groups multiple times. The attempt throws an error with the following message: A collision filter group with the collision_fg_name self_group already exists in this RigidBodyManipulator
2. The only functionality matlab offers is the ability to parse and make use of the collision filter groups in URDF files. None of the features listed above dealing with post-parsing, programmatic manipulation are currently supported in the matlab interface.

Can anyone confirm the second point?

[patmarion]

That sounds right. I have not used the matlab api to programmatically inspect or modify collision filter groups. The only programmatic work I've done was that c++ code I linked to you, which required using bit masks to define groups.

[SeanCurtis-TRI]

Dynamics team status - what it will take to close this:

• PR #4773 addresses bringing collision filter group parsing into C++. They are parsed and used in collision computations, but that is all. There is no support for programmatic manipulation (in fact, in some ways it is reduced from the matlab functionality).

• Programmatic manipulation will be deferred to GeometryWorld (#4781).

[jwnimmer-tri]

For the record, today in TRI we found need of having drake:collision_filter_group also exist in the SDF parser (not just URDF). The attic SDF parser had this feature.

[SeanCurtis-TRI]

How much is this mucking up your productivity? (I.e., how much of a priority is it?)

[jwnimmer-tri]

It's @ggould-tri working on it. Last I heard, it was possible to work around this by adding C++ code after parsing to set up additional filtering.

[SeanCurtis-TRI]

Indeed; I noted the slack where he'd declared he'd found the "get collision geometries for body" functionality.

[ggould-tri]

Yes, we were able to do it post-parsing via:

scene_graph->ExcludeCollisionsBetween(
  plant.CollectRegisteredGeometries(plant.GetBodiesWeldedTo(plant.GetBodyByName(...))),
  plant.CollectRegisteredGeometries(plant.GetBodiesWeldedTo(plant.GetBodyByName(...)))
)

IMO this is annoying but not serious. I might disagree if I weren't a regular MBP user, though. Any call that puts SG, MBP, and the words "Collect" and "Register" within a line of each other is fairly unfriendly.

[SeanCurtis-TRI]

Your life would presumably have been simpler if it had been specified in the SDF?

[ggould-tri]

I think so, yes -- I could have xacro-constructed it fairly easily. Although possibly the "Collect"/"GetBodiesWelded" there is an indication that we would want to filter whole subtrees of the scenario rather than individual bodies when there is complex geometric modeling going on.

[SeanCurtis-TRI]

This issue has two topics:

1. Abstract discussion of filter group semantics.
2. Request that the functionality in URDFs be available in SDFs. I've created the issue #14124 to capture that particular feature.

As such, I'm closing this in favor of the new issue.