diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel index f019cdcbcc65..38eba634ce16 100644 --- a/multibody/parsing/BUILD.bazel +++ b/multibody/parsing/BUILD.bazel @@ -73,6 +73,7 @@ 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", @@ -80,6 +81,7 @@ drake_cc_library( "detail_tinyxml2_diagnostic.cc", ], hdrs = [ + "detail_collision_filter_group_resolver.h", "detail_common.h", "detail_ignition.h", "detail_path_utils.h", @@ -87,6 +89,7 @@ drake_cc_library( "detail_tinyxml2_diagnostic.h", ], install_hdrs_exclude = [ + "detail_collision_filter_group_resolver.h", "detail_common.h", "detail_ignition.h", "detail_path_utils.h", @@ -99,6 +102,7 @@ drake_cc_library( ], deps = [ ":package_map", + ":scoped_names", "//common:diagnostic_policy", "//common:essential", "//common:filesystem", @@ -600,4 +604,12 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "detail_collision_filter_group_resolver_test", + deps = [ + ":detail_misc", + ":diagnostic_policy_test_base", + ], +) + add_lint_tests() diff --git a/multibody/parsing/detail_collision_filter_group_resolver.cc b/multibody/parsing/detail_collision_filter_group_resolver.cc new file mode 100644 index 000000000000..a73fea44c1c6 --- /dev/null +++ b/multibody/parsing/detail_collision_filter_group_resolver.cc @@ -0,0 +1,164 @@ +#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* plant) : plant_(plant) { + DRAKE_DEMAND(plant != nullptr); +} + +void CollisionFilterGroupResolver::AddGroup( + const DiagnosticPolicy& diagnostic, + const std::string& group_name, + const std::set& body_names, + std::optional 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; } + ScopedName scoped_group_name = ParseScopedName(group_name); + if (!scoped_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 body_model; + if (!scoped_name.instance_name.empty() && + plant_->HasModelInstanceNamed(scoped_name.instance_name)) { + body_model = plant_->GetModelInstanceByName(scoped_name.instance_name); + } + + const Body* 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 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, + const 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 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* CollisionFilterGroupResolver::FindBody( + const std::string& name, + std::optional 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 diff --git a/multibody/parsing/detail_collision_filter_group_resolver.h b/multibody/parsing/detail_collision_filter_group_resolver.h new file mode 100644 index 000000000000..ec3398867a33 --- /dev/null +++ b/multibody/parsing/detail_collision_filter_group_resolver.h @@ -0,0 +1,112 @@ +#pragma once + +#include +#include +#include +#include + +#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* 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& body_names, + std::optional 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 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, + const std::string_view name, + const std::string& description) const; + std::string FullyQualify( + const std::string& name, + std::optional model_instance) const; + const geometry::GeometrySet* CheckGroup( + const drake::internal::DiagnosticPolicy& diagnostic, + const std::string& group_name) const; + const Body* FindBody( + const std::string& name, + std::optional model_instance); + MultibodyPlant* const plant_; + std::map groups_; + std::set> pairs_; +}; + +} // namespace internal +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/detail_common.cc b/multibody/parsing/detail_common.cc index 164c82e5ee3e..55556b1f159f 100644 --- a/multibody/parsing/detail_common.cc +++ b/multibody/parsing/detail_common.cc @@ -135,10 +135,10 @@ const LinearBushingRollPitchYaw* ParseLinearBushingRollPitchYaw( // See ParseCollisionFilterGroupCommon at header for documentation void CollectCollisionFilterGroup( + const DiagnosticPolicy& diagnostic, ModelInstanceIndex model_instance, const MultibodyPlant& plant, const ElementNode& group_node, - std::map* collision_filter_groups, - std::set>* collision_filter_pairs, + CollisionFilterGroupResolver* resolver, const std::function& next_child_element, const std::function& @@ -159,7 +159,7 @@ void CollectCollisionFilterGroup( const std::string group_name = read_string_attribute(group_node, "name"); if (group_name.empty()) { return; } - geometry::GeometrySet collision_filter_geometry_set; + std::set bodies; for (auto member_node = next_child_element(group_node, "drake:member"); std::holds_alternative(member_node) ? std::get(member_node) != nullptr @@ -168,11 +168,9 @@ void CollectCollisionFilterGroup( const std::string body_name = read_tag_string(member_node, "link"); if (body_name.empty()) { continue; } - const auto& body = plant.GetBodyByName(body_name.c_str(), model_instance); - collision_filter_geometry_set.Add( - plant.GetBodyFrameIdOrThrow(body.index())); + bodies.insert(body_name); } - collision_filter_groups->insert({group_name, collision_filter_geometry_set}); + resolver->AddGroup(diagnostic, group_name, bodies, model_instance); for (auto ignore_node = next_child_element( group_node, "drake:ignored_collision_filter_group"); @@ -187,14 +185,16 @@ void CollectCollisionFilterGroup( // 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. - collision_filter_pairs->insert({group_name.c_str(), target_name.c_str()}); + resolver->AddPair(diagnostic, group_name, target_name, model_instance); } } void ParseCollisionFilterGroupCommon( + const DiagnosticPolicy& diagnostic, ModelInstanceIndex model_instance, const ElementNode& model_node, MultibodyPlant* plant, + CollisionFilterGroupResolver* resolver, const std::function& next_child_element, const std::function& @@ -207,8 +207,6 @@ void ParseCollisionFilterGroupCommon( const std::function& read_tag_string) { DRAKE_DEMAND(plant->geometry_source_is_registered()); - std::map collision_filter_groups; - std::set> collision_filter_pairs; for (auto group_node = next_child_element(model_node, "drake:collision_filter_group"); @@ -218,20 +216,10 @@ void ParseCollisionFilterGroupCommon( group_node = next_sibling_element(group_node, "drake:collision_filter_group")) { CollectCollisionFilterGroup( - model_instance, *plant, group_node, &collision_filter_groups, - &collision_filter_pairs, next_child_element, next_sibling_element, - has_attribute, read_string_attribute, read_bool_attribute, - read_tag_string); - } - - for (const auto& [name_a, name_b] : collision_filter_pairs) { - const auto group_a = collision_filter_groups.find(name_a); - DRAKE_DEMAND(group_a != collision_filter_groups.end()); - const auto group_b = collision_filter_groups.find(name_b); - DRAKE_DEMAND(group_b != collision_filter_groups.end()); - - plant->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( - {name_a, group_a->second}, {name_b, group_b->second}); + diagnostic, + model_instance, *plant, group_node, resolver, next_child_element, + next_sibling_element, has_attribute, read_string_attribute, + read_bool_attribute, read_tag_string); } } diff --git a/multibody/parsing/detail_common.h b/multibody/parsing/detail_common.h index 0ffae0604663..a2581266c486 100644 --- a/multibody/parsing/detail_common.h +++ b/multibody/parsing/detail_common.h @@ -12,6 +12,7 @@ #include "drake/common/diagnostic_policy.h" #include "drake/common/drake_copyable.h" #include "drake/geometry/proximity_properties.h" +#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h" #include "drake/multibody/plant/coulomb_friction.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/linear_bushing_roll_pitch_yaw.h" @@ -168,10 +169,14 @@ const LinearBushingRollPitchYaw* ParseLinearBushingRollPitchYaw( // tag in both SDF and URDF can be controlled/modified in a single function. // Functors are allowed to throw an exception when the requested quantities // are not available. +// @param diagnostic The error-reporting channel. // @param model_instance Model Instance that contains the bodies involved // in the collision filter groups. // @param model_node Node used to parse for the // collision_gilter_group tag. +// @param plant MultibodyPlant used to register the collision +// filter groups. +// @param resolver Collects the collision filter group data. // @param next_child_element Function that returns the next child element // with the specified tag in the ElementNode // provided. @@ -188,11 +193,11 @@ const LinearBushingRollPitchYaw* ParseLinearBushingRollPitchYaw( // name provided in the ElementNoded provided. // @param read_bool_attribute Function that reads a boolean attribute with // the name provided in the ElementNode provided. -// @param plant MultibodyPlant used to register the collision -// filter groups. void ParseCollisionFilterGroupCommon( + const drake::internal::DiagnosticPolicy& diagnostic, ModelInstanceIndex model_instance, const ElementNode& model_node, MultibodyPlant* plant, + internal::CollisionFilterGroupResolver* resolver, const std::function& next_child_element, const std::function& diff --git a/multibody/parsing/detail_parsing_workspace.h b/multibody/parsing/detail_parsing_workspace.h index 66d28cbf6e9f..f51d58bc027d 100644 --- a/multibody/parsing/detail_parsing_workspace.h +++ b/multibody/parsing/detail_parsing_workspace.h @@ -3,6 +3,7 @@ #include "drake/common/diagnostic_policy.h" #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" +#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h" #include "drake/multibody/parsing/package_map.h" #include "drake/multibody/plant/multibody_plant.h" @@ -15,29 +16,31 @@ namespace internal { // elsewhere. // // Note that code using this struct may pass it via const-ref, but the -// indicated plant will still be mutable; only its pointer within the struct is -// const. +// indicated plant and resolver objects will still be mutable; only the pointer +// values within the struct are const. struct ParsingWorkspace { DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ParsingWorkspace) // All parameters are aliased; they must have a lifetime greater than that of // this struct. - ParsingWorkspace( - const PackageMap& package_map_in, - const drake::internal::DiagnosticPolicy& diagnostic_in, - MultibodyPlant* plant_in) + ParsingWorkspace(const PackageMap& package_map_in, + const drake::internal::DiagnosticPolicy& diagnostic_in, + MultibodyPlant* plant_in, + internal::CollisionFilterGroupResolver* resolver_in) : package_map(package_map_in), diagnostic(diagnostic_in), - plant(plant_in) { + plant(plant_in), + resolver(resolver_in) { DRAKE_DEMAND(plant != nullptr); + DRAKE_DEMAND(resolver != nullptr); } const PackageMap& package_map; const drake::internal::DiagnosticPolicy& diagnostic; MultibodyPlant* const plant; + internal::CollisionFilterGroupResolver* const resolver; }; } // namespace internal } // namespace multibody } // namespace drake - diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 021a6d0e6c54..e29d85bfa09f 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -1063,9 +1063,11 @@ bool AreWelded( return false; } -void ParseCollisionFilterGroup(ModelInstanceIndex model_instance, +void ParseCollisionFilterGroup(const DiagnosticPolicy& diagnostic, + ModelInstanceIndex model_instance, const sdf::Model& model, - MultibodyPlant* plant) { + MultibodyPlant* plant, + CollisionFilterGroupResolver* resolver) { auto next_child_element = [](const ElementNode& data_element, const char* element_name) { return std::get(data_element) @@ -1081,37 +1083,48 @@ void ParseCollisionFilterGroup(ModelInstanceIndex model_instance, return std::get(data_element) ->HasAttribute(std::string(attribute_name)); }; - auto get_string_attribute = [](const ElementNode& data_element, - const char* attribute_name) { - if (!std::get(data_element) - ->HasAttribute(attribute_name)) { - throw std::runtime_error(fmt::format( - "{}:{}:{} The tag <{}> is missing the required attribute \"{}\"", - __FILE__, __func__, __LINE__, - std::get(data_element)->GetName(), - attribute_name)); - } - return std::get(data_element) - ->Get(attribute_name); - }; + auto get_string_attribute = + [&diagnostic](const ElementNode& data_element, + const char* attribute_name) -> std::string { + auto element = std::get(data_element); + if (!element->HasAttribute(attribute_name)) { + DiagnosticDetail detail; + detail.filename = element->FilePath(); + detail.line = element->LineNumber(); + detail.message = fmt::format( + "The tag <{}> is missing the required attribute \"{}\"", + element->GetName(), attribute_name); + diagnostic.Error(detail); + return {}; + } + return std::get(data_element) + ->Get(attribute_name); + }; auto get_bool_attribute = [](const ElementNode& data_element, const char* attribute_name) { return std::get(data_element)->Get(attribute_name); }; - auto read_tag_string = [](const ElementNode& data_element, const char*) { - sdf::ParamPtr param = std::get(data_element)->GetValue(); - if (param == nullptr) { - throw std::runtime_error(fmt::format( - "{}:{}:{} The tag <{}> is missing a required string value.", __FILE__, - __func__, __LINE__, - std::get(data_element)->GetName())); - } - return param->GetAsString(); - }; - ParseCollisionFilterGroupCommon(model_instance, model.Element(), plant, - next_child_element, next_sibling_element, - has_attribute, get_string_attribute, - get_bool_attribute, read_tag_string); + auto read_tag_string = + [&diagnostic](const ElementNode& data_element, const char*) + -> std::string { + auto element = std::get(data_element); + sdf::ParamPtr param = element->GetValue(); + if (param == nullptr) { + DiagnosticDetail detail; + detail.filename = element->FilePath(); + detail.line = element->LineNumber(); + detail.message = fmt::format( + "The tag <{}> is missing a required string value.", + element->GetName()); + diagnostic.Error(detail); + return {}; + } + return param->GetAsString(); + }; + ParseCollisionFilterGroupCommon( + diagnostic, model_instance, model.Element(), plant, resolver, + next_child_element, next_sibling_element, has_attribute, + get_string_attribute, get_bool_attribute, read_tag_string); } // Helper method to add a model to a MultibodyPlant given an sdf::Model @@ -1122,6 +1135,7 @@ std::vector AddModelsFromSpecification( const std::string& model_name, const RigidTransformd& X_WP, MultibodyPlant* plant, + CollisionFilterGroupResolver* resolver, const PackageMap& package_map, const std::string& root_dir) { @@ -1162,7 +1176,7 @@ std::vector AddModelsFromSpecification( AddModelsFromSpecification( diagnostic, nested_model, sdf::JoinName(model_name, nested_model.Name()), X_WM, - plant, package_map, root_dir); + plant, resolver, package_map, root_dir); added_model_instances.insert(added_model_instances.end(), nested_model_instances.begin(), @@ -1273,7 +1287,8 @@ std::vector AddModelsFromSpecification( // Parses the collision filter groups only if the scene graph is registered. if (plant->geometry_source_is_registered()) { drake::log()->trace("sdf_parser: Add collision filter groups"); - ParseCollisionFilterGroup(model_instance, model, plant); + ParseCollisionFilterGroup( + diagnostic, model_instance, model, plant, resolver); } return added_model_instances; @@ -1350,7 +1365,8 @@ void AddJointsToInterfaceModel(const MultibodyPlant& plant, // This is a forward-declaration of an anonymous helper that's defined later // in this file. sdf::ParserConfig MakeSdfParserConfig( - const PackageMap&, MultibodyPlant*, bool test_sdf_forced_nesting); + const PackageMap&, MultibodyPlant*, CollisionFilterGroupResolver*, + bool test_sdf_forced_nesting); sdf::Error MakeSdfError(sdf::ErrorCode code, const DiagnosticDetail& detail) { sdf::Error result(code, detail.message); @@ -1370,10 +1386,11 @@ sdf::Error MakeSdfError(sdf::ErrorCode code, const DiagnosticDetail& detail) { // parsers comply with this assumption. sdf::InterfaceModelPtr ParseNestedInterfaceModel( MultibodyPlant* plant, const PackageMap& package_map, + CollisionFilterGroupResolver* resolver, const sdf::NestedInclude& include, sdf::Errors* errors, bool test_sdf_forced_nesting) { const sdf::ParserConfig parser_config = MakeSdfParserConfig( - package_map, plant, test_sdf_forced_nesting); + package_map, plant, resolver, test_sdf_forced_nesting); // Do not attempt to parse anything other than URDF or forced nesting files. const bool is_urdf = EndsWith(include.ResolvedFileName(), kExtUrdf); @@ -1408,7 +1425,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( // New instances will have indices starting from cur_num_models int cur_num_models = plant->num_model_instances(); if (is_urdf) { - ParsingWorkspace workspace{package_map, diagnostic, plant}; + ParsingWorkspace workspace{package_map, diagnostic, plant, resolver}; const std::optional maybe_model = AddModelFromUrdf(data_source, include.LocalModelName().value_or(""), include.AbsoluteParentName(), workspace); @@ -1451,7 +1468,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( main_model_instance = AddModelsFromSpecification( diagnostic, model, sdf::JoinName(include.AbsoluteParentName(), model_name), {}, - plant, package_map, data_source.GetRootDir()).front(); + plant, resolver, package_map, data_source.GetRootDir()).front(); } // Now that the model is parsed, we create interface elements to send to @@ -1531,6 +1548,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( sdf::ParserConfig MakeSdfParserConfig( const PackageMap& package_map, MultibodyPlant* plant, + CollisionFilterGroupResolver* resolver, bool test_sdf_forced_nesting) { // The error severity settings here are somewhat subtle. We set all of them @@ -1560,9 +1578,10 @@ sdf::ParserConfig MakeSdfParserConfig( }); parser_config.RegisterCustomModelParser( - [&package_map, plant, test_sdf_forced_nesting]( + [&package_map, plant, resolver, test_sdf_forced_nesting]( const sdf::NestedInclude& include, sdf::Errors& errors) { - return ParseNestedInterfaceModel(plant, package_map, include, &errors, + return ParseNestedInterfaceModel(plant, package_map, resolver, + include, &errors, test_sdf_forced_nesting); }); @@ -1578,7 +1597,8 @@ std::optional AddModelFromSdf( DRAKE_THROW_UNLESS(!workspace.plant->is_finalized()); sdf::ParserConfig parser_config = MakeSdfParserConfig( - workspace.package_map, workspace.plant, test_sdf_forced_nesting); + workspace.package_map, workspace.plant, workspace.resolver, + test_sdf_forced_nesting); sdf::Root root; @@ -1600,6 +1620,7 @@ std::optional AddModelFromSdf( std::vector added_model_instances = AddModelsFromSpecification( workspace.diagnostic, model, model_name, {}, workspace.plant, + workspace.resolver, workspace.package_map, data_source.GetRootDir()); DRAKE_DEMAND(!added_model_instances.empty()); @@ -1613,7 +1634,8 @@ std::vector AddModelsFromSdf( DRAKE_THROW_UNLESS(!workspace.plant->is_finalized()); sdf::ParserConfig parser_config = MakeSdfParserConfig( - workspace.package_map, workspace.plant, test_sdf_forced_nesting); + workspace.package_map, workspace.plant, workspace.resolver, + test_sdf_forced_nesting); sdf::Root root; @@ -1648,6 +1670,7 @@ std::vector AddModelsFromSdf( std::vector added_model_instances = AddModelsFromSpecification( workspace.diagnostic, model, model.Name(), {}, workspace.plant, + workspace.resolver, workspace.package_map, data_source.GetRootDir()); model_instances.insert(model_instances.end(), added_model_instances.begin(), @@ -1675,6 +1698,7 @@ std::vector AddModelsFromSdf( std::vector added_model_instances = AddModelsFromSpecification( workspace.diagnostic, model, model.Name(), {}, workspace.plant, + workspace.resolver, workspace.package_map, data_source.GetRootDir()); model_instances.insert(model_instances.end(), added_model_instances.begin(), diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index e2391ed4d1d0..51dba2f16bde 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -311,10 +311,10 @@ void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) { attribute_name, &attribute_value); return attribute_value == std::string("true") ? true : false; }; - ParseCollisionFilterGroupCommon(model_instance_, node, w_.plant, - next_child_element, next_sibling_element, - has_attribute, get_string_attribute, - get_bool_attribute, get_string_attribute); + ParseCollisionFilterGroupCommon( + diagnostic_.MakePolicyForNode(node), model_instance_, node, w_.plant, + w_.resolver, next_child_element, next_sibling_element, has_attribute, + get_string_attribute, get_bool_attribute, get_string_attribute); } // Parses a joint URDF specification to obtain the names of the joint, parent diff --git a/multibody/parsing/parser.cc b/multibody/parsing/parser.cc index 3ece8e8518d4..7cbb8f048ab0 100644 --- a/multibody/parsing/parser.cc +++ b/multibody/parsing/parser.cc @@ -3,6 +3,7 @@ #include #include "drake/common/filesystem.h" +#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h" #include "drake/multibody/parsing/detail_common.h" #include "drake/multibody/parsing/detail_parsing_workspace.h" #include "drake/multibody/parsing/detail_sdf_parser.h" @@ -12,9 +13,11 @@ namespace drake { namespace multibody { using drake::internal::DiagnosticDetail; +using drake::internal::DiagnosticPolicy; using internal::AddModelFromSdf; using internal::AddModelFromUrdf; using internal::AddModelsFromSdf; +using internal::CollisionFilterGroupResolver; using internal::DataSource; using internal::ParsingWorkspace; @@ -58,27 +61,34 @@ FileType DetermineFileType(const std::string& file_name) { std::vector Parser::AddAllModelsFromFile( const std::string& file_name) { DataSource data_source(DataSource::kFilename, &file_name); - ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_}; + CollisionFilterGroupResolver resolver{plant_}; + ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_, + &resolver}; const FileType type = DetermineFileType(file_name); + std::vector result; if (type == FileType::kSdf) { - return AddModelsFromSdf(data_source, workspace); + result = AddModelsFromSdf(data_source, workspace); } else { const std::optional maybe_model = AddModelFromUrdf(data_source, {}, {}, workspace); if (maybe_model.has_value()) { - return {*maybe_model}; + result = {*maybe_model}; } else { throw std::runtime_error( fmt::format("{}: URDF model file parsing failed", file_name)); } } + resolver.Resolve(diagnostic_policy_); + return result; } ModelInstanceIndex Parser::AddModelFromFile( const std::string& file_name, const std::string& model_name) { DataSource data_source(DataSource::kFilename, &file_name); - ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_}; + CollisionFilterGroupResolver resolver{plant_}; + ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_, + &resolver}; const FileType type = DetermineFileType(file_name); std::optional maybe_model; if (type == FileType::kSdf) { @@ -90,6 +100,7 @@ ModelInstanceIndex Parser::AddModelFromFile( throw std::runtime_error( fmt::format("{}: parsing failed", file_name)); } + resolver.Resolve(diagnostic_policy_); return *maybe_model; } @@ -99,7 +110,9 @@ ModelInstanceIndex Parser::AddModelFromString( const std::string& model_name) { DataSource data_source(DataSource::kContents, &file_contents); const std::string pseudo_name(data_source.GetStem() + "." + file_type); - ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_}; + CollisionFilterGroupResolver resolver{plant_}; + ParsingWorkspace workspace{package_map_, diagnostic_policy_, plant_, + &resolver}; const FileType type = DetermineFileType(pseudo_name); std::optional maybe_model; if (type == FileType::kSdf) { @@ -111,6 +124,7 @@ ModelInstanceIndex Parser::AddModelFromString( throw std::runtime_error( fmt::format("{}: parsing failed", pseudo_name)); } + resolver.Resolve(diagnostic_policy_); return *maybe_model; } diff --git a/multibody/parsing/parsing_doxygen.h b/multibody/parsing/parsing_doxygen.h index e23628ba63b3..f2e530857f24 100644 --- a/multibody/parsing/parsing_doxygen.h +++ b/multibody/parsing/parsing_doxygen.h @@ -31,6 +31,47 @@ For Drake extensions to SDFormat files, see Drake's parser does not implement all of the features of SDFormat. In the future, we intend to update this documentation to itemize what isn't supported. +@subsection model_composition SDFormat model composition + +Drake's SDFormat parsing supports composing multiple models into a single +model, via lexical nesting and file inclusion. The file inclusion feature +supports both SDFormat files and URDF files. Note that included URDF files pass +through the Drake URDF parser, with all of it's extensions and limitations. + +For full details, see the +SDFormat documentation of model composition. + +An important feature of SDFormat model composition is the ability to +cross-reference features of other models. Cross-references are denoted by using +scoped names. + +@subsubsection scoped_names Scoped names + +Scoped names allow referring to an element of a model nested within the current +model. They take the form of some number of nested model names, plus the +element name, all joined by the delimiter `::`. Names not using the `::` +delimeter are not considered scoped names; they are used to define or refer to +elements of the current model. For example, suppose that model A contains model +B, which in turn contains model C. Here are some valid scoped names: + +- within A: + - `a_frame_in_A` + - `B::a_frame_in_B` + - `B::C::a_frame_in_C` +- within B: + - `a_frame_in_B` + - `C::a_frame_in_C` +- within C: + - `a_frame_in_C` + +Note that (deliberately) there is no way to refer to elements outward or upward +from the current model; all names are relative to the current model and can +only refer to the current or a nested model. In particular, names must not +start with `::`; there is no way to denote any "outer" or "outermost" scope. + +For a detailed design discussion with examples, see the +SDFormat documentation of name scoping. + @section multibody_parsing_urdf URDF Support Drake supports URDF files as described here: http://wiki.ros.org/urdf/XML. @@ -296,6 +337,9 @@ rules. If the `ignore` attribute is present and true-valued, the entire element is ignored. The nested elements must included one or more `drake:member` elements, and zero or more `drake:ignored_collision_filter_group` elements. +This element defines a new group name that is only available during parsing. It +must not be a scoped name. + Collision filtering rules are only constructed with **pairs** of groups, where both sides of the pair may name the same group. The `drake:ignored_collision_filter_group` element establishes a pairing between @@ -305,7 +349,7 @@ different collision groups excludes collisions between members of those groups naming the same group twice excludes collisions within the group (see drake::geometry::CollisionFilterDeclaration::ExcludeWithin()). -@see @ref tag_drake_member, @ref tag_drake_ignored_collision_filter_group +@see @ref tag_drake_member, @ref tag_drake_ignored_collision_filter_group, @ref scoped_names @subsection tag_drake_compliant_hydroelastic drake:compliant_hydroelastic @@ -438,7 +482,10 @@ under `(hydroelastic, hydroelastic_modulus)`. The string names a collision filter group that will be paired with the parent group when constructing filtering rules. It may name the parent group. -@see @ref tag_drake_collision_filter_group +In SDFormat files only, the name may refer to a group within a nested model +(either URDF or SDFormat) by using a scoped name. + +@see @ref tag_drake_collision_filter_group, @ref scoped_names @subsection tag_drake_joint drake:joint @@ -491,7 +538,10 @@ This element adds a drake::multibody::LinearBushingRollPitchYaw to the model. This element names a link (defined elsewhere in the model) to be a member of the parent collision filter group. -@see @ref tag_drake_collision_filter_group +In SDFormat files only, the name may refer to a link within a nested model +(either URDF or SDFormat) by using a scoped name. + +@see @ref tag_drake_collision_filter_group, @ref scoped_names @subsection tag_drake_mesh_resolution_hint drake:mesh_resolution_hint diff --git a/multibody/parsing/scoped_names.cc b/multibody/parsing/scoped_names.cc index 83805927d3b4..accf7927606c 100644 --- a/multibody/parsing/scoped_names.cc +++ b/multibody/parsing/scoped_names.cc @@ -4,7 +4,7 @@ namespace drake { namespace multibody { namespace parsing { -constexpr char const* kDelim = "::"; +using internal::kScopedNameDelim; const drake::multibody::Frame* GetScopedFrameByNameMaybe( @@ -34,7 +34,7 @@ std::string GetScopedFrameName( } ScopedName ParseScopedName(const std::string& full_name) { - size_t pos = full_name.rfind(kDelim); + size_t pos = full_name.rfind(kScopedNameDelim); ScopedName result; if (pos == std::string::npos) { result.name = full_name; @@ -42,7 +42,7 @@ ScopedName ParseScopedName(const std::string& full_name) { result.instance_name = full_name.substr(0, pos); // "Global scope" (e.g. "::my_frame") not supported. DRAKE_DEMAND(!result.instance_name.empty()); - result.name = full_name.substr(pos + std::string(kDelim).size()); + result.name = full_name.substr(pos + std::string(kScopedNameDelim).size()); } return result; } @@ -53,7 +53,7 @@ std::string PrefixName(const std::string& namespace_, const std::string& name) { else if (name.empty()) return namespace_; else - return namespace_ + kDelim + name; + return namespace_ + kScopedNameDelim + name; } std::string GetInstanceScopeName( diff --git a/multibody/parsing/scoped_names.h b/multibody/parsing/scoped_names.h index 33eb2ef06045..0fccf1fcec17 100644 --- a/multibody/parsing/scoped_names.h +++ b/multibody/parsing/scoped_names.h @@ -11,6 +11,11 @@ namespace drake { namespace multibody { namespace parsing { +namespace internal { +// Expose the delimeter string for reference, especially in tests. +constexpr char kScopedNameDelim[] = "::"; +} // namespace internal + /// Finds an optionally model-scoped frame, using the naming rules of /// `ParseScopedName`. diff --git a/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc b/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc new file mode 100644 index 000000000000..c015d24f32b3 --- /dev/null +++ b/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc @@ -0,0 +1,150 @@ +#include "drake/multibody/parsing/detail_collision_filter_group_resolver.h" + +#include +#include + +#include "drake/geometry/scene_graph.h" +#include "drake/multibody/parsing/test/diagnostic_policy_test_base.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace multibody { +namespace internal { +namespace { + +using ::testing::MatchesRegex; + +using geometry::SceneGraph; +using geometry::GeometryId; +using math::RigidTransformd; + +class CollisionFilterGroupResolverTest : public test::DiagnosticPolicyTestBase { + public: + CollisionFilterGroupResolverTest() { + plant_.RegisterAsSourceForSceneGraph(&scene_graph_); + } + + // Add a body to a model, with collision geometry and return the geometry id. + GeometryId AddBody(const std::string& name, + std::optional model_instance) { + auto model = model_instance.value_or(default_model_instance()); + return plant_.RegisterCollisionGeometry( + plant_.AddRigidBody(name, model, SpatialInertia()), + RigidTransformd::Identity(), geometry::Sphere(1.0), + name, CoulombFriction()); + } + + + protected: + MultibodyPlant plant_{0.0}; + SceneGraph scene_graph_; + const geometry::SceneGraphInspector& inspector_{ + scene_graph_.model_inspector()}; + internal::CollisionFilterGroupResolver resolver_{&plant_}; +}; + +// These tests concentrate on name resolution details and responses to errors +// in the parsed text. TODO(rpoyner-tri) Migrate actual filter construction +// tests here from detail_{urdf,sdf}_parser_test. + +TEST_F(CollisionFilterGroupResolverTest, IllegalPair) { + resolver_.AddPair(diagnostic_policy_, "::a", "b::", {}); + resolver_.Resolve(diagnostic_policy_); + EXPECT_THAT(TakeError(), MatchesRegex(".*'::a'.*neither begin nor end.*")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'b::'.*neither begin nor end.*")); +} + +TEST_F(CollisionFilterGroupResolverTest, BogusPairGlobal) { + ModelInstanceIndex r1 = plant_.AddModelInstance("r1"); + resolver_.AddPair(diagnostic_policy_, "a", "sub::b", r1); + resolver_.Resolve(diagnostic_policy_); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'r1::a'.*not found")); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'r1::sub::b'.*not found")); +} + +TEST_F(CollisionFilterGroupResolverTest, BogusPairScoped) { + resolver_.AddPair(diagnostic_policy_, "a", "b", {}); + resolver_.Resolve(diagnostic_policy_); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'a'.*not found")); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'b'.*not found")); +} + +TEST_F(CollisionFilterGroupResolverTest, IllegalGroupName) { + resolver_.AddGroup(diagnostic_policy_, "::a", {}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*'::a'.*neither begin nor end.*")); + resolver_.AddGroup(diagnostic_policy_, "b::", {}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*'b::'.*neither begin nor end.*")); +} + +TEST_F(CollisionFilterGroupResolverTest, IllegalBodies) { + resolver_.AddGroup(diagnostic_policy_, "g", {"::a", "b::"}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*'::a'.*neither begin nor end.*")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'b::'.*neither begin nor end.*")); +} + +TEST_F(CollisionFilterGroupResolverTest, BogusGroupName) { + resolver_.AddGroup(diagnostic_policy_, "haha::a", {}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*'haha::a' cannot be.*scoped.*")); +} + +TEST_F(CollisionFilterGroupResolverTest, EmptyGroupGlobal) { + resolver_.AddGroup(diagnostic_policy_, "a", {}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'a'.*no members")); +} + +TEST_F(CollisionFilterGroupResolverTest, EmptyGroupsScoped) { + ModelInstanceIndex r1 = plant_.AddModelInstance("r1"); + resolver_.AddGroup(diagnostic_policy_, "a", {}, r1); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'r1::a'.*no members")); + ModelInstanceIndex r2 = plant_.AddModelInstance("r2"); + resolver_.AddGroup(diagnostic_policy_, "b", {}, r2); + EXPECT_THAT(TakeError(), MatchesRegex(".*group.*'r2::b'.*no members")); +} + +TEST_F(CollisionFilterGroupResolverTest, MissingBodyGlobal) { + plant_.AddModelInstance("r1"); + resolver_.AddGroup(diagnostic_policy_, "a", + {"abody", "r1::abody", "zzz::abody"}, {}); + EXPECT_THAT(TakeError(), MatchesRegex(".*'abody'.*not found")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'r1::abody'.*not found")); + // Note that AddGroup() doesn't choke on an instance name that doesn't exist. + EXPECT_THAT(TakeError(), MatchesRegex(".*'zzz::abody'.*not found")); +} + +TEST_F(CollisionFilterGroupResolverTest, MissingBodyScoped) { + ModelInstanceIndex r1 = plant_.AddModelInstance("r1"); + plant_.AddModelInstance("r1::sub"); + resolver_.AddGroup(diagnostic_policy_, "a", + {"abody", "sub::abody", "zzz::abody"}, r1); + EXPECT_THAT(TakeError(), MatchesRegex(".*'r1::abody'.*not found")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'r1::sub::abody'.*not found")); + // Note that AddGroup() doesn't choke on an instance name that doesn't exist. + 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", {}); + resolver_.Resolve(diagnostic_policy_); + EXPECT_TRUE(inspector_.CollisionFiltered(b1, b2)); +} + +TEST_F(CollisionFilterGroupResolverTest, LinkScoped) { + // Ensure body names can be resolved in various model scopes. + ModelInstanceIndex r1 = plant_.AddModelInstance("r1"); + GeometryId ra = AddBody("abody", r1); + ModelInstanceIndex sub = plant_.AddModelInstance("r1::sub"); + GeometryId rsa = AddBody("abody", sub); + resolver_.AddGroup(diagnostic_policy_, "a", {"abody", "sub::abody"}, r1); + resolver_.AddPair(diagnostic_policy_, "a", "a", r1); + resolver_.Resolve(diagnostic_policy_); + EXPECT_TRUE(inspector_.CollisionFiltered(ra, rsa)); +} + +} // namespace +} // namespace internal +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 5a18c79fdfec..41c4ed6bbcfc 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -71,6 +71,7 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ std::optional result = AddModelFromSdf( data_source, model_name, w_, test_sdf_forced_nesting); EXPECT_TRUE(result.has_value()); + resolver_.Resolve(diagnostic_policy_); return result.value_or(ModelInstanceIndex{}); } @@ -78,15 +79,19 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ const std::string& file_name, bool test_sdf_forced_nesting = false) { const DataSource data_source{DataSource::kFilename, &file_name}; - return AddModelsFromSdf( + auto result = AddModelsFromSdf( data_source, w_, test_sdf_forced_nesting); + resolver_.Resolve(diagnostic_policy_); + return result; } std::vector AddModelsFromSdfString( const std::string& file_contents, bool test_sdf_forced_nesting = false) { const DataSource data_source{DataSource::kContents, &file_contents}; - return AddModelsFromSdf(data_source, w_, test_sdf_forced_nesting); + auto result = AddModelsFromSdf(data_source, w_, test_sdf_forced_nesting); + resolver_.Resolve(diagnostic_policy_); + return result; } void ParseTestString(const std::string& inner, @@ -127,12 +132,35 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ return warning_records_[0].FormatWarning(); } + // Returns all of the ids, sorted by name, in a vector adjusted for 1-based + // indexing. + std::vector SortGeometryIds() { + const geometry::SceneGraphInspector& inspector = + scene_graph_.model_inspector(); + // Get the unsorted geometry ids. + const std::vector unsorted = inspector.GetAllGeometryIds(); + // Use a map to get them in order by name. + std::map by_name; + for (const auto& gid : unsorted) { + by_name[inspector.GetName(gid)] = gid; + } + // Extract the in-order ids to a vector; allow 1-based indices. + int num_geometries = inspector.num_geometries(); + std::vector ids(1 + num_geometries); + int index{1}; + for (const auto& pair : by_name) { + ids[index++] = pair.second; + } + return ids; + } + protected: PackageMap package_map_; DiagnosticPolicy diagnostic_; MultibodyPlant plant_{0.0}; SceneGraph scene_graph_; - ParsingWorkspace w_{package_map_, diagnostic_policy_, &plant_}; + internal::CollisionFilterGroupResolver resolver_{&plant_}; + ParsingWorkspace w_{package_map_, diagnostic_policy_, &plant_, &resolver_}; }; const Frame& GetModelFrameByName(const MultibodyPlant& plant, @@ -2129,6 +2157,7 @@ TEST_F(SdfParserTest, FramesAsJointParentOrChild) { // SDFormat files can be forced to be loaded via the Interface API by changing // their file extension and registering the appropriate custom parser. TEST_F(SdfParserTest, InterfaceAPI) { + AddSceneGraph(); const std::string sdf_file_path = FindResourceOrThrow( "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" "top.sdf"); @@ -2138,6 +2167,51 @@ TEST_F(SdfParserTest, InterfaceAPI) { DRAKE_ASSERT_NO_THROW(AddModelFromSdfFile(sdf_file_path, "", true)); + // Test collision filtering across models. Done before Finalize() to exclude + // default filters. + { + const geometry::SceneGraphInspector& inspector = + scene_graph_.model_inspector(); + static constexpr int kNumLinks = 7; + // Verify the number we expect and that they are all in proximity role. + ASSERT_EQ(kNumLinks, inspector.num_geometries()); + ASSERT_EQ(kNumLinks, + inspector.NumGeometriesWithRole(geometry::Role::kProximity)); + std::vector ids = SortGeometryIds(); + ASSERT_EQ(ids.size(), 1 + kNumLinks); + + // Make sure the plant is not finalized such that the Finalize() default + // filtering has not taken into effect yet. This guarantees that the + // collision filtering is applied due to the collision filter group parsing. + ASSERT_FALSE(plant_.is_finalized()); + + // Verify filtering among all links. + for (int m = 1; m <= kNumLinks; ++m) { + const std::string& m_name = inspector.GetName(ids[m]); + for (int n = m; n <= kNumLinks; ++n) { + const std::string& n_name = inspector.GetName(ids[n]); + SCOPED_TRACE(fmt::format("{}[{}] vs {}[{}]", m_name, m, n_name, n)); + if ((m == n) || + // top::arm::L1[1] vs top::arm::gripper::gripper_link[4] + (m == 1 && n == 4) || + // top::arm::L1[1] vs top::extra_arm::L2[6] + (m == 1 && n == 6) || + // top::arm::L1[1] vs top::torso[7] + (m == 1 && n == 7) || + // top::arm::gripper::gripper_link[4] vs top::extra_arm::L2[6] + (m == 4 && n == 6) || + // top::arm::gripper::gripper_link[4] vs top::torso[7] + (m == 4 && n == 7) || + // top::extra_arm::L2[6] vs top::torso[7] + (m == 6 && n == 7)) { + EXPECT_TRUE(inspector.CollisionFiltered(ids[m], ids[n])); + } else { + EXPECT_FALSE(inspector.CollisionFiltered(ids[m], ids[n])); + } + } + } + } + plant_.Finalize(); auto context = plant_.CreateDefaultContext(); @@ -2274,95 +2348,117 @@ TEST_F(SdfParserTest, ErrorsFromIncludedNestingSdf) { TEST_F(SdfParserTest, CollisionFilterGroupParsingTest) { const std::string full_sdf_filename = FindResourceOrThrow( "drake/multibody/parsing/test/" - "sdf_parser_test/collision_filter_group_parsing_test.sdf"); + "sdf_parser_test/collision_filter_group_parsing_test/test.sdf"); AddSceneGraph(); // Read in the SDF file. + package_map_.PopulateFromFolder( + filesystem::path(full_sdf_filename).parent_path()); AddModelFromSdfFile(full_sdf_filename, ""); // Get geometry ids for all the bodies. const geometry::SceneGraphInspector& inspector = scene_graph_.model_inspector(); - static constexpr int kNumLinks = 6; - std::vector ids(1 + kNumLinks); // allow 1-based indices. - for (int k = 1; k <= 6; ++k) { - const auto geometry_id = inspector.GetGeometryIdByName( - plant_.GetBodyFrameIdOrThrow( - plant_.GetBodyByName(fmt::format("link{}", k)).index()), - geometry::Role::kProximity, - fmt::format("collision_filter_group_parsing_test::link{}_sphere", k)); - ids[k] = geometry_id; + static constexpr int kNumRobots = 2; + static constexpr int kNumLinksPerRobot = 6; + static constexpr int kNumLinks = kNumLinksPerRobot * kNumRobots; + // Verify the number we expect and that they are all in proximity role. + ASSERT_EQ(kNumLinks, inspector.num_geometries()); + ASSERT_EQ(kNumLinks, + inspector.NumGeometriesWithRole(geometry::Role::kProximity)); + std::vector ids = SortGeometryIds(); + ASSERT_EQ(ids.size(), 1 + kNumLinks); + + // Make sure the plant is not finalized such that the Finalize() default + // filtering has not taken into effect yet. This guarantees that the + // collision filtering is applied due to the collision filter group parsing. + ASSERT_FALSE(plant_.is_finalized()); + + // Verify filtering local to the nested robots. + for (int k = 0; k < kNumRobots; ++k) { + int offset = k * kNumLinksPerRobot; + // We have six geometries and 15 possible pairs, each with a particular + // disposition. + // (1, 2) - unfiltered + EXPECT_FALSE(inspector.CollisionFiltered(ids[1 + offset], ids[2 + offset])); + // (1, 3) - filtered by group_link3 ignores group_link14 + EXPECT_TRUE(inspector.CollisionFiltered(ids[1 + offset], ids[3 + offset])); + // (1, 4) - filtered by group_link14 ignores itself + EXPECT_TRUE(inspector.CollisionFiltered(ids[1 + offset], ids[4 + offset])); + // (1, 5) - unfiltered + EXPECT_FALSE(inspector.CollisionFiltered(ids[1 + offset], ids[5 + offset])); + // (1, 6) - unfiltered + EXPECT_FALSE(inspector.CollisionFiltered(ids[1 + offset], ids[6 + offset])); + // (2, 3) - filtered by group_link2 ignores group_link3 + EXPECT_TRUE(inspector.CollisionFiltered(ids[2 + offset], ids[3 + offset])); + // (2, 4) - unfiltered (although declared in an *ignored* self-filtering + // group_link24). + EXPECT_FALSE(inspector.CollisionFiltered(ids[2 + offset], ids[4 + offset])); + // (2, 5) - filtered by group_link56 ignored group_link2 + EXPECT_TRUE(inspector.CollisionFiltered(ids[2 + offset], ids[5 + offset])); + // (2, 6) - filtered by group_link56 ignored group_link2 + EXPECT_TRUE(inspector.CollisionFiltered(ids[2 + offset], ids[6 + offset])); + // (3, 4) - filtered by group_link3 ignores group_link14 + EXPECT_TRUE(inspector.CollisionFiltered(ids[3 + offset], ids[4 + offset])); + // (3, 5) - filtered by group_link56 ignored group_link3 + EXPECT_TRUE(inspector.CollisionFiltered(ids[3 + offset], ids[5 + offset])); + // (3, 6) - filtered by group_link56 ignored group_link3 + EXPECT_TRUE(inspector.CollisionFiltered(ids[3 + offset], ids[6 + offset])); + // (4, 5) - unfiltered + EXPECT_FALSE(inspector.CollisionFiltered(ids[4 + offset], ids[5 + offset])); + // (4, 6) - unfiltered + EXPECT_FALSE(inspector.CollisionFiltered(ids[4 + offset], ids[6 + offset])); + // (5, 6) - filtered by group_link56 ignores itself + EXPECT_TRUE(inspector.CollisionFiltered(ids[5 + offset], ids[6 + offset])); } - // Make sure the plant is not finalized such that the adjacent joint filter - // has not taken into effect yet. This guarantees that the collision filtering - // is applied due to the collision filter group parsing. - ASSERT_FALSE(plant_.is_finalized()); + // Verify filtering spanning multiple robots. + for (int m = 1; m <= kNumLinksPerRobot; ++m) { + for (int n = 1; n <= kNumLinksPerRobot; ++n) { + SCOPED_TRACE(fmt::format("robot1::link{} vs robot2::link{}", m, n)); + if ((m == 3 && n == 3) || // group_3s + (m == 6 && n == 6)) { // group_6s + EXPECT_TRUE(inspector.CollisionFiltered( + ids[m], ids[n + kNumLinksPerRobot])); + } else { + EXPECT_FALSE(inspector.CollisionFiltered( + ids[m], ids[n + kNumLinksPerRobot])); + } + } + } - // We have six geometries and 15 possible pairs, each with a particular - // disposition. - // (1, 2) - unfiltered - EXPECT_FALSE(inspector.CollisionFiltered(ids[1], ids[2])); - // (1, 3) - filtered by group_link3 ignores group_link14 - EXPECT_TRUE(inspector.CollisionFiltered(ids[1], ids[3])); - // (1, 4) - filtered by group_link14 ignores itself - EXPECT_TRUE(inspector.CollisionFiltered(ids[1], ids[4])); - // (1, 5) - unfiltered - EXPECT_FALSE(inspector.CollisionFiltered(ids[1], ids[5])); - // (1, 6) - unfiltered - EXPECT_FALSE(inspector.CollisionFiltered(ids[1], ids[6])); - // (2, 3) - filtered by group_link2 ignores group_link3 - EXPECT_TRUE(inspector.CollisionFiltered(ids[2], ids[3])); - // (2, 4) - unfiltered (although declared in an *ignored* self-filtering - // group_link24). - EXPECT_FALSE(inspector.CollisionFiltered(ids[2], ids[4])); - // (2, 5) - filtered by group_link56 ignored group_link2 - EXPECT_TRUE(inspector.CollisionFiltered(ids[2], ids[5])); - // (2, 6) - filtered by group_link56 ignored group_link2 - EXPECT_TRUE(inspector.CollisionFiltered(ids[2], ids[6])); - // (3, 4) - filtered by group_link3 ignores group_link14 - EXPECT_TRUE(inspector.CollisionFiltered(ids[3], ids[4])); - // (3, 5) - filtered by group_link56 ignored group_link3 - EXPECT_TRUE(inspector.CollisionFiltered(ids[3], ids[5])); - // (3, 6) - filtered by group_link56 ignored group_link3 - EXPECT_TRUE(inspector.CollisionFiltered(ids[3], ids[6])); - // (4, 5) - unfiltered - EXPECT_FALSE(inspector.CollisionFiltered(ids[4], ids[5])); - // (4, 6) - unfiltered - EXPECT_FALSE(inspector.CollisionFiltered(ids[4], ids[6])); - // (5, 6) - filtered by group_link56 ignores itself - EXPECT_TRUE(inspector.CollisionFiltered(ids[5], ids[6])); // Make sure we can add the model a second time. AddModelFromSdfFile(full_sdf_filename, "model2"); } -// TODO(marcoag) We might want to add some form of feedback for: -// - ignore_collision_filter_groups with non-existing group names. -// - Empty collision_filter_groups. TEST_F(SdfParserTest, CollisionFilterGroupParsingErrorsTest) { AddSceneGraph(); - DRAKE_EXPECT_THROWS_MESSAGE( + DRAKE_EXPECT_NO_THROW( ParseTestString(R"""( -)"""), - ".*The tag is " - "missing the required attribute " - "\"name\".*"); +)""")); + EXPECT_THAT(TakeError(), MatchesRegex( + ".*The tag is " + "missing the required attribute \"name\".*")); + FlushDiagnostics(); - DRAKE_EXPECT_THROWS_MESSAGE( + DRAKE_EXPECT_NO_THROW( ParseTestString(R"""( -)"""), - ".*The tag is missing a required string value.*"); +)""")); + EXPECT_THAT(TakeError(), MatchesRegex(".*The tag is missing" + " a required string value.*")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'error2::group_a'.*no members")); + FlushDiagnostics(); - DRAKE_EXPECT_THROWS_MESSAGE( + DRAKE_EXPECT_NO_THROW( ParseTestString(R"""( @@ -2370,9 +2466,11 @@ TEST_F(SdfParserTest, CollisionFilterGroupParsingErrorsTest) { -)"""), - ".*The tag is missing a " - "required string value.*"); +)""")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'error3::group_a'.*no members")); + EXPECT_THAT(TakeError(), MatchesRegex( + ".*The tag is missing" + " a required string value.*")); } TEST_F(SdfParserTest, PoseWithRotationInDegreesOrQuaternions) { diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc index ccd253fa3ae9..477b3ba3da47 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -49,21 +49,27 @@ class UrdfParserTest : public test::DiagnosticPolicyTestBase { std::optional AddModelFromUrdfFile( const std::string& file_name, const std::string& model_name) { - return AddModelFromUrdf( + auto result = AddModelFromUrdf( {DataSource::kFilename, &file_name}, model_name, {}, w_); + resolver_.Resolve(diagnostic_policy_); + return result; } std::optional AddModelFromUrdfString( const std::string& file_contents, const std::string& model_name) { - return AddModelFromUrdf( + auto result = AddModelFromUrdf( {DataSource::kContents, &file_contents}, model_name, {}, w_); + resolver_.Resolve(diagnostic_policy_); + return result; } + protected: PackageMap package_map_; MultibodyPlant plant_{0.0}; SceneGraph scene_graph_; - ParsingWorkspace w_{package_map_, diagnostic_policy_, &plant_}; + internal::CollisionFilterGroupResolver resolver_{&plant_}; + ParsingWorkspace w_{package_map_, diagnostic_policy_, &plant_, &resolver_}; }; // Some tests contain deliberate typos to provoke parser errors or warnings. In @@ -1337,10 +1343,6 @@ TEST_F(UrdfParserTest, CollisionFilterGroupParsingTest) { AddModelFromUrdfFile(full_name, "model2"); } -// TODO(marcoag) We might want to add some form of feedback for: -// - ignore_collision_filter_groups with non-existing group names. -// - Empty collision_filter_groups. - TEST_F(UrdfParserTest, CollisionFilterGroupMissingName) { EXPECT_NE(AddModelFromUrdfString(R"""( @@ -1363,6 +1365,7 @@ TEST_F(UrdfParserTest, CollisionFilterGroupMissingLink) { EXPECT_THAT(TakeError(), MatchesRegex( ".*The tag does not specify the required " "attribute \"link\".")); + EXPECT_THAT(TakeError(), MatchesRegex(".*'robot::group_a'.*no members")); } TEST_F(UrdfParserTest, IgnoredCollisionFilterGroupMissingName) { @@ -1373,6 +1376,7 @@ TEST_F(UrdfParserTest, IgnoredCollisionFilterGroupMissingName) { )""", ""), std::nullopt); + EXPECT_THAT(TakeError(), MatchesRegex(".*'robot::group_a'.*no members")); EXPECT_THAT(TakeError(), MatchesRegex( ".*The tag does not" " specify the required attribute \"name\".")); diff --git a/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/package.xml b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/package.xml new file mode 100644 index 000000000000..e5f3af5d379f --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/package.xml @@ -0,0 +1,5 @@ + + collision_filter_group_parsing_test + 0.0.1 + + diff --git a/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test.sdf b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/robot.sdf similarity index 98% rename from multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test.sdf rename to multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/robot.sdf index 844970437feb..eba004ac8b5d 100644 --- a/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test.sdf +++ b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/robot.sdf @@ -1,6 +1,6 @@ - + diff --git a/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/test.sdf b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/test.sdf new file mode 100644 index 000000000000..6bd54ebf4d1a --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/collision_filter_group_parsing_test/test.sdf @@ -0,0 +1,24 @@ + + + + + robot1 + package://collision_filter_group_parsing_test/robot.sdf + + + robot2 + package://collision_filter_group_parsing_test/robot.sdf + + + + robot1::link6 + robot2::link6 + group_6s + + + + robot1::link3 + robot2::group_link3 + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf index 02ac14c7938e..e122e9fa1c71 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf @@ -1,7 +1,15 @@ - + + + + + 0.4 + + + + 0 0 1 0 0 0 L1 @@ -13,10 +21,26 @@ - + + + + + 0.4 + + + + 0 2 1 0 0 0 - + + + + + 0.4 + + + + 0 0 2 0.1 0.2 0.3 diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf index 6cb6691cc23e..39ac36b7060b 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf @@ -1,10 +1,22 @@ - + + + + + + + - + + + + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf index c2a2110e5718..f76b864a7093 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf @@ -1,7 +1,15 @@ - + + + + + 0.4 + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf index 2affd9444fc6..226d7de11c6a 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf @@ -1,7 +1,15 @@ - + - + + + + + 0.4 + + + + @@ -35,5 +43,12 @@ package://interface_api_test/table_and_mug.forced_nesting_sdf table_and_mug + + torso + arm::L1 + extra_arm::L2 + arm::gripper::gripper_link + g1 +