[parsing] Enable multi-robot collision filter groups

Relevant to: RobotLocomotion#14785

This patch extends the parsing of collision filter groups so that collision
filters can be defined across multiple robots by using SDFormat model
composition.

Summary of changes:
 * add CollisionFilterGroupResolver, with unit test
 * port existing parser modules to use it
   * modernize a few sdf_parser errors
 * demonstrate multi-robot filters in sdf_parser_test

multibody/parsing/BUILD.bazel

@@ -60,13 +60,15 @@ drake_cc_library(
 drake_cc_library(
  name = "detail_misc",
  srcs = [
+ "detail_collision_filter_group_resolver.cc",
  "detail_common.cc",
  "detail_ignition.cc",
  "detail_path_utils.cc",
  "detail_tinyxml.cc",
  "detail_tinyxml2_diagnostic.cc",
  ],
  hdrs = [
+ "detail_collision_filter_group_resolver.h",
  "detail_common.h",
  "detail_ignition.h",
  "detail_path_utils.h",
@@ -77,6 +79,7 @@ drake_cc_library(
  visibility = ["//visibility:private"],
  deps = [
  ":package_map",
+ ":scoped_names",
  "//common:diagnostic_policy",
  "//common:essential",
  "//common:filesystem",
@@ -505,4 +508,12 @@ drake_cc_googletest(
  ],
 )
 
+drake_cc_googletest(
+ name = "detail_collision_filter_group_resolver_test",
+ deps = [
+ ":detail_misc",
+ ":diagnostic_policy_test_base",
+ ],
+)
+
 add_lint_tests()

multibody/parsing/detail_collision_filter_group_resolver.cc

@@ -0,0 +1,168 @@
+#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h"
+
+#include "drake/multibody/parsing/scoped_names.h"
+
+namespace drake {
+namespace multibody {
+namespace internal {
+
+using drake::internal::DiagnosticPolicy;
+using geometry::GeometrySet;
+using parsing::GetInstanceScopeName;
+using parsing::ParseScopedName;
+using parsing::PrefixName;
+using parsing::ScopedName;
+
+CollisionFilterGroupResolver::CollisionFilterGroupResolver(
+ MultibodyPlant<double>* plant) : plant_(plant) {
+ DRAKE_DEMAND(plant != nullptr);
+}
+
+void CollisionFilterGroupResolver::AddGroup(
+ const DiagnosticPolicy& diagnostic,
+ const std::string& group_name,
+ const std::set<std::string>& body_names,
+ std::optional<ModelInstanceIndex> model_instance) {
+ if (model_instance) {
+ DRAKE_DEMAND(*model_instance < plant_->num_model_instances());
+ }
+ DRAKE_DEMAND(!group_name.empty());
+ if (!CheckLegalName(diagnostic, group_name, "group name")) { return; }
+ if (!ParseScopedName(group_name).instance_name.empty()) {
+ diagnostic.Error(fmt::format("group name '{}' cannot be a scoped name",
+ FullyQualify(group_name, model_instance)));
+ return;
+ }
+ if (body_names.empty()) {
+ diagnostic.Error(fmt::format("group '{}' has no members",
+ FullyQualify(group_name, model_instance)));
+ return;
+ }
+
+ geometry::GeometrySet geometry_set;
+ for (const auto& body_name : body_names) {
+ DRAKE_DEMAND(!body_name.empty());
+ if (!CheckLegalName(diagnostic, body_name, "body name")) { continue; }
+ 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));
+ continue;
+ }
+ }
+
+ 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});
+}
+
+void CollisionFilterGroupResolver::AddPair(
+ const DiagnosticPolicy& diagnostic,
+ const std::string& group_name_a,
+ const std::string& group_name_b,
+ std::optional<ModelInstanceIndex> model_instance) {
+ DRAKE_DEMAND(!group_name_a.empty());
+ DRAKE_DEMAND(!group_name_b.empty());
+ if (model_instance) {
+ DRAKE_DEMAND(*model_instance < plant_->num_model_instances());
+ }
+ bool a_ok = CheckLegalName(diagnostic, group_name_a, "group name");
+ bool b_ok = CheckLegalName(diagnostic, group_name_b, "group name");
+ if (!(a_ok && b_ok)) {
+ return;
+ }
+
+ // Store group pairs by fully qualified name. The groups don't need to
+ // actually be defined until Resolve() time.
+ const std::string name_a = FullyQualify(group_name_a, model_instance);
+ const std::string name_b = FullyQualify(group_name_b, model_instance);
+
+ // These two group names are allowed to be identical, which means the
+ // bodies inside this collision filter group should be collision excluded
+ // among each other.
+ pairs_.insert({name_a, name_b});
+}
+
+void CollisionFilterGroupResolver::Resolve(const DiagnosticPolicy& diagnostic) {
+ for (const auto& [name_a, name_b] : pairs_) {
+ const GeometrySet* set_a = CheckGroup(diagnostic, name_a);
+ const GeometrySet* set_b = CheckGroup(diagnostic, name_b);
+ if (!(set_a && set_b)) {
+ continue;
+ }
+ plant_->ExcludeCollisionGeometriesWithCollisionFilterGroupPair(
+ {name_a, *set_a}, {name_b, *set_b});
+ }
+ groups_.clear();
+ pairs_.clear();
+}
+
+bool CollisionFilterGroupResolver::CheckLegalName(
+ const drake::internal::DiagnosticPolicy& diagnostic,
+ std::string_view name,
+ const std::string& description) const {
+ DRAKE_DEMAND(!name.empty());
+
+ const std::string_view delim(parsing::internal::kScopedNameDelim);
+ // The main objective here is to avoid aborting with a dubious message in
+ // ParseScope(). There are numerous other degenerate name cases, but those
+ // likely won't abort in ParseScope() and will be caught later on by full
+ // qualification and lookup.
+ bool legal = (name.front() != delim.front()) &&
+ (name.back() != delim.back());
+ if (!legal) {
+ diagnostic.Error(fmt::format("{} '{}' can neither begin nor end with '{}'",
+ description, name, delim));
+ }
+ return legal;
+}
+
+std::string CollisionFilterGroupResolver::FullyQualify(
+ const std::string& name,
+ std::optional<ModelInstanceIndex> model_instance) const {
+ if (!model_instance) {
+ // Names found in global scope are just themselves.
+ return name;
+ }
+
+ // Treat the name as relative and just prefix it with the model instance
+ // name. Misuses of scoping will fail in lookup.
+ return PrefixName(GetInstanceScopeName(*plant_, *model_instance), name);
+}
+
+const GeometrySet* CollisionFilterGroupResolver::CheckGroup(
+ const DiagnosticPolicy& diagnostic, const std::string& group_name) const {
+ auto iter = groups_.find(group_name);
+ if (iter == groups_.end()) {
+ diagnostic.Error(
+ fmt::format("collision filter group with name '{}' not found",
+ group_name));
+ return nullptr;
+ }
+ return &iter->second;
+}
+
+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);
+ }
+ return {};
+}
+
+} // namespace internal
+} // namespace multibody
+} // namespace drake

multibody/parsing/detail_collision_filter_group_resolver.h

@@ -0,0 +1,112 @@
+#pragma once
+
+#include <map>
+#include <optional>
+#include <set>
+#include <string>
+
+#include "drake/common/diagnostic_policy.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/common/sorted_pair.h"
+#include "drake/geometry/geometry_set.h"
+#include "drake/multibody/plant/multibody_plant.h"
+
+namespace drake {
+namespace multibody {
+namespace internal {
+
+// Collects and resolves collision filter group information, potentially across
+// multiple models. Uses the same scoped-name semantics as SDFormat and model
+// directives to allow reference and resolution of names across models.
+//
+// All methods use the supplied MultibodyPlant as the source of truth for
+// existence and naming of models and bodies. The Resolve() step writes the
+// finished collision filtering rules to the plant.
+//
+// All methods may emit warnings or errors to a passed diagnostic policy
+// channel.
+//
+// @note model_instance: All methods use a model instance parameter to denote
+// the current scope where information is found during parsing. Its name is
+// used to expand the relative names found to absolute names for further
+// 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).
+//
+class CollisionFilterGroupResolver {
+ public:
+ DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CollisionFilterGroupResolver)
+
+ // The @p plant parameter is aliased, and must outlive the resolver.
+ // @pre plant is not nullptr.
+ explicit CollisionFilterGroupResolver(MultibodyPlant<double>* plant);
+
+ // Adds a collision filter group. Locates bodies by name immediately, and
+ // emits errors for illegal names, empty groups, or missing bodies.
+ //
+ // @param diagnostic The error-reporting channel.
+ // @param group_name The name of the group being defined.
+ // @param body_names The names of the bodies that are group members.
+ // @param model_instance The current model, for scoping purposes.
+ // @see model_instance note above.
+ //
+ // @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.
+ void AddGroup(
+ const drake::internal::DiagnosticPolicy& diagnostic,
+ const std::string& group_name,
+ const std::set<std::string>& body_names,
+ std::optional<ModelInstanceIndex> model_instance);
+
+ // Adds a group pair. Emits diagnostics for illegal names. Two distinct
+ // names will resolve to an exclude-between-groups rule; identical names will
+ // resolve to an exclude-within-group rule. Group names may refer to a group
+ // not defined until later; group existence will be checked in the Resolve()
+ // step.
+ //
+ // @param diagnostic The error-reporting channel.
+ // @param group_name_a The name of a defined group.
+ // @param group_name_b The name of a defined group.
+ // @param model_instance The current model, for scoping purposes.
+ // @see model_instance note above.
+ //
+ // @pre neither group_name_a nor group_name_b is empty.
+ // @pre model_instance has a valid index or else no value.
+ void AddPair(
+ const drake::internal::DiagnosticPolicy& diagnostic,
+ const std::string& group_name_a,
+ const std::string& group_name_b,
+ std::optional<ModelInstanceIndex> model_instance);
+
+ // Resolves group pairs to rules, and clears all internal bookkeeping
+ // state. Emits diagnostics for undefined groups.
+ //
+ // @note Clearing state allows the object to be reused for subsequent
+ // parsing. It is up to clients of this class to Resolve() at the end of a
+ // defined parsing operation, where resolution across models makes sense.
+ void Resolve(const drake::internal::DiagnosticPolicy& diagnostic);
+
+ private:
+ bool CheckLegalName(const drake::internal::DiagnosticPolicy& diagnostic,
+ std::string_view name,
+ const std::string& description) const;
+ std::string FullyQualify(
+ const std::string& name,
+ std::optional<ModelInstanceIndex> model_instance) const;
+ const geometry::GeometrySet* CheckGroup(
+ const drake::internal::DiagnosticPolicy& diagnostic,
+ const std::string& group_name) const;
+ const Body<double>* FindBody(
+ const std::string& name,
+ std::optional<ModelInstanceIndex> model_instance);
+ MultibodyPlant<double>* const plant_;
+ std::map<std::string, geometry::GeometrySet> groups_;
+ std::set<SortedPair<std::string>> pairs_;
+};
+
+} // namespace internal
+} // namespace multibody
+} // namespace drake