From 11d7b7be55943147c12005f79ddbc3ec3efd9a55 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 1 Jul 2021 13:46:25 -0500 Subject: [PATCH] parsing: Use libsdformat's Interface API to parse URDF files --- multibody/parsing/BUILD.bazel | 3 + multibody/parsing/detail_ignition.cc | 7 + multibody/parsing/detail_ignition.h | 4 + multibody/parsing/detail_sdf_parser.cc | 356 ++++++++++++++++-- multibody/parsing/detail_sdf_parser.h | 14 +- multibody/parsing/detail_urdf_parser.cc | 8 +- multibody/parsing/detail_urdf_parser.h | 7 + multibody/parsing/parser.cc | 6 +- .../parsing/test/detail_sdf_parser_test.cc | 248 +++++++++++- .../parsing/test/detail_urdf_parser_test.cc | 2 +- .../frames_as_joint_parent_or_child.sdf | 40 ++ .../interface_api_test/arm.forced_nesting_sdf | 35 ++ .../interface_api_test/arm.urdf | 10 + .../interface_api_test/gripper.sdf | 8 + .../interface_api_test/mug.sdf | 10 + .../interface_api_test/package.xml | 5 + .../interface_api_test/table.sdf | 10 + .../table_and_mug.forced_nesting_sdf | 24 ++ .../interface_api_test/top.sdf | 39 ++ 19 files changed, 790 insertions(+), 46 deletions(-) create mode 100644 multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/mug.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/package.xml create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/table.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel index 4066489166a0..7c4ce39faf15 100644 --- a/multibody/parsing/BUILD.bazel +++ b/multibody/parsing/BUILD.bazel @@ -15,6 +15,7 @@ filegroup( "test/**/*.config", "test/**/*.obj", "test/**/*.sdf", + "test/**/*.forced_nesting_sdf", "test/**/*.urdf", "test/**/*.xml", "test/**/*.png", @@ -129,6 +130,7 @@ drake_cc_library( deps = [ ":detail_misc", ":detail_scene_graph", + ":detail_urdf_parser", ":scoped_names", "//multibody/plant", "@sdformat", @@ -151,6 +153,7 @@ drake_cc_library( deps = [ ":detail_misc", ":package_map", + ":scoped_names", "//multibody/plant", "@fmt", "@tinyxml2", diff --git a/multibody/parsing/detail_ignition.cc b/multibody/parsing/detail_ignition.cc index ac715cada252..50d9d0988c4f 100644 --- a/multibody/parsing/detail_ignition.cc +++ b/multibody/parsing/detail_ignition.cc @@ -17,6 +17,13 @@ RigidTransformd ToRigidTransform(const ignition::math::Pose3d& pose) { return RigidTransformd(rotation, ToVector3(pose.Pos()));; } +ignition::math::Pose3d ToIgnPose3d(const RigidTransformd& pose) { + const auto& quat = pose.rotation().ToQuaternion(); + return ignition::math::Pose3d( + ignition::math::Vector3d(pose.translation().x(), pose.translation().y(), + pose.translation().z()), + ignition::math::Quaterniond(quat.w(), quat.x(), quat.y(), quat.z())); +} } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/parsing/detail_ignition.h b/multibody/parsing/detail_ignition.h index d22a657883ca..0fb4a0dacabd 100644 --- a/multibody/parsing/detail_ignition.h +++ b/multibody/parsing/detail_ignition.h @@ -17,6 +17,10 @@ Eigen::Vector3d ToVector3(const ignition::math::Vector3d& vector); // a RigidTransform instance. math::RigidTransformd ToRigidTransform(const ignition::math::Pose3d& pose); +// Helper function to express a RigidTransform instance as an +// ignition::math::Pose3d instance. +ignition::math::Pose3d ToIgnPose3d(const math::RigidTransformd& pose); + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index b3d22f1f2b2f..606495ec62fb 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -1,6 +1,7 @@ #include "drake/multibody/parsing/detail_sdf_parser.h" #include +#include #include #include #include @@ -9,6 +10,7 @@ #include #include +#include #include "drake/geometry/geometry_instance.h" #include "drake/math/rigid_transform.h" @@ -16,6 +18,7 @@ #include "drake/multibody/parsing/detail_ignition.h" #include "drake/multibody/parsing/detail_path_utils.h" #include "drake/multibody/parsing/detail_scene_graph.h" +#include "drake/multibody/parsing/detail_urdf_parser.h" #include "drake/multibody/parsing/scoped_names.h" #include "drake/multibody/tree/ball_rpy_joint.h" #include "drake/multibody/tree/fixed_offset_frame.h" @@ -83,6 +86,44 @@ std::pair GetResolvedModelInstanceAndLocalName( return {resolved_model_instance, unscoped_local_name}; } +// Calculates the relative name of a body relative to the model instance @p +// relative_to_model_instance. If the body is a direct child of the model, +// this simply returns the local name of the body. However, if the body is +// a child of a nested model, the local name of the body is prefixed with the +// relative name of the nested model. +std::string GetRelativeBodyName( + const Body& body, + ModelInstanceIndex relative_to_model_instance, + const MultibodyPlant& plant) { + const std::string& relative_to_model_absolute_name = + plant.GetModelInstanceName(relative_to_model_instance); + // If the body is inside a nested model, we need to prefix the + // name with the relative name of the nested model. + if (body.model_instance() != relative_to_model_instance) { + const std::string& nested_model_absolute_name = + plant.GetModelInstanceName(body.model_instance()); + // The relative_to_model_absolute_name must be a prefix of the + // nested_model_absolute_name. Otherwise the nested model is not a + // descendent of the model relative to which we are computing the name. + DRAKE_DEMAND(0 == nested_model_absolute_name.compare( + 0, relative_to_model_absolute_name.size(), + relative_to_model_absolute_name)); + + DRAKE_DEMAND(nested_model_absolute_name.size() > + relative_to_model_absolute_name.size() + + sdf::kSdfScopeDelimiter.size()); + + const std::string nested_model_relative_name = + nested_model_absolute_name.substr( + relative_to_model_absolute_name.size() + + sdf::kSdfScopeDelimiter.size()); + + return sdf::JoinName(nested_model_relative_name, body.name()); + } else { + return body.name(); + } +} + // Given an ignition::math::Inertial object, extract a RotationalInertia object // for the rotational inertia of body B, about its center of mass Bcm and, // expressed in the inertial frame Bi (as specified in in the SDF @@ -415,8 +456,9 @@ void AddJointFromSpecification( // Get the pose of frame J in the frame of the child link C, as specified in // ... . The default `relative_to` pose of a // joint will be the child link. - const RigidTransformd X_CJ = - ResolveRigidTransform(joint_spec.SemanticPose()); + const RigidTransformd X_CJ = ResolveRigidTransform( + joint_spec.SemanticPose(), + GetRelativeBodyName(child_body, model_instance, *plant)); // Pose of the frame J in the parent body frame P. std::optional X_PJ; @@ -429,7 +471,8 @@ void AddJointFromSpecification( X_PJ = X_WM * X_MJ; // Since P == W. } else { X_PJ = ResolveRigidTransform( - joint_spec.SemanticPose(), joint_spec.ParentLinkName()); + joint_spec.SemanticPose(), + GetRelativeBodyName(parent_body, model_instance, *plant)); } // If P and J are coincident, we won't create a new frame for J, but use frame @@ -508,29 +551,21 @@ void AddJointFromSpecification( joint_types->insert(joint_spec.Type()); } +sdf::InterfaceModelPtr ParseNestedInterfaceModel( + MultibodyPlant* plant, const PackageMap& package_map, + const sdf::NestedInclude& include, sdf::Errors* errors, + const sdf::ParserConfig& parser_config, + bool test_sdf_forced_nesting = false); + // Helper method to load an SDF file and read the contents into an sdf::Root // object. std::string LoadSdf( sdf::Root* root, const DataSource& data_source, - const PackageMap& package_map) { + const sdf::ParserConfig& parser_config) { data_source.DemandExactlyOne(); std::string root_dir; - // TODO(marcoag) ensure that we propagate the right ParserConfig instance. - sdf::ParserConfig parser_config; - parser_config.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); - parser_config.SetDeprecatedElementsPolicy(sdf::EnforcementPolicy::WARN); - parser_config.SetFindCallback( - [=](const std::string &_input) { - return ResolveUri(_input, package_map, "."); - }); - // TODO(#15018): This means that unrecognized elements won't be shown to a - // user directly (via console or exception). We should change unrecognized - // elements policy to print a warning, and later become an error. - DRAKE_DEMAND( - parser_config.UnrecognizedElementsPolicy() - == sdf::EnforcementPolicy::LOG); if (data_source.file_name) { const std::string full_path = GetFullPath(*data_source.file_name); @@ -686,7 +721,31 @@ const Frame& AddFrameFromSpecification( const auto [parent_model_instance, local_name] = GetResolvedModelInstanceAndLocalName(frame_spec.AttachedTo(), model_instance, *plant); - parent_frame = &plant->GetFrameByName(local_name, parent_model_instance); + if (plant->HasFrameNamed(local_name, parent_model_instance)) { + parent_frame = + &plant->GetFrameByName(local_name, parent_model_instance); + } else { + // If there is no frame named `local_name`, the `attached_to` attribute + // must be pointing to something we don't create implicit frames for in + // Drake. Currently these are models and joints. Models are handled in + // the first `if` block, so we're dealing with joints here. Since joints + // may end up in a model instance different from the model in which they + // were defined, we don't bother to find the joint and use its child + // frame. Instead we ask libsdformat to resolve the body associated with + // whaterver is referenced by the `attached_to` attribute. Since this is + // a body, we're assured that its implicit frame exists in the plant. + std::string resolved_attached_to_body_name; + ThrowAnyErrors( + frame_spec.ResolveAttachedToBody(resolved_attached_to_body_name)); + + const std::string resolved_attached_to_body_absolute_name = + parsing::PrefixName( + parsing::GetInstanceScopeName(*plant, model_instance), + resolved_attached_to_body_name); + parent_frame = parsing::GetScopedFrameByNameMaybe( + *plant, resolved_attached_to_body_absolute_name); + DRAKE_DEMAND(nullptr != parent_frame); + } } } const Frame& frame = @@ -934,6 +993,249 @@ std::vector AddModelsFromSpecification( return added_model_instances; } +bool EndsWith(const std::string& input, const std::string & ext) { + if (ext.size() > input.size()) { + return false; + } + return input.compare(input.size() - ext.size(), ext.size(), ext) == 0; +} + +// Helper function that computes the default pose of a Frame +RigidTransformd GetDefaultFramePose( + const MultibodyPlant& plant, + const Frame& frame) { + const RigidTransformd X_WB = + plant.GetDefaultFreeBodyPose(frame.body()); + const RigidTransformd X_WF = + X_WB * frame.GetFixedPoseInBodyFrame(); + return X_WF; +} + +// For the libsdformat API, see: +// http://sdformat.org/tutorials?tut=composition_proposal +constexpr char kExtUrdf[] = ".urdf"; + +// To test re-parsing an SDFormat document, but in complete isolation. Tests +// out separate model formats. +constexpr char kExtForcedNesting[] = ".forced_nesting_sdf"; + +void AddBodiesToInterfaceModel(const MultibodyPlant& plant, + ModelInstanceIndex model_instance, + const sdf::InterfaceModelPtr& interface_model) { + const auto& model_frame = plant.GetFrameByName("__model__", model_instance); + const RigidTransformd X_MW = + GetDefaultFramePose(plant, model_frame).inverse(); + for (auto index : plant.GetBodyIndices(model_instance)) { + const auto& link = plant.get_body(index); + RigidTransformd X_ML = X_MW * plant.GetDefaultFreeBodyPose(link); + interface_model->AddLink({link.name(), ToIgnPose3d(X_ML)}); + } +} + +void AddFramesToInterfaceModel(const MultibodyPlant& plant, + ModelInstanceIndex model_instance, + const sdf::InterfaceModelPtr& interface_model) { + for (FrameIndex index(0); index < plant.num_frames(); ++index) { + const auto& frame = plant.get_frame(index); + if (frame.model_instance() != model_instance) { + continue; + } + if (frame.name().empty() || frame.name() == "__model__" || + plant.HasBodyNamed(frame.name(), model_instance)) { + // Skip unnamed frames, and __model__ since it's already added. Also + // skip frames with the same name as a link since those are frames added + // by Drake and are considered implicit by SDFormat. Sending such frames + // to SDFormat would imply that these frames are explicit (i.e frames + // created using the tag). + continue; + } + interface_model->AddFrame( + {frame.name(), GetRelativeBodyName(frame.body(), model_instance, plant), + ToIgnPose3d(frame.GetFixedPoseInBodyFrame())}); + } +} + +void AddJointsToInterfaceModel(const MultibodyPlant& plant, + ModelInstanceIndex model_instance, + const sdf::InterfaceModelPtr& interface_model) { + for (auto index : plant.GetJointIndices(model_instance)) { + const auto& joint = plant.get_joint(index); + const std::string& child_name = joint.child_body().name(); + const RigidTransformd X_CJ = + joint.frame_on_child().GetFixedPoseInBodyFrame(); + interface_model->AddJoint({joint.name(), child_name, ToIgnPose3d(X_CJ)}); + } +} + +// This assumes that parent models will have their parsing start before child +// models! This is a safe assumption because we only encounter nested models +// when force testing SDFormat files and libsdformat parses models in a top-down +// order. If we add support for other file formats, we should ensure that the +// parsers comply with this assumption. +sdf::InterfaceModelPtr ParseNestedInterfaceModel( + MultibodyPlant* plant, const PackageMap& package_map, + const sdf::NestedInclude& include, sdf::Errors* errors, + const sdf::ParserConfig& parser_config, bool 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); + const bool is_forced_nesting = + test_sdf_forced_nesting && + EndsWith(include.resolvedFileName, kExtForcedNesting); + if (!is_urdf && !is_forced_nesting) { + return nullptr; + } + + if (include.isStatic) { + errors->emplace_back( + sdf::ErrorCode::ELEMENT_INVALID, + "Drake does not yet support //include/static for custom nesting."); + return nullptr; + } + + DataSource data_source; + data_source.file_name = &include.resolvedFileName; + + ModelInstanceIndex main_model_instance; + // New instances will have indices starting from cur_num_models + int cur_num_models = plant->num_model_instances(); + if (is_urdf) { + main_model_instance = + AddModelFromUrdf(data_source, include.localModelName.value_or(""), + include.absoluteParentName, package_map, plant); + // Add explicit model frame to first link. + auto body_indices = plant->GetBodyIndices(main_model_instance); + if (body_indices.empty()) { + errors->emplace_back(sdf::ErrorCode::ELEMENT_INVALID, + "URDF must have at least one link."); + return nullptr; + } + const auto& canonical_link = plant->get_body(body_indices[0]); + + const Frame& canonical_link_frame = + plant->GetFrameByName(canonical_link.name(), main_model_instance); + plant->AddFrame(std::make_unique>( + "__model__", canonical_link_frame, RigidTransformd::Identity(), + main_model_instance)); + } else { + // N.B. Errors will just happen via thrown exceptions. + DRAKE_DEMAND(is_forced_nesting); + // Since this is just for testing, we'll assume that there wouldn't be + // another included model that requires a custom parser. + sdf::Root root; + + std::string root_dir = LoadSdf(&root, data_source, parser_config); + DRAKE_DEMAND(nullptr != root.Model()); + const sdf::Model &model = *root.Model(); + + const std::string model_name = + include.localModelName.value_or(model.Name()); + main_model_instance = AddModelsFromSpecification( + model, sdf::JoinName(include.absoluteParentName, model_name), {}, plant, + package_map, root_dir).front(); + } + + // Now that the model is parsed, we create interface elements to send to + // libsdformat. + + // This will be populated for the first model instance. + sdf::InterfaceModelPtr main_interface_model; + // Record by name to remember model hierarchy since model instances do not + // encode hierarchical information. Related to this comment: + // https://github.com/RobotLocomotion/drake/issues/12270#issuecomment-606757766 + std::map interface_model_hierarchy; + + // N.B. For hierarchy, this assumes that "parent" models are defined before + // their "child" models. + for (ModelInstanceIndex model_instance(cur_num_models); + model_instance < plant->num_model_instances(); ++model_instance) { + sdf::RepostureFunction reposture_model = [plant, model_instance]( + const sdf::InterfaceModelPoseGraph &graph) { + // N.B. This should also posture the model appropriately. + for (auto interface_link_ind : plant->GetBodyIndices(model_instance)) { + const auto& interface_link = plant->get_body(interface_link_ind); + + ignition::math::Pose3d X_WL; + ThrowAnyErrors( + graph.ResolveNestedFramePose(X_WL, interface_link.name())); + plant->SetDefaultFreeBodyPose(interface_link, ToRigidTransform(X_WL)); + } + }; + + const std::string absolute_name = + plant->GetModelInstanceName(model_instance); + const auto [absolute_parent_name, local_name] = + sdf::SplitName(absolute_name); + + const auto& model_frame = + plant->GetFrameByName("__model__", model_instance); + std::string canonical_link_name = + GetRelativeBodyName(model_frame.body(), model_instance, *plant); + RigidTransformd X_PM = RigidTransformd::Identity(); + // The pose of the main (non-nested) model will be updated by the reposture + // callback, so we can use identity as the pose. For the nested model, + // however, we use the pose of the model relative to the parent frame, which + // is the parent model's frame. + if (model_instance != main_model_instance) { + auto parent_model_instance = + plant->GetModelInstanceByName(absolute_parent_name); + const auto& parent_model_frame = + plant->GetFrameByName("__model__", parent_model_instance); + RigidTransformd X_WP = GetDefaultFramePose(*plant, parent_model_frame); + RigidTransformd X_WM = GetDefaultFramePose(*plant, model_frame); + X_PM = X_WP.inverse() * X_WM; + } + + auto interface_model = std::make_shared( + local_name, reposture_model, false, canonical_link_name, + ToIgnPose3d(X_PM)); + + AddBodiesToInterfaceModel(*plant, model_instance, interface_model); + AddFramesToInterfaceModel(*plant, model_instance, interface_model); + AddJointsToInterfaceModel(*plant, model_instance, interface_model); + + if (!main_interface_model) { + main_interface_model = interface_model; + interface_model_hierarchy[absolute_name] = main_interface_model; + } else { + // Register with its parent model. + sdf::InterfaceModelPtr parent_interface_model = + interface_model_hierarchy.at(absolute_parent_name); + parent_interface_model->AddNestedModel(interface_model); + } + } + + return main_interface_model; +} + +sdf::ParserConfig CreateNewSdfParserConfig( + const PackageMap& package_map, + MultibodyPlant* plant, + bool test_sdf_forced_nesting) { + // TODO(marcoag) ensure that we propagate the right ParserConfig instance. + sdf::ParserConfig parser_config; + parser_config.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); + parser_config.SetDeprecatedElementsPolicy(sdf::EnforcementPolicy::WARN); + parser_config.SetFindCallback( + [=](const std::string &_input) { + return ResolveUri(_input, package_map, "."); + }); + // TODO(#15018): This means that unrecognized elements won't be shown to a + // user directly (via console or exception). We should change unrecognized + // elements policy to print a warning, and later become an error. + DRAKE_DEMAND( + parser_config.UnrecognizedElementsPolicy() + == sdf::EnforcementPolicy::LOG); + + parser_config.RegisterCustomModelParser( + [plant, &package_map, &parser_config, test_sdf_forced_nesting]( + const sdf::NestedInclude& include, sdf::Errors& errors) { + return ParseNestedInterfaceModel(plant, package_map, include, &errors, + parser_config, + test_sdf_forced_nesting); + }); + + return parser_config; +} } // namespace ModelInstanceIndex AddModelFromSdf( @@ -941,13 +1243,17 @@ ModelInstanceIndex AddModelFromSdf( const std::string& model_name_in, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph) { + geometry::SceneGraph* scene_graph, + bool test_sdf_forced_nesting) { DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_THROW_UNLESS(!plant->is_finalized()); + sdf::ParserConfig parser_config = + CreateNewSdfParserConfig(package_map, plant, test_sdf_forced_nesting); + sdf::Root root; - std::string root_dir = LoadSdf(&root, data_source, package_map); + std::string root_dir = LoadSdf(&root, data_source, parser_config); // TODO(jwnimmer-tri) When we upgrade to a version of libsdformat that no // longer offers ModelCount(), remove this entire paragraph of code. @@ -985,13 +1291,17 @@ std::vector AddModelsFromSdf( const DataSource& data_source, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph) { + geometry::SceneGraph* scene_graph, + bool test_sdf_forced_nesting) { DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_THROW_UNLESS(!plant->is_finalized()); + sdf::ParserConfig parser_config = + CreateNewSdfParserConfig(package_map, plant, test_sdf_forced_nesting); + sdf::Root root; - std::string root_dir = LoadSdf(&root, data_source, package_map); + std::string root_dir = LoadSdf(&root, data_source, parser_config); // Throw an error if there are no models or worlds. if (root.Model() == nullptr && root.WorldCount() == 0) { diff --git a/multibody/parsing/detail_sdf_parser.h b/multibody/parsing/detail_sdf_parser.h index 547deaa98b1e..3a540a197d42 100644 --- a/multibody/parsing/detail_sdf_parser.h +++ b/multibody/parsing/detail_sdf_parser.h @@ -39,13 +39,18 @@ namespace internal { // @param scene_graph // A pointer to a mutable SceneGraph object used for geometry registration // (either to model visual or contact geometry). May be nullptr. +// @param test_sdf_forced_nesting +// If true, a custom parser for SDFormat files (but using a different file +// extention) will be registered when using libsdformat's Interface API. This +// should only be used for testing. // @returns The model instance index for the newly added model. ModelInstanceIndex AddModelFromSdf( const DataSource& data_source, const std::string& model_name, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph = nullptr); + geometry::SceneGraph* scene_graph = nullptr, + bool test_sdf_forced_nesting = false); // Parses all `` elements from the SDF file specified by `file_name` // and adds them to `plant`. The SDF file can contain multiple `` @@ -69,12 +74,17 @@ ModelInstanceIndex AddModelFromSdf( // @param scene_graph // A pointer to a mutable SceneGraph object used for geometry registration // (either to model visual or contact geometry). May be nullptr. +// @param test_sdf_forced_nesting +// If true, a custom parser for SDFormat files (but using a different file +// extention) will be registered when using libsdformat's Interface API. This +// should only be used for testing. // @returns The set of model instance indices for the newly added models. std::vector AddModelsFromSdf( const DataSource& data_source, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph = nullptr); + geometry::SceneGraph* scene_graph = nullptr, + bool test_sdf_forced_nesting = false); } // namespace internal } // namespace multibody diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index c3e955550a78..c7b596a19296 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -18,6 +19,7 @@ #include "drake/multibody/parsing/detail_tinyxml.h" #include "drake/multibody/parsing/detail_urdf_geometry.h" #include "drake/multibody/parsing/package_map.h" +#include "drake/multibody/parsing/scoped_names.h" #include "drake/multibody/tree/ball_rpy_joint.h" #include "drake/multibody/tree/fixed_offset_frame.h" #include "drake/multibody/tree/planar_joint.h" @@ -682,6 +684,7 @@ void ParseBushing(XMLElement* node, MultibodyPlant* plant) { ModelInstanceIndex ParseUrdf( const std::string& model_name_in, + const std::optional& parent_model_name, const multibody::PackageMap& package_map, const std::string& root_dir, XMLDocument* xml_doc, @@ -699,6 +702,8 @@ ModelInstanceIndex ParseUrdf( "must be specified."); } + model_name = parsing::PrefixName(parent_model_name.value_or(""), model_name); + // Parses the model's material elements. Throws an exception if there's a // material name clash regardless of whether the associated RGBA values are // the same. @@ -780,6 +785,7 @@ ModelInstanceIndex ParseUrdf( ModelInstanceIndex AddModelFromUrdf( const DataSource& data_source, const std::string& model_name_in, + const std::optional& parent_model_name, const PackageMap& package_map, MultibodyPlant* plant, geometry::SceneGraph* scene_graph) { @@ -825,7 +831,7 @@ ModelInstanceIndex AddModelFromUrdf( plant->RegisterAsSourceForSceneGraph(scene_graph); } - return ParseUrdf(model_name_in, package_map, root_dir, + return ParseUrdf(model_name_in, parent_model_name, package_map, root_dir, &xml_doc, plant); } diff --git a/multibody/parsing/detail_urdf_parser.h b/multibody/parsing/detail_urdf_parser.h index e6c4188e1a86..efde88d8a354 100644 --- a/multibody/parsing/detail_urdf_parser.h +++ b/multibody/parsing/detail_urdf_parser.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "drake/geometry/scene_graph.h" @@ -24,6 +25,11 @@ namespace internal { // @param model_name // The name given to the newly created instance of this model. If // empty, the "name" attribute from the model tag will be used. +// @param parent_model_name +// Optional name of parent model. If set, this will be prefixed with the model +// name (either `model_name` or from the "name" attribute) using the SDFormat +// scope delimiter "::". The prefixed name will used as the name given to the +// newly created instance of this model. // @param plant // A pointer to a mutable MultibodyPlant object to which the model will be // added. @@ -34,6 +40,7 @@ namespace internal { ModelInstanceIndex AddModelFromUrdf( const DataSource& data_source, const std::string& model_name, + const std::optional& parent_model_name, const PackageMap& package_map, MultibodyPlant* plant, geometry::SceneGraph* scene_graph = nullptr); diff --git a/multibody/parsing/parser.cc b/multibody/parsing/parser.cc index 170be0c0a88c..a8680aef1869 100644 --- a/multibody/parsing/parser.cc +++ b/multibody/parsing/parser.cc @@ -49,7 +49,7 @@ std::vector Parser::AddAllModelsFromFile( data_source, package_map_, plant_, scene_graph_); } else { return {AddModelFromUrdf( - data_source, {}, package_map_, plant_, scene_graph_)}; + data_source, {}, {}, package_map_, plant_, scene_graph_)}; } } @@ -67,7 +67,7 @@ ModelInstanceIndex Parser::AddModelFromFile( data_source, model_name, package_map_, plant_, scene_graph_); } else { return AddModelFromUrdf( - data_source, model_name, package_map_, plant_, scene_graph_); + data_source, model_name, {}, package_map_, plant_, scene_graph_); } } @@ -83,7 +83,7 @@ ModelInstanceIndex Parser::AddModelFromString( data_source, model_name, package_map_, plant_, scene_graph_); } else { return AddModelFromUrdf( - data_source, model_name, package_map_, plant_, scene_graph_); + data_source, model_name, {}, package_map_, plant_, scene_graph_); } } diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index d777e146eb06..81e0429b8960 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -56,19 +56,23 @@ ModelInstanceIndex AddModelFromSdfFile( const std::string& model_name, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph = nullptr) { + geometry::SceneGraph* scene_graph = nullptr, + bool test_sdf_forced_nesting = false +) { return AddModelFromSdf( { .file_name = &file_name }, - model_name, package_map, plant, scene_graph); + model_name, package_map, plant, scene_graph, test_sdf_forced_nesting); } std::vector AddModelsFromSdfFile( const std::string& file_name, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph = nullptr) { + geometry::SceneGraph* scene_graph = nullptr, + bool test_sdf_forced_nesting = false +) { return AddModelsFromSdf( { .file_name = &file_name }, - package_map, plant, scene_graph); + package_map, plant, scene_graph, test_sdf_forced_nesting); } const Frame& GetModelFrameByName(const MultibodyPlant& plant, @@ -508,7 +512,7 @@ GTEST_TEST(SdfParser, StaticModelSupported) { const RigidTransformd X_WA = frame_A.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices( X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); - EXPECT_EQ(frame_A.body().node_index(), plant->world_body().node_index()); + EXPECT_EQ(frame_A.body().index(), plant->world_body().index()); } { @@ -532,7 +536,7 @@ GTEST_TEST(SdfParser, StaticModelSupported) { const RigidTransformd X_WA = frame_A.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices( X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); - EXPECT_EQ(frame_A.body().node_index(), plant->world_body().node_index()); + EXPECT_EQ(frame_A.body().index(), plant->world_body().index()); const RigidTransformd X_WB_expected( RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); @@ -541,7 +545,7 @@ GTEST_TEST(SdfParser, StaticModelSupported) { const RigidTransformd X_WB = frame_B.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices( X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); - EXPECT_EQ(frame_B.body().node_index(), plant->world_body().node_index()); + EXPECT_EQ(frame_B.body().index(), plant->world_body().index()); } } @@ -570,7 +574,7 @@ GTEST_TEST(SdfParser, StaticFrameOnlyModelsSupported) { const RigidTransformd X_WF = frame.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); - EXPECT_EQ(frame.body().node_index(), plant->world_body().node_index()); + EXPECT_EQ(frame.body().index(), plant->world_body().index()); }; test_frame("__model__", {RollPitchYawd(0.0, 0.0, 0.0), Vector3d(1, 0, 0)}); @@ -1705,11 +1709,11 @@ GTEST_TEST(SdfParser, FrameAttachedToMultiLevelNestedFrame) { // Also check that the frame is attached to the right body ModelInstanceIndex model_c_instance = plant->GetModelInstanceByName("a::b::c"); - EXPECT_EQ(frame_E.body().node_index(), - plant->GetBodyByName("d", model_c_instance).node_index()); + EXPECT_EQ(frame_E.body().index(), + plant->GetBodyByName("d", model_c_instance).index()); - EXPECT_EQ(frame_F.body().node_index(), - plant->GetBodyByName("d", model_c_instance).node_index()); + EXPECT_EQ(frame_F.body().index(), + plant->GetBodyByName("d", model_c_instance).index()); } // Verify frames and links can have the same local name without violating name @@ -1771,11 +1775,63 @@ GTEST_TEST(SdfParser, FrameAttachedToModelFrameInWorld) { X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); // Also check that the frame is attached to the right body - EXPECT_EQ(frame_E.body().node_index(), - plant->GetBodyByName("d").node_index()); + EXPECT_EQ(frame_E.body().index(), + plant->GetBodyByName("d").index()); - EXPECT_EQ(frame_F.body().node_index(), - plant->GetBodyByName("d").node_index()); + EXPECT_EQ(frame_F.body().index(), + plant->GetBodyByName("d").index()); +} + +// Verify frames can be attached to joint frames +GTEST_TEST(SdfParser, FrameAttachedToJointFrame) { + const std::string model_string = R"""( + + + 0.1 0.2 0.0 0 0 0 + + + + 0 0.0 0.3 0 0 0 + + + + L1 + L2 + + + L1 + M1::L3 + + + + +)"""; + auto [plant, scene_graph] = ParseTestString(model_string, "1.8"); + + ASSERT_NE(nullptr, plant); + plant->Finalize(); + auto context = plant->CreateDefaultContext(); + + const RigidTransformd X_WF1_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(0.1, 0.2, 0.0)); + + const RigidTransformd X_WF2_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(0.1, 0.2, 0.3)); + + const auto& frame_F1 = plant->GetFrameByName("F1"); + const auto& frame_F2 = plant->GetFrameByName("F2"); + const RigidTransformd X_WF1 = frame_F1.CalcPoseInWorld(*context); + const RigidTransformd X_WF2 = frame_F2.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices( + X_WF1_expected.GetAsMatrix4(), X_WF1.GetAsMatrix4(), kEps)); + EXPECT_TRUE(CompareMatrices( + X_WF2_expected.GetAsMatrix4(), X_WF2.GetAsMatrix4(), kEps)); + + // Also check that the frame is attached to the right body + EXPECT_EQ(frame_F1.body().index(), + plant->GetBodyByName("L2").index()); + EXPECT_EQ(frame_F2.body().index(), + plant->GetBodyByName("L3").index()); } GTEST_TEST(SdfParser, SupportNonDefaultCanonicalLink) { @@ -1803,6 +1859,166 @@ GTEST_TEST(SdfParser, SupportNonDefaultCanonicalLink) { EXPECT_EQ(GetModelFrameByName(*plant, "a::c").body().index(), plant->GetBodyByName("f").index()); } + +// Verify that frames can be used for //joint/parent and //joint/child +GTEST_TEST(SdfParser, FramesAsJointParentOrChild) { + const std::string full_name = FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/" + "frames_as_joint_parent_or_child.sdf"); + MultibodyPlant plant(0.0); + + PackageMap package_map; + package_map.PopulateUpstreamToDrake(full_name); + AddModelsFromSdfFile(full_name, package_map, &plant); + ASSERT_TRUE(plant.HasModelInstanceNamed("parent_model")); + + plant.Finalize(); + auto context = plant.CreateDefaultContext(); + + // Frames attached to links in the same model + { + const auto& joint_J1 = plant.GetJointByName("J1"); + + // J1p and J1c are the frames of J1 on its parent and child links + // respectively. The absolute poses of both frames should be identical. + const RigidTransformd X_WJ1_expected(RollPitchYawd(0, 0, 0), + Vector3d(4, 5, 6)); + + const RigidTransformd X_WJ1p = + joint_J1.frame_on_parent().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ1_expected.GetAsMatrix4(), + X_WJ1p.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WJ1c = + joint_J1.frame_on_child().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ1_expected.GetAsMatrix4(), + X_WJ1c.GetAsMatrix4(), kEps)); + } + // Frames attached to links in the other (nested) models + { + const auto& joint_J2 = plant.GetJointByName("J2"); + + // J2p and J2c are the frames of J2 on its parent and child links + // respectively. The absolute poses of both frames should be identical. + const RigidTransformd X_WJ2_expected(RollPitchYawd(0, 0, 0), + Vector3d(4, 5, 6)); + const RigidTransformd X_WJ2p = + joint_J2.frame_on_parent().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ2_expected.GetAsMatrix4(), + X_WJ2p.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WJ2c = + joint_J2.frame_on_child().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ2_expected.GetAsMatrix4(), + X_WJ2c.GetAsMatrix4(), kEps)); + } +} + +// Verifies that URDF files can be loaded into Drake via libsdformat's Interface +// API which bypasses the URDF->SDFormat conversion. This also verifies that +// SDFormat files can be forced to be loaded via the Interface API by changing +// their file extension and registering the appropriate custom parser. +GTEST_TEST(SdfParser, InterfaceAPI) { + const std::string sdf_file_path = FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "top.sdf"); + PackageMap package_map; + package_map.PopulateUpstreamToDrake(sdf_file_path); + MultibodyPlant plant(0.0); + + DRAKE_ASSERT_NO_THROW(AddModelFromSdfFile(sdf_file_path, "", package_map, + &plant, nullptr, true)); + + plant.Finalize(); + auto context = plant.CreateDefaultContext(); + + { + // Frame A represents the model frame of model top::arm + const RigidTransformd X_WA_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(1, 0, 0)); + const auto arm_model_instance = plant.GetModelInstanceByName("top::arm"); + const auto& arm_model_frame = + plant.GetFrameByName("__model__", arm_model_instance); + const RigidTransformd X_WA = arm_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WA_expected.GetAsMatrix4(), + X_WA.GetAsMatrix4(), kEps)); + const auto& arm_L1 = plant.GetFrameByName("L1", arm_model_instance); + const RigidTransformd X_WL1 = arm_L1.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WA_expected.GetAsMatrix4(), + X_WL1.GetAsMatrix4(), kEps)); + } + + { + // Frame E represents the model frame of model top::extra_arm + const RigidTransformd X_WE_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(1, 2, 0)); + const auto extra_arm_model_instance = + plant.GetModelInstanceByName("top::extra_arm"); + const auto& extra_arm_model_frame = + plant.GetFrameByName("__model__", extra_arm_model_instance); + const RigidTransformd X_WE = + extra_arm_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WE_expected.GetAsMatrix4(), + X_WE.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WL2_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(2, 4, 3)); + const auto& arm_L2 = plant.GetFrameByName("L2", extra_arm_model_instance); + const RigidTransformd X_WL2 = arm_L2.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WL2_expected.GetAsMatrix4(), + X_WL2.GetAsMatrix4(), kEps)); + } + { + // Frame F represents the model frame of model top::arm::flange + const RigidTransformd X_WF_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(1, 2, 1)); + const auto flange_model_instance = + plant.GetModelInstanceByName("top::arm::flange"); + const auto& flange_model_frame = + plant.GetFrameByName("__model__", flange_model_instance); + const RigidTransformd X_WF = + flange_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WF_expected.GetAsMatrix4(), + X_WF.GetAsMatrix4(), kEps)); + + // Frame M represents the frame of model top::arm::flange::gripper_mount + const RigidTransformd X_WM_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); + const auto& gripper_mount_frame = + plant.GetFrameByName("gripper_mount", flange_model_instance); + const RigidTransformd X_WM = gripper_mount_frame .CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WM_expected.GetAsMatrix4(), + X_WM.GetAsMatrix4(), kEps)); + + // Frame G represents the frame of model top::arm::flange::gripper + const RigidTransformd X_WG_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); + const auto gripper_model_instance = + plant.GetModelInstanceByName("top::arm::gripper"); + const auto& gripper_model_frame = + plant.GetFrameByName("__model__", gripper_model_instance); + const RigidTransformd X_WG = gripper_model_frame.CalcPoseInWorld(*context); + // TODO(azeey) There is a precision loss that occurs in libsdformat when + // resolving poses. Use just kEps when the following ign-math issue is + // resolved: https://github.com/ignitionrobotics/ign-math/issues/212. + EXPECT_TRUE(CompareMatrices(X_WG_expected.GetAsMatrix4(), + X_WG.GetAsMatrix4(), 10 * kEps)); + } + // Test placement_frame using a table and a mug flipped upside down + { + // Frame T represents the frame top::table_and_mug::mug::top + const RigidTransformd X_WT_expected(RollPitchYawd(1.570796, 0.0, 0.0), + Vector3d(3, 0, 0.5)); + const auto mug_model_instance = + plant.GetModelInstanceByName("top::table_and_mug::mug"); + const auto& mug_top_frame = + plant.GetFrameByName("top", mug_model_instance); + const RigidTransformd X_WT = + mug_top_frame.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WT_expected.GetAsMatrix4(), + X_WT.GetAsMatrix4(), kEps)); + } +} } // namespace } // namespace internal } // namespace multibody diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc index f08928f38452..cf91b53114b4 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -44,7 +44,7 @@ ModelInstanceIndex AddModelFromUrdfFile( geometry::SceneGraph* scene_graph = nullptr) { return AddModelFromUrdf( { .file_name = &file_name }, - model_name, package_map, plant, scene_graph); + model_name, {}, package_map, plant, scene_graph); } // Verifies that the URDF loader can leverage a specified package map. diff --git a/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf b/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf new file mode 100644 index 000000000000..d337804d5e0b --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf @@ -0,0 +1,40 @@ + + + + + + + + + 1 2 3 0 0 0 + + + 4 5 6 0 0 0 + + + + L1_offset + L2_offset + + + + + + + + + + + + 1 2 3 0 0 0 + + + 4 5 6 0 0 0 + + + + M1_base_link_offset + M2_base_link_offset + + + 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 new file mode 100644 index 000000000000..02ac14c7938e --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.forced_nesting_sdf @@ -0,0 +1,35 @@ + + + + + + 0 0 1 0 0 0 + L1 + L2 + + 0 0 1 + + + + + + + + 0 2 1 0 0 0 + + + 0 0 2 0.1 0.2 0.3 + + + + L2 + flange + + + + package://interface_api_test/gripper.sdf + gripper + + + + 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 new file mode 100644 index 000000000000..6cb6691cc23e --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + 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 new file mode 100644 index 000000000000..c2a2110e5718 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/gripper.sdf @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/mug.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/mug.sdf new file mode 100644 index 000000000000..992c86a5ad65 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/mug.sdf @@ -0,0 +1,10 @@ + + + + + + 0 0 0.07 0 0 0 + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/package.xml b/multibody/parsing/test/sdf_parser_test/interface_api_test/package.xml new file mode 100644 index 000000000000..de5fd0ed5365 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/package.xml @@ -0,0 +1,5 @@ + + interface_api_test + 0.0.1 + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/table.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/table.sdf new file mode 100644 index 000000000000..ad131591a8b3 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/table.sdf @@ -0,0 +1,10 @@ + + + + + + 0 0 0.5 0 0 0 + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf new file mode 100644 index 000000000000..0384185453b0 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/table_and_mug.forced_nesting_sdf @@ -0,0 +1,24 @@ + + + + + package://interface_api_test/table.sdf + table + + + package://interface_api_test/mug.sdf + mug + top + 0 0 0 1.570796 0 0 + + + + + table_top_alias + mug_top_alias + + + + + + 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 new file mode 100644 index 000000000000..2affd9444fc6 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top.sdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + torso + arm::mount + + + + 1 0 0 0 0 0 + package://interface_api_test/arm.forced_nesting_sdf + arm + + + + 0 2 0 0 0 0 + package://interface_api_test/arm.urdf + extra_arm + + + + arm::L1 + extra_arm::L1 + + + + 3 0 0 0 0 0 + package://interface_api_test/table_and_mug.forced_nesting_sdf + table_and_mug + + +