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 d104a8f01149..92914a54c2c9 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" @@ -89,6 +92,13 @@ bool StartsWith(const std::string_view str, const std::string_view prefix) { std::equal(str.begin(), str.begin() + prefix.size(), prefix.begin()); } +// Returns true if `str` ends with `ext`. The length of `ext` has to be +// strictly less than the size of `str`. +bool EndsWith(const std::string_view str, const std::string_view ext) { + return ext.size() < str.size() && + std::equal(str.end() - ext.size(), str.end(), ext.begin()); +} + // Calculates the scoped 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 @@ -548,29 +558,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); @@ -998,6 +1000,242 @@ std::vector AddModelsFromSpecification( return added_model_instances; } +// 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( @@ -1005,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. @@ -1049,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 82ed90697907..b29881ec4787 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 e78a7a78db02..c7e6d69ed055 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 6e336c2c9aeb..6397bd89ab74 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -54,28 +54,31 @@ 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) { const DataSource data_source{&file_name, {}}; - return AddModelFromSdf( - data_source, model_name, package_map, plant, scene_graph); + return AddModelFromSdf(data_source, 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) { const DataSource data_source{&file_name, {}}; return AddModelsFromSdf( - data_source, package_map, plant, scene_graph); + data_source, package_map, plant, scene_graph, test_sdf_forced_nesting); } std::vector AddModelsFromSdfString( const std::string& file_contents, const PackageMap& package_map, MultibodyPlant* plant, - geometry::SceneGraph* scene_graph = nullptr) { + geometry::SceneGraph* scene_graph = nullptr, + bool test_sdf_forced_nesting = false) { const DataSource data_source{{}, &file_contents}; return AddModelsFromSdf( - data_source, package_map, plant, scene_graph); + data_source, package_map, plant, scene_graph, test_sdf_forced_nesting); } const Frame& GetModelFrameByName(const MultibodyPlant& plant, @@ -1916,6 +1919,112 @@ GTEST_TEST(SdfParser, FramesAsJointParentOrChild) { X_CJc.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/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 + + +