2

When constructing a MultiBodyPlant from multiple ModelInstances each loaded from a separate URDF, is there a way to define collision filter groups to exclude collisions across model instances? 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.
syler
  • 23
  • 3

1 Answers1

1

The scope of drake:collision_filter_group is limited to the scope of the file. So, it's sufficient to prevent "fake" collision at a robot's joints but insufficient to prevent two robots that get parsed independently from colliding.

In order to achieve that end, you'll have to parse the two robots, and then algorithmically declare the collision filters.

I don't have a great deal of time right now, so I'll give a rough overview of what it looks like, and then provide code when I free up later:

The approach.

  1. Parse both URDFs (SDFs).
  2. Grab the bodies for model instance "a" (link1 and link2), and similarly the bodies for model instance "b" (link4 and link5).
  3. Using the MBP interface get the geometry::FrameId associated with each of those bodies. (We'll call them frameA1, frameA2, frameB4, frameB5, respectively.
  4. Create two instances of geometry::GeometrySet. geometry_set_A = {frameA1, frameA2} and geometry_set_B = {frameB4, frameB5}.
  5. Invoke SceneGraph::ExcludeCollisionsBetween(geometry_set_A, geometry_set_b).

Done

This should give you the first glimmer of how to proceed. I'll come by for a second pass to give concrete code to accomplish the task later.

Sean Curtis
  • 1,425
  • 7
  • 8
  • Thanks. This ended up working, but it would be really nice to support this kind of functionality in a way that could be specified in the URDF without hardcoding when constructing the system model. – syler Mar 09 '21 at 14:04
  • I can understand the desire. In many ways, SDF may be more amenable to this kind of thing because it fundamentally incorporates the idea of composition. With URDF, there is no formal composition, so creating links between otherwise independent files is fraught with danger. However, you've clearly got an application that would benefit from this. I'd propose you create an issue where we can discuss what the semantics and mechanism may be. That'd be the first logical step in realizing this kind of functionality. – Sean Curtis Mar 10 '21 at 15:20
  • For those interested, @syler has posted an issue and you can follow along with the conversation: https://github.com/RobotLocomotion/drake/issues/14785 – Sean Curtis Mar 17 '21 at 18:04