Defining collision filter groups across model instances

[5yler]

I while ago I posted on Stack Overflow inquiring about the possibility of defining collision filter groups across model instances. It seems this functionality is currently lacking.

In the multibody URDF parser, loading collision filter groups is restricted to the those present in the same model instance.

What I would like to do is in URDF A specify:

  <drake:collision_filter_group name="model_a_group">
    <drake:member link="link1"/>
    <drake:member link="link2"/>
    <drake:ignored_collision_filter_group name="model_b_group"/>
  </drake:collision_filter_group>

and in URDF B specify:

  <drake:collision_filter_group name="model_b_group">
    <drake:member link="link4"/>
    <drake:member link="link5"/>
    <drake:ignored_collision_filter_group name="model_a_group"/>
  </drake:collision_filter_group>

But currently attempting to do this results in the following error:

abort: Failure at multibody/parsing/detail_urdf_parser.cc:238 in abort: 
Failure at multibody/parsing/detail_urdf_parser.cc:238 in ParseCollisionFilterGroup(): 
condition 'collision_filter_group_b != collision_filter_groups.end()' failed.

Would it be possible to add an optional ignored collision filter group type, for example:

    <drake:optional_ignored_collision_filter_group name="group_that_maybe_exists"/>

which would not throw such an error?

A specific example use case if helpful:

I'm using a multibody system with a 7DOF Franka Panda arm whose base geometry is overlapping with a 2DOF robot. I want to specify that the base of the Franka and the base of the 2DOF arm are part of the same collision filter group despite being part of different URDF files.

[SeanCurtis-TRI]

This is very helpful, thank you. Here are a random collection of thoughts as I consider this kind of feature.

1. It seems to me that the parsing should, by default, not persist collision filter groups across multiple files. It seems to be a dangerous trap otherwise. Instead, the parser should take some kind of configuration that explicitly requests that kind of persistence and cross file association
2. Could/should there be some sort of scope included when referencing named collision filter groups so that it's clear that a when referencing collision filter groups by name, it's clear whether that group is expected to be found inside the file vs outside the file? If so, what would that look like?
3. If persisting collision filter groups, if two different files create identically-named "local" collision filter groups should those be considered distinct groups? Or a single group consisting of the union of members? Should it be explicitly configured in the XML?
   • If a single group, should collision filters declared w.r.t. that group in previous files be extended implicitly to the new members?
   • If distinct groups, how do are they made distinct and how would other collision filter groups successfully reference one or the other? This may be related to item number 2 above: scoped group names.

On the whole, I lean towards, as @sherm1 calls it, "the principle of least astonishment" and, for me, that means relationships between things should be explicit with the minimum of hidden machinery.

[sherm1]

My 2 cents: I'd like to see us work this out first in terms of objects known to Drake rather than files. Urdf and sdf files are just a convenient way to define Drake ModelInstances, which combined define a Drake System. I think what we're looking for is a way to define collision filters that span multiple ModelInstances.

Consider a System comprised of two identical Panda arms defined by reading panda.urdf twice into instances panda1 and panda2. Both will define a collision filter groups of the same names. However, there is no reason to think those should be combined -- they are likely to have been defined to avoid self-collisions. In the case where they should be combined there would need to be a higher-level specification that says panda1/filter_group_a and panda2/filter_group_a should be combined.

An alternative (or addition) might be to define a distinct kind of collision filter group, say a "global collision filter", that automatically combines with any other global collision filter of the same name in the same Drake System. That would allow urdfs to be combinable without a higher-level system specification. Those couldn't be scoped since no individual urdf can know the names of the model instances that will be created; they would just be globals like externs in C.

[SeanCurtis-TRI]

@sherm1 If you're willing to do post-parsing coding, you already have the API you need to declare arbitrary collision filters between any bodies, regardless of their provenance or model instance affiliation. At the end of the day, SceneGraph doesn't care about MultibodyPlant ModelInstances, just geometries. So, those are "objects known to Drake" and the problem is "solved" in that sense. Furthermore, the declaration of collision filter groups in URDF/SDF are Drake-specific features, so, in some sense, they are also "object known to Drake". :) As I understand this issue, "just [the] convenient way" is really the crux of the issue.

What the current API doesn't have is the ability to articulate collision filter groups based on what was stated in URDF/SDF files. (The API will allow you to determine if collision between two geometries is filtered, but gives absolutely zero explanation for why.) Named collision filter groups in urdf and sdf only exist during the parsing of a specific file. The lifespan of such a group is limited to the time of the function invocation for that particular file, and the scope of impact they have is limited to the bodies introduced by that file in that parsing invocation. (I.e., parsing the same file twice will not cause cross-model instance collision filtering). So, to use the current API to achieve the affect in the OP, the C++ code would have to have a lot of hard-coded knowledge about the contents of the URDF/SDF files being parsed. Persisting those aspects that are currently thrown out would help, but would be incredibly brittle depending on what information is actually given in the specification files.

So, we have a few options:

1. Allow the declared collision filters to persist after parsing so that users can reference them and "act" on them (although I'd have to think hard about what the appropriate space of actions are). This would be a "objects known to Drake"-based solution that would essentially be sugar over what Drake's API currently allows.
2. Define XML semantics to do the same (along the lines of what you proposed with a `"global collision filter" or some kind of "scope_name/collision_filter_group" name) where the users efforts are limited to authoring specification files and setting flags at parse time.
3. Some third option that isn't occurring to me right now, but is, nevertheless, vastly superior to everything listed.

[sherm1]

Got it, thanks. That third option sounds great! Meanwhile, maybe the "global collision filter" would be sufficient? Though it would have to persist post-parsing so the "linker" could connect the geometries.

[SeanCurtis-TRI]

Can you elaborate on the concept of the "global collision filter"?

1. Is it "the" global filter? (i.e., unique and inclusive)?
2. Or can an arbitrary collision filter be declared to be global and if two urdfs independently declare a collision filter group with the same name to be global, they are treated the same? The advantage of this is we could add an attribute to the current syntax which would be an "opt in", thus the legacy behavior wouldn't be changed at all.
3. What if I parse the same URDF multiple times and I want them all to not collide with the table so I include some reference to a "global" collision filter group, do they also, now, implicitly not collide with each other? What if we don't want that kind of transitivity?

Further thoughts?

[sherm1]

Your point 2 above is what I was thinking of. We would invent a way to mark a collision group name "global" (or maybe "shared" is better?). Something like:

  <drake:global_collision_filter_group name="my_global_group">
     ... specification of affected local geometries ...
  </drake:global_collision_filter_group>

These groups would have to persist post-parsing so that they can be combined once all instances of a global group have been seen.

Regarding transitivity, I think we don't want it. In the example you gave the urdf defining the table would provide a global group for the table geometry ("table_geometry_global"). Then each robot urdf would create (local_geometry, table_geometry_global) filters that would resolve into local_geometry filtered against each of the geometries contained in table_geometry_global group. (I'm fuzzy on how that would be specified.) But that would not have the effect of filtering out any other geometries.

[SeanCurtis-TRI]

These groups would have to persist post-parsing...

In my mind, they would exist as a hidden data of the Parser instance, so that it can coordinate parsing multiple files. The data would not be part of any public API. Essentially, the parser would track all of the information as things get parsed, building the state as it goes, so, at any time you stop parsing, you've got the most complete state given the information given, but you don't have to do anything after parsing.

Regarding transitivity...

We certainly don't want accidental transitivity. :) In your example, I recognize the key is the table declares a group and the arm declares a group. The arm also declares the filter -- i.e., itself with the table's group. The key here is that each time we parse the arm, we're creating a new group that "happens" to share a name with a previously parsed group -- they don't all dump into the same group. While referencing collision filter groups in other files for the purpose of declaring filtering would be perfectly valid, the contents of any single group can only consist of bodies defined in the same "instance" of the file as the group is created.

Further thoughts

Possible tags:

I can conceive of a pair of tags that work together:

• One tag to declare that this collision filter group is visible outside the file.
• Another tag to declare that I'm declaring a group I want to filter collisions with, I expect/demand that it exist outside of this file.
• So, to coordinate across files, one filter must explicitly declare itself to be available, and the other must explicitly declare that it's looking somewhere else, but that's it. This may be overkill, but it declares intent loudly and clearly, but limits flexibility.

Name collisions

When declaring a filtered relationship between two groups, it should always be a group declared in the current file with a group declared either in the same file or another file. What if the name appears multiple times (see the arm example above). What should we do?

• Demand uniqueness and throw.
• Have some priority mechanism that can include scope and first-declared-gets-preference kind of thing.
• Accept all such named groups?
• What about the case where I declare a filtered relationship with a group that doesn't exist yet? Do I filter against the first one I find after the initial parsed declaration? Everyone I find?

Arbitrary order

It would be important to make sure that we don't require strict dependencies on the order in which things are defined. I.e., you have to parse the files in just the right order, or things blow up. If you've got circular collision filtering, that'd be impossible. And, even in the context of a single file, we don't require that. Fortunately, the idea of persisting these declarations beyond the lifespan of the file is perfectly consistent with this property, so no worries there.

[sherm1]

Regarding name collisions: I think we would want to allow any urdf to declare a global group containing some local geometry. If a same-named global group appears in another urdf (or by multiple readings of the same urdf) then the all the mentioned geometries should be mutually filtered. (I'm not sure if that addresses your questions above.)

[SeanCurtis-TRI]

@syler Much of this discussion has spiraled into technical nitty gritty, but we've lost sight of the origin and I'd like to revisit your specific problem.

Looking at the original post, you've outlined some tentative syntax and the idea that you want the geometries of the "base of the Franka and the base of the 2DOF arm" to be filtered for collisions. It'll be good to step away from hypothetical implementation and return application. Could you elaborate on your application?

1. What are the two robots? There's a lot going on in the picture, and we're not entirely sure what we're seeing there.
2. How do the two robots relate in the MBP? Are they both floating joints connected to the world? Are they otherwise connected?
3. Why do you need those geometries excluded from collision consideration?

A greater insight in your application/workflow will help inform our efforts to make sure the motivating problem is actually getting efficiently addressed. Thanks.

[5yler]

Here's a pic with a little less clutter, hope it makes things a little clearer. We use a 2DOF arm to hold a bowl that the Franka arm serves food into. You can check out video of it in action here - see Meet Alfred 2.0, With Amazing New Features!

When planning, we don't want to smack the center assembly of the robot either with the 2DOF arm/bowl or the Franka.

Snippet of how we're constructing our system model:

  // created a plant that will later receive the two robots.
  robots_plant_ = builder_.AddSystem<MultibodyPlant>(time_step);

  momap::log()->debug("SM:Build: Creating robot model parser with {} entities.",
                      robot_entities.size());

  for (const auto& entity : robot_entities) {
      mb_entity_map_.emplace(entity->name, entity);
      // Parse the robot's urdf
      const auto robot_model_index {
          Parser(robots_plant_, scene_graph_)
              .AddModelFromFile(entity->urdf, entity->name)};
      model_index_map_.emplace(entity->name, std::move(robot_model_index));
      log()->trace("SysModel:Build: added robot: {}", entity->name);
  }

  for (const auto& entity : robot_entities) {
      // get child frame of urdf that is used as root
      auto& child_frame {robots_plant_->GetFrameByName(
          entity->world_frame, model_index_map_.at(entity->name))};
      robots_plant_->WeldFrames(robots_plant_->world_frame(), child_frame,
                                entity->X_WorldToEntity);
  }
  // Now the model is complete.
  robots_plant_->Finalize();

The reason we want the collisions to be excluded is there is overlapping collision geo in the cylindrical base part of both the 2DOF arm model and the Franka model. We've had other use cases where the same functionality was desired, like for large static parts of our workspace with partially overlapping collision geometry from different system models.

[5yler]

I like @sherm1 's suggestion of a global/shared collision filter group definition, it would address all of our use cases

[sherm1]

Thanks for the explanation! A follow-up Q: from the picture it looks as though collision forces between the base geometries wouldn't affect motion at all (since both parts are immobile). Is that right? If so, is the goal of filtering to improve performance or is there another reason?

[5yler]

The primary reason for filtering is to avoid states from incorrectly getting flagged as invalid/in collision during planning. If the Franka and ancillary arm base are not part of the same collision filter group, all states get flagged as in collision.

[sherm1]

@5yler can you clarify "flagged as in collision during planning"? How is that happening? Are you making a particular query call to SceneGraph and then making a determination about state validity? Is there a temporary workaround where you can exclude unwanted collision pairs in code?

[jwnimmer-tri]

Are you making a particular query call to SceneGraph and then making a determination about state validity?

More than likely, this is referring to what ComputePointPairPenetration or ComputeSignedDistancePairwiseClosestPoints is returning. A non-empty result for penetrations, or less-than-threshold result for signed distance, indicates a self-collision. The goal is to easily declare some of those to be filtered out, using model data (SDFormat, URDF, model directives, or the like) instead of API calls onto SceneGraph.

I'll be looking into this ticket a bit this week. I think having a single SDFormat file that uses <include> for the two robot URDFs (as a single model instance), but then declares the "global" collisions in the SDFormat file, would work. (Possibly https://github.com/RobotLocomotion/drake/issues/15950 will get in the way?)

We might also be able to extend the model directives syntax to offer the same effect via yaml (instead of xml).

[rpoyner-tri]

I'm working on this with @calderpg-tri . We've had some preliminary discussions. I'm starting to draft those ideas into a development branch for further discussion. That branch is here: https://github.com/rpoyner-tri/drake/tree/multi-cfg-dev

Most of the work is on parsing, rather than anything deeper, so far. FYI @calderpg-tri @jwnimmer-tri @sammy-tri

[EricCousineau-TRI]

In VC w/ @rpoyner-tri and @calderpg-tri, we discussed how scoped names for collision groups may affect SDFormat composition-based parsing. Rico mentioned that his current branch will look something like this (I think):

Parser::AddModelFromFile(...) {
  CollisionFilterBookkeeping bookkeeping(plant);
  auto models = SomeInternalParsingRoutine(plant, &bookkeeping);
  bookkeeping.FinalizeForModels(models);
}

For SDFormat composition, there are four "modes": (1.a) Pure SDFormat files - thus libsdformat handles composition, and Drake consumption of libsdformat info behaves nominally (1.b) Same as (1.a): As before, libsdformat handles everything, and Drake consumption of libsdformat info behaves nominally (2.a) Mix of SDFormat files and URDF/MJCF files. This will use SDFormat for top-level, and SDFormat's interface API for lower-level models, but will be parsed directly into the plant. Accurate scoping information will be available. (2.b) Same as (2.a), but with merge-include. To avoid infecting other parsing subroutines for lower-level models, we use a temporary plant, parse the included model into it, then graft the plant with some model-instance remapping.

On master, (1.a), (1.b), and (2.a) are currently in there, and should not pose an issue to properly function scoped names (per Rico's dev branch - at least per discussion).

(2.b) is pending #16727, and this is the only mode that poses a special challenge. Relevant to parsing, the fix may be something like this in the SDFormat:

CustomNestedModelParser(..., CollisionFilterBookkeeping* bookkeeping) {
  CollisionFilterBookkeeping nested_bookkeeping(nested_plant);
  DoInternalParsing(nested_plant);
  ...
  bookkeeping->ConsumeNestedBookkeeping(nested_plant, model_instance_remap_rules);
}

\cc @azeey

[rpoyner-tri]

Progress: rebased the dev branch (link above) so that multi-robot filtering in a simple SDFormat nested model case is working. Still TODO:

• test all the composition cases listed by @EricCousineau-TRI above.
• implement/test various error cases (names that violate nesting/encapsulation, typos, etc.)
• documentation
• back-fill unit tests for newly factored-out module
• extend to model directives

[jwnimmer-tri]

Could you hyperlink to the specification for the syntax of scoped names, and the corresponding semantics of how they shall be resolved? With a quick skim, I wasn't able to find it in the WIP branch.

[rpoyner-tri]

There's nothing I've yet written that tries to nail down scoped names. I'd say I'm working by analogy to the SDFormat composition proposal so far.

• scoping delimiter is '::', same as that chosen by SDFormat, and already supported by scoped_names.{cc,h}.
• 'local" names should still just work as before
• it is legal to use scoped names to refer to models/links/groups nested within the current scope ("downward", in @EricCousineau-TRI parlance)
• it is illegal to refer to things outside the current scope
• (therefore), all scoped names found in the input are treated as relative to a current scope

The widest "span" across which filters can be defined is determined by the top-level entry point used for the parse. For now it is the span implied by any of the Multibody::Parser methods (file, or string, or SDFormat file that composes various things). In some later iteration, that span will be widened by support for model directives, which can compose multiple calls to Multibody::Parser.

As for resolution, the collision filter bookkeeping to attempts to fully qualify the names it picks up by prepending the scoped name associated with the current model instance. If there is none, that is treated as "global scope" and names are passed on unmodified. Names of groups are stored as fully qualified string names until the final Resolve() step (see below); Name of members (links/bodies) are fully qualified, then reparsed to maybe find a model instance name. The parsed halves (model instance name, local name) are analyzed and used to find bodies and build up a geometry set.

Since the prior code permitted forward references from <drake:ignored_collision_filter_group name='blah'>, those names are not checked until the Resolve() step. The only requirement is that the fully qualified name match the name of a defined group.

So far, everything is "happy path" tested only; error behaviors need to be worked out. In particular, a lot of the existing lookup machinery supplied by MbP throws on error; that may not be the result we want.

[rpoyner-tri]

Pertinent example is the branch version of multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test.sdf. It has two identical nested models with some existing/internal filter groups, and the composition layer defines some additional multi-model filter groups.

[EricCousineau-TRI]

@rpoyner-tri Any chance we can watercooler on this briefly? I'm wondering if you need bookkeeping, or if you can leverage parsing phases to render info w/o bookkeeping. Then we don't need additional affordances for doing our SDFormat compositing.

[EricCousineau-TRI]

Ah, derp :facepalm: Forgot - the issue here is bookkeeping for the filter groups, not just the resolved filtering.

[jwnimmer-tri]

@rpoyner-tri Thanks, that's clear so far.

I think my only remaining question is clarity about what the levels of scoping would be. Could you enumerate the list (hierarchy) of levels, and give one concrete example of a fully-qualified name? Is it only model_instance_name::filter_group_name or are there other layers I'm missing?

[rpoyner-tri]

In sdformat (but, I think, not in model directives?) it is possible to nest models recursively. So , you could end up with model instances/names like 2<=>"a", 3<=>"a::b", 4<=>"a::b::c". The model instance index space is still flat, but model naming can indicate nesting. So, collision filter groups could have relative naming for members and other-groups that are like "b::c::link1" or "b::c::group1". In practice, the existing scoped_name routines split on last delimiter, so it's almost possible to ignore the nested-models detail. And given sane nesting and encapsulation practices, it seems maybe unnecessary to reach down multiple levels in most cases.

All that said, the prototype branch is still sorely lacking tests, and the implementation still relies on a bunch of throw-happy MbP api. So there is still work to do.

[jwnimmer-tri]

Thanks, I think I'm caught up now.

I'll highlight that the victory condition here is declaring filtering across two URDFs. In terms of an acceptance test, the code that we work on finishing and landing to master first should solve that problem specifically.

[rpoyner-tri]

About that victory condition: "across two URDFs" will not happen without some enclosing object; either an SDFormat file (via <include>) or model directives. Since URDF is inherently single-model, only some kind of forward/external reference could work. The SDFormat team spent plenty of time thinking about these issues and decided against supporting forward/external references.

[jwnimmer-tri]

Absolutely -- I didn't mean to imply that only those two files exist. The filters between "arm" and "torso" should neither live in the arm definition nor the torso definition; they should live in the file that assembles the arm and torso as a union, so either sdformat and/or model directives.

[rpoyner-tri]

Now that #17175 is close to landing, it's worth looking at some of the things it didn't solve. Some may want doing now/soon, some never, and some may merit an issue to work on later.

I believe these three should all happen. The first two could happen soon. The "subgraph" thing may have to wait for other patches to land.

1. clarify semantics of naming w.r.t. to the magic world_instance (0) and default_instance (1). (current PR draft just has a TODO).
2. allow filter definitions in SDFormat //sdf/world tag, not just with /model.
3. integration with "subgraph" parsing -- see #16727 and comments above from @EricCousineau-TRI .

These three are not strictly necessary, and might even be bad ideas.

4. model directives support
5. fancier xml tags and/or directives for defining filters 5.a. nesting: drag the entire contents of an existing group into a new group by name 5.b. explicit exclude-between by name of groups already defined, without having to add or relist member bodies
6. create plant/scene API so group names can persist and be used after parsing

[jwnimmer-tri]

(1) Seems worth doing.

(2) Can you provide an example (maybe a file) where this would be useful? Or is it maybe just easier to implement it correctly than try to explain in our documentation and warnings that it's unsupported?

(3) That pull request is doomed. I don't think we need to work on filters for that anytime soon.

(4) This might provide an easier onramp to this feature, versus someone needing to learn how SDFormat includes work. The choice here probably depends on how much effort it would take.

(5,6) Does the OP's example demonstrate any need for this? If not, punt and ignore.

[rpoyner-tri]

(1) Yup. I don't think there are dragons here, just needs more tests and doc. (2) I'll study this a bit more and come up with some examples. I speculate that the implementation is easy, but haven't confirmed. (3) nod. (4) Not huge effort but a bit fiddly. Based on current public APIs, probably requires a friend/attorney back-channel from ProcessModelDirectives() to Parser. (5,6) nod. Note that there are enthusiasts who may continue to raise these, at which point probably new issues should be filed.

[rpoyner-tri]

17241 is the first attempt to address (1).

[rpoyner-tri]

re (2), my current inclination is to punt. As far as I can tell, there is always the workaround of adding a layer of nested <model> inside <world>, and lodging any "global" collision filters there. That has the cost of "wasting" a model index, but probably it doesn't matter.

[rpoyner-tri]

Splitting (4) to its own issue: #17242.