[parsing] Restrict collision filters to models within a parse operati…

…on (RobotLocomotion#17241)

* [parsing] Restrict collision filters to models within a parse operation

Relevant to: RobotLocomotion#14785

This patch implements a stronger enforcement of model encapsulation for
collision filters. Previously, filters defined outside any model could refer to
the world body or parts of the default model by scoped names, or to default
model bodies by unqualified names. Now, those references are parse errors, and
using a model index from outside the parse as a parameter is a programming
error.

There might ever be a reason (I couldn't think of one) to define collision
filters against the world body; I am fine for that to involve a special
privileged name and be future work.

I couldn't think of any reason why a model file should be able to refer to
bodies in the default model, which (in the current implementation of Parser),
could never be populated from model files.

multibody/parsing/detail_collision_filter_group_resolver.cc

@@ -14,8 +14,11 @@ using parsing::PrefixName;
 using parsing::ScopedName;
 
 CollisionFilterGroupResolver::CollisionFilterGroupResolver(
- MultibodyPlant<double>* plant) : plant_(plant) {
+ MultibodyPlant<double>* plant)
+ : plant_(plant) {
  DRAKE_DEMAND(plant != nullptr);
+ minimum_model_instance_index_ =
+ ModelInstanceIndex(plant->num_model_instances());
 }
 
 CollisionFilterGroupResolver::~CollisionFilterGroupResolver() {}
@@ -27,6 +30,7 @@ void CollisionFilterGroupResolver::AddGroup(
  std::optional<ModelInstanceIndex> model_instance) {
  if (model_instance) {
  DRAKE_DEMAND(*model_instance < plant_->num_model_instances());
+ DRAKE_DEMAND(*model_instance >= minimum_model_instance_index_);
  }
  DRAKE_DEMAND(!group_name.empty());
  if (!CheckLegalName(diagnostic, group_name, "group name")) { return; }
@@ -48,22 +52,22 @@ void CollisionFilterGroupResolver::AddGroup(
  const std::string qualified = FullyQualify(body_name, model_instance);
  ScopedName scoped_name = ParseScopedName(qualified);
 
- std::optional<ModelInstanceIndex> body_model;
- if (!scoped_name.instance_name.empty()) {
- if (plant_->HasModelInstanceNamed(scoped_name.instance_name)) {
- body_model = plant_->GetModelInstanceByName(scoped_name.instance_name);
- } else {
- diagnostic.Error(fmt::format("body with name '{}' not found",
- qualified));
+ const Body<double>* body{};
+ if (plant_->HasModelInstanceNamed(scoped_name.instance_name)) {
+ ModelInstanceIndex body_model =
+   plant_->GetModelInstanceByName(scoped_name.instance_name);
+ if (body_model < minimum_model_instance_index_) {
+ diagnostic.Error(fmt::format("body name '{}' refers to a model outside"
+ " the current parse", qualified));
  continue;
  }
+ body = FindBody(scoped_name.name, body_model);
  }
-
- const Body<double>* body = FindBody(scoped_name.name, body_model);
  if (!body) {
  diagnostic.Error(fmt::format("body with name '{}' not found", qualified));
  continue;
  }
+
  geometry_set.Add(plant_->GetBodyFrameIdOrThrow(body->index()));
  }
  groups_.insert({FullyQualify(group_name, model_instance), geometry_set});
@@ -78,6 +82,7 @@ void CollisionFilterGroupResolver::AddPair(
  DRAKE_DEMAND(!group_name_b.empty());
  if (model_instance) {
  DRAKE_DEMAND(*model_instance < plant_->num_model_instances());
+ DRAKE_DEMAND(*model_instance >= minimum_model_instance_index_);
  }
  bool a_ok = CheckLegalName(diagnostic, group_name_a, "group name");
  bool b_ok = CheckLegalName(diagnostic, group_name_b, "group name");
@@ -158,10 +163,9 @@ const GeometrySet* CollisionFilterGroupResolver::FindGroup(
 
 const Body<double>* CollisionFilterGroupResolver::FindBody(
  const std::string& name,
- std::optional<ModelInstanceIndex> model_instance) {
- ModelInstanceIndex model = model_instance.value_or(default_model_instance());
- if (plant_->HasBodyNamed(name, model)) {
- return &plant_->GetBodyByName(name, model);
+ ModelInstanceIndex model_instance) {
+ if (plant_->HasBodyNamed(name, model_instance)) {
+ return &plant_->GetBodyByName(name, model_instance);
  }
  return {};
 }

multibody/parsing/detail_collision_filter_group_resolver.h

@@ -32,8 +32,26 @@ namespace internal {
 // internal processing. If a model instance value is not provided, names are
 // evaluated with respect to the "root" of the plant.
 //
-// TODO(rpoyner-tri): clarify the roles of the world instance (0) and the
-// default model instance (1).
+// @note valid_model_instance The resolver rejects model instance parameters
+// and scoped names that refer to models built prior to the "current parsing
+// operation". The current parsing operation covers the processing of some
+// outermost model definition file, and all of its nested or included
+// models. During that processing, one or more new model instances will be
+// added to represent the models in the input data.
+
+// The collision filter group resolver expects to be constructed before the
+// parsing operation begins, and be called to resolve the filters after the
+// parsing operation ends. To restrict references to models within the current
+// parse, it remembers the plant's `num_model_instances()` at the time the
+// resolver is constructed, calling it the
+// `minimum_model_instance_index`. Hence, valid model instances must lie with
+// in the range:
+//
+// valid_model ∈ [minimum_model_instance_index(), num_model_instances())
+//
+// Passing model instance parameters out of that range is a programming
+// error. Parsed names that refer to models out of range will generate
+// error messages.
 //
 class CollisionFilterGroupResolver {
  public:
@@ -45,6 +63,11 @@ class CollisionFilterGroupResolver {
 
  ~CollisionFilterGroupResolver();
 
+ // @returns the minimum model instance index in force for this resolver.
+ ModelInstanceIndex minimum_model_instance_index() const {
+ return minimum_model_instance_index_;
+ }
+
  // Adds a collision filter group. Locates bodies by name immediately, and
  // emits errors for illegal names, empty groups, or missing bodies.
  //
@@ -57,6 +80,7 @@ class CollisionFilterGroupResolver {
  // @pre group_name is not empty.
  // @pre no member strings of body_names are empty.
  // @pre model_instance has a valid index or else no value.
+ // @see valid_model_instance note above.
  void AddGroup(
  const drake::internal::DiagnosticPolicy& diagnostic,
  const std::string& group_name,
@@ -77,6 +101,7 @@ class CollisionFilterGroupResolver {
  //
  // @pre neither group_name_a nor group_name_b is empty.
  // @pre model_instance has a valid index or else no value.
+ // @see valid_model_instance note above.
  void AddPair(
  const drake::internal::DiagnosticPolicy& diagnostic,
  const std::string& group_name_a,
@@ -101,14 +126,14 @@ class CollisionFilterGroupResolver {
  const drake::internal::DiagnosticPolicy& diagnostic,
  const std::string& group_name) const;
 
- const Body<double>* FindBody(
- const std::string& name,
- std::optional<ModelInstanceIndex> model_instance);
+ const Body<double>* FindBody(const std::string& name,
+ ModelInstanceIndex model_instance);
 
  MultibodyPlant<double>* const plant_;
  std::map<std::string, geometry::GeometrySet> groups_;
  std::set<SortedPair<std::string>> pairs_;
  bool is_resolved_{false};
+ ModelInstanceIndex minimum_model_instance_index_{};
 };
 
 } // namespace internal

multibody/parsing/test/detail_collision_filter_group_resolver_test.cc

@@ -110,6 +110,25 @@ TEST_F(CollisionFilterGroupResolverTest, MissingBodyGlobal) {
  EXPECT_THAT(TakeError(), MatchesRegex(".*'zzz::abody'.*not found"));
 }
 
+TEST_F(CollisionFilterGroupResolverTest, OutOfParseBodyGlobal) {
+ // The world body and bodies in the default model are always outside of any
+ // parse operation.
+ AddBody("stuff", {});
+ resolver_.AddGroup(diagnostic_policy_, "a", {
+ "DefaultModelInstance::stuff",
+ "WorldModelInstance::WorldBody",
+ "stuff",
+ },
+ {});
+ EXPECT_THAT(TakeError(), MatchesRegex(".*'DefaultModelInstance::stuff'"
+ ".*outside the current parse"));
+ EXPECT_THAT(TakeError(), MatchesRegex(".*'WorldModelInstance::WorldBody'"
+ ".*outside the current parse"));
+ // Ensure that unqualified bodies names at global scope aren't looked up in
+ // the default model inadvertently.
+ EXPECT_THAT(TakeError(), MatchesRegex(".*'stuff'.*not found"));
+}
+
 TEST_F(CollisionFilterGroupResolverTest, MissingBodyScoped) {
  ModelInstanceIndex r1 = plant_.AddModelInstance("r1");
  plant_.AddModelInstance("r1::sub");
@@ -121,12 +140,15 @@ TEST_F(CollisionFilterGroupResolverTest, MissingBodyScoped) {
  EXPECT_THAT(TakeError(), MatchesRegex(".*'r1::zzz::abody'.*not found"));
 }
 
-TEST_F(CollisionFilterGroupResolverTest, BodyGlobal) {
- // Ensure body names can be resolved in the absence of any model scopes.
- GeometryId b1 = AddBody("body1", {});
- GeometryId b2 = AddBody("body2", {});
- resolver_.AddGroup(diagnostic_policy_, "a", {"body1", "body2"}, {});
- resolver_.AddPair(diagnostic_policy_, "a", "a", {});
+TEST_F(CollisionFilterGroupResolverTest, GroupGlobal) {
+ // Ensure names can be resolved for groups defined at global scope.
+ ModelInstanceIndex r1 = plant_.AddModelInstance("r1");
+ ModelInstanceIndex r2 = plant_.AddModelInstance("r2");
+ GeometryId b1 = AddBody("body1", r1);
+ GeometryId b2 = AddBody("body2", r2);
+ resolver_.AddGroup(diagnostic_policy_, "a", {"r1::body1"}, {});
+ resolver_.AddGroup(diagnostic_policy_, "b", {"r2::body2"}, {});
+ resolver_.AddPair(diagnostic_policy_, "a", "b", {});
  resolver_.Resolve(diagnostic_policy_);
  EXPECT_TRUE(inspector_.CollisionFiltered(b1, b2));
 }