From d258c760aa32095d8c99d2b8c42ae4f00df92d1c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 20 Apr 2023 00:42:34 -0500 Subject: [PATCH 1/6] Merge-include via Interface API using approach from r1 --- multibody/parsing/detail_parsing_workspace.h | 25 ++ multibody/parsing/detail_sdf_parser.cc | 373 +++++++++++++----- multibody/parsing/detail_urdf_parser.cc | 58 ++- multibody/parsing/detail_urdf_parser.h | 5 + .../parsing/test/detail_sdf_parser_test.cc | 266 ++++++++++++- .../interface_api_test/arm.urdf | 5 + .../interface_api_test/arm_merge_include.sdf | 57 +++ .../arm_with_gripper_merge_include.sdf | 22 ++ .../interface_api_test/top_merge_include.sdf | 59 +++ .../top_merge_include_in_nested_model.sdf | 14 + .../top_merge_include_world.sdf | 14 + .../sdformat_internal/repository.bzl | 2 +- 12 files changed, 771 insertions(+), 129 deletions(-) create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/arm_with_gripper_merge_include.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_in_nested_model.sdf create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_world.sdf diff --git a/multibody/parsing/detail_parsing_workspace.h b/multibody/parsing/detail_parsing_workspace.h index 0531c2faa114..d22d99c170a6 100644 --- a/multibody/parsing/detail_parsing_workspace.h +++ b/multibody/parsing/detail_parsing_workspace.h @@ -51,6 +51,31 @@ class ParserInterface { const std::optional& parent_model_name, const ParsingWorkspace& workspace) = 0; + // Parses a model from the input file specified by @p data_source add all of + // its contents into an existing model instance specified by @p + // model_instance. @p model_instance must already exist in @p plant. + // + // @param data_source + // The model data to be parsed. + // @param model_name + // The name given to the newly created instance of this model. If + // empty, the model name found within the model data will be used. + // @param model_instance + // The model instance into which the contents of the model will be added. + // @param workspace + // The ParsingWorkspace. + // @returns The name of the the model that was loaded from @p data_source. + // Note, this is not the name of the @p model_instance. Instead, it's the + // name of the original model that has now been merged into @p + // model_instance. + virtual std::string MergeModel(const DataSource& /* data_source */, + const std::string& /* model_name */, + ModelInstanceIndex /* model_instance */, + const ParsingWorkspace& /* workspace */) { + throw std::runtime_error( + fmt::format("MergeModel is not implemented for this input type")); + } + // Parses all models from the input file specified by @p data_source and adds // them to @p plant. New model instances will be added to @p plant. // diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 0864e26772d4..bf523d866465 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -1,5 +1,6 @@ #include "drake/multibody/parsing/detail_sdf_parser.h" +#include #include #include #include @@ -61,6 +62,41 @@ using std::unique_ptr; // Unnamed namespace for free functions local to this file. namespace { +// A short summary of parsing stages in libsdformat: +// +// When we load a model using `sdf::Root::Load`, the following happens: +// * libsdformat parses the XML into `sdf::ElementPtr` objects. Included +// SDFormat models are expanded during this process. If the included model is +// not an SDFormat file type and there are custom parsers registered in the +// passed in `ParserConfig`, the contents of the `` tag are saved for +// later processing +// * DOM objects are created from `sdf::ElementPtr` objects. During the creation +// of `sdf::World` and `sdf::Model`, custom parsers are called on the +// non-SDFormat included files mentioned above. This is what's called the +// Interface API. +// * Drake's custom parser (eg. detail_urdf_parser) parses the files into a +// MultibodyPlant. +// * The frame bearing elements (Links, Joints, Frames, and Models) are read +// back from the MultibodyPlant and returned to libsdformat via the Interface +// API data structures. +// * Once DOM object creation is complete, libsdformat constructs the frame and +// pose graph for the entire model. +// * `sdf::Root::Load` returns and Drake starts its parsing stage where it +// populates the MultibodyPlant using the DOM objects created above (e.g. +// AddModelsFromSpecification). +// +// Data structure to hold model instances that were created during libsdformat's +// Interface API callback for handling merged models. +// When handling merged models during libsdformat's Interface API callback, we +// have to create model instances of parents of merged models since the parent +// models are regular SDFormat elements and will only be handled at a later +// stage of parsing. This data structure is used to store these parent model +// instances so that when we're going through Drake's parsing stage (last step +// in the parsing stage summary above), we are aware of these model instances +// and not try to recreate them. +using ModelInstanceIndexRange = + std::pair; + // Returns the model instance name for the given `instance`, unless it's the // world model instance in which case returns the empty string. std::string GetInstanceScopeNameIgnoringWorld( @@ -1324,6 +1360,101 @@ void ParseCollisionFilterGroup(const SDFormatDiagnostic& diagnostic, get_string_attribute, get_bool_attribute, read_tag_string); } +bool CanReuseModelInstance( + const ModelInstanceIndex& model_instance, + const ModelInstanceIndexRange reusable_model_instance_range) { + return model_instance >= reusable_model_instance_range.first && + model_instance < reusable_model_instance_range.second; +} + +ModelInstanceIndex AddModelInstanceIfReusable( + MultibodyPlant* plant, const std::string& model_name, + const ModelInstanceIndexRange& reusable_model_instance_range) { + if (plant->HasModelInstanceNamed(model_name)) { + auto model_instance = plant->GetModelInstanceByName(model_name); + if (!CanReuseModelInstance(model_instance, reusable_model_instance_range)) { + throw std::logic_error( + "This model already contains a model instance named '" + model_name + + "'. Model instance names must be unique within a given model."); + } + return model_instance; + } else { + return plant->AddModelInstance(model_name); + } +} + +// Helper class that does bookkeeping on the frame bearing elements of a given +// model instance in a MultibodyPlant. This is used to keep track of newly +// created elements after a custom parser is called so that we can create +// Interface API data structures from just the newly created elements. Note: We +// assume that Drake custom parsers do not support nested models. +// TODO(azeey) If any of Drakes custom parsers start supporting nested models, +// this needs to be updated to keep track of them. +struct InterfaceModelHelper { + const MultibodyPlant& plant; + bool is_merged; + ModelInstanceIndex model_instance; + std::string name; + std::vector body_indices; + std::vector frame_indices; + std::vector joint_indices; + std::vector model_instance_indices; + std::string model_frame_name = "__model__"; + + bool have_snapshot{false}; + + InterfaceModelHelper(const MultibodyPlant& pt, bool merged) + : plant(pt), is_merged(merged) {} + + std::vector GetChildModelInstanceIndices() const { + std::vector output; + + for (ModelInstanceIndex mi(model_instance + 1); + mi < plant.num_model_instances(); ++mi) { + if (multibody::ScopedName::Parse(plant.GetModelInstanceName(mi)) + .get_element() == name) { + output.push_back(mi); + } + } + return output; + } + + // Take a snapshot of the current contents of a model instance so that we + // later on do a diff + void TakeSnapShot(ModelInstanceIndex model_instance_index) { + model_instance = model_instance_index; + name = plant.GetModelInstanceName(model_instance); + body_indices = plant.GetBodyIndices(model_instance); + frame_indices = plant.GetFrameIndices(model_instance); + joint_indices = plant.GetJointIndices(model_instance); + model_instance_indices = GetChildModelInstanceIndices(); + have_snapshot = true; + } + + template + static std::vector GetVectorDiff(const std::vector& current, + const std::vector& previous) { + std::vector output; + std::set_difference(current.begin(), current.end(), previous.begin(), + previous.end(), std::back_inserter(output)); + return output; + } + + // Compute the diff to determine the elements that were created after the + // snapshot. + void ComputeDiffFromSnapshot() { + DRAKE_DEMAND(have_snapshot); + body_indices = + GetVectorDiff(plant.GetBodyIndices(model_instance), body_indices); + frame_indices = + GetVectorDiff(plant.GetFrameIndices(model_instance), frame_indices); + joint_indices = + GetVectorDiff(plant.GetJointIndices(model_instance), joint_indices); + model_instance_indices = + GetVectorDiff(GetChildModelInstanceIndices(), model_instance_indices); + } +}; + // Helper method to add a model to a MultibodyPlant given an sdf::Model // specification object. std::vector AddModelsFromSpecification( @@ -1334,7 +1465,10 @@ std::vector AddModelsFromSpecification( MultibodyPlant* plant, CollisionFilterGroupResolver* resolver, const PackageMap& package_map, - const std::string& root_dir) { + const std::string& root_dir, + const ModelInstanceIndexRange &reusable_model_instance_range) { + const ModelInstanceIndex model_instance = AddModelInstanceIfReusable( + plant, model_name, reusable_model_instance_range); const std::set supported_model_elements{ "drake:joint", @@ -1351,9 +1485,6 @@ std::vector AddModelsFromSpecification( CheckSupportedElements( diagnostic, model.Element(), supported_model_elements); - - const ModelInstanceIndex model_instance = - plant->AddModelInstance(model_name); std::vector added_model_instances{model_instance}; // "P" is the parent frame. If the model is in a child of //world or //sdf, @@ -1373,8 +1504,8 @@ std::vector AddModelsFromSpecification( std::vector nested_model_instances = AddModelsFromSpecification( diagnostic, nested_model, - sdf::JoinName(model_name, nested_model.Name()), X_WM, - plant, resolver, package_map, root_dir); + sdf::JoinName(model_name, nested_model.Name()), X_WM, plant, + resolver, package_map, root_dir, reusable_model_instance_range); added_model_instances.insert(added_model_instances.end(), nested_model_instances.begin(), @@ -1529,12 +1660,10 @@ RigidTransformd GetDefaultFramePose( constexpr char kExtUrdf[] = ".urdf"; 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 sdf::InterfaceModelPtr& interface_model, + const std::vector& body_indices, + const RigidTransformd& X_MW) { + for (auto index : body_indices) { const auto& link = plant.get_body(index); RigidTransformd X_ML = X_MW * plant.GetDefaultFreeBodyPose(link); interface_model->AddLink({link.name(), ToIgnitionPose3d(X_ML)}); @@ -1543,19 +1672,21 @@ void AddBodiesToInterfaceModel(const MultibodyPlant& plant, void AddFramesToInterfaceModel(const MultibodyPlant& plant, ModelInstanceIndex model_instance, - const sdf::InterfaceModelPtr& interface_model) { - for (FrameIndex index(0); index < plant.num_frames(); ++index) { + const sdf::InterfaceModelPtr& interface_model, + const std::vector& frame_indices, + const std::string& model_frame_name) { + for (auto index : frame_indices) { const auto& frame = plant.get_frame(index); if (frame.model_instance() != model_instance) { continue; } - if (frame.name() == "__model__" || + if (frame.name().empty() || frame.name() == model_frame_name || plant.HasBodyNamed(frame.name(), model_instance)) { - // Skip __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). + // 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( @@ -1565,9 +1696,9 @@ void AddFramesToInterfaceModel(const MultibodyPlant& plant, } void AddJointsToInterfaceModel(const MultibodyPlant& plant, - ModelInstanceIndex model_instance, - const sdf::InterfaceModelPtr& interface_model) { - for (auto index : plant.GetJointIndices(model_instance)) { + const sdf::InterfaceModelPtr& interface_model, + const std::vector& joint_indices) { + for (auto index : joint_indices) { const auto& joint = plant.get_joint(index); const std::string& child_name = joint.child_body().name(); const RigidTransformd X_CJ = @@ -1592,6 +1723,60 @@ sdf::Error MakeSdfError(sdf::ErrorCode code, const DiagnosticDetail& detail) { return result; } +ModelInstanceIndex GetOrCreateModelInstanceByName( + MultibodyPlant* plant, const std::string& model_name) { + if (plant->HasModelInstanceNamed(model_name)) { + return plant->GetModelInstanceByName(model_name); + } + return plant->AddModelInstance(model_name); +} + +sdf::InterfaceModelPtr ConvertToInterfaceModel( + MultibodyPlant* plant, std::string model_name, + const InterfaceModelHelper& interface_model_helper, sdf::Errors* errors, + RigidTransformd X_WP = RigidTransformd::Identity()) { + auto model_instance = interface_model_helper.model_instance; + sdf::RepostureFunction reposture_model = + [plant, model_instance, + errors](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); + + gz::math::Pose3d X_WL; + sdf::Errors inner_errors = + graph.ResolveNestedFramePose(X_WL, interface_link.name()); + PropagateErrors(std::move(inner_errors), errors); + plant->SetDefaultFreeBodyPose(interface_link, ToRigidTransform(X_WL)); + } + }; + + const auto& model_frame = plant->GetFrameByName( + interface_model_helper.model_frame_name, model_instance); + std::string canonical_link_name = + GetRelativeBodyName(model_frame.body(), model_instance, *plant); + RigidTransformd X_PM = RigidTransformd::Identity(); + RigidTransformd X_WM = GetDefaultFramePose(*plant, model_frame); + X_PM = X_WP.inverse() * X_WM; + + auto interface_model = std::make_shared( + model_name, reposture_model, false, canonical_link_name, + ToIgnitionPose3d(X_PM)); + + AddBodiesToInterfaceModel(*plant, interface_model, + interface_model_helper.body_indices, + X_WM.inverse()); + + AddFramesToInterfaceModel(*plant, model_instance, interface_model, + interface_model_helper.frame_indices, + interface_model_helper.model_frame_name); + + AddJointsToInterfaceModel(*plant, interface_model, + interface_model_helper.joint_indices); + + return interface_model; +} + // 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 @@ -1635,18 +1820,49 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( ModelInstanceIndex main_model_instance; // New instances will have indices starting from cur_num_models - int cur_num_models = plant->num_model_instances(); + const bool isMergeInclude = include.IsMerge().value_or(false); + + InterfaceModelHelper interface_model_helper(*plant, isMergeInclude); ParsingWorkspace subworkspace{options, package_map, subdiagnostic, plant, collision_resolver, parser_selector}; - const std::optional maybe_model = - parser_selector(diagnostic, resolved_filename). - AddModel(data_source, include.LocalModelName().value_or(""), - include.AbsoluteParentName(), subworkspace); - if (maybe_model.has_value()) { - main_model_instance = *maybe_model; + + std::string model_frame_name = "__model__"; + std::string model_name; + if (isMergeInclude) { + // Create the parent model instance if it hasn't been created already. + // This can happen if this is the first model to be merge-included. + const auto parent_model_instance = + GetOrCreateModelInstanceByName(plant, include.AbsoluteParentName()); + + interface_model_helper.TakeSnapShot(parent_model_instance); + auto& parser = parser_selector(diagnostic, resolved_filename); + model_name = + parser.MergeModel(data_source, include.LocalModelName().value_or(""), + parent_model_instance, subworkspace); + interface_model_helper.ComputeDiffFromSnapshot(); + + model_frame_name = sdf::computeMergedModelProxyFrameName(model_name); + interface_model_helper.model_frame_name = model_frame_name; + + main_model_instance = parent_model_instance; } else { - return nullptr; + const std::optional maybe_model = + parser_selector(diagnostic, resolved_filename). + AddModel(data_source, include.LocalModelName().value_or(""), + include.AbsoluteParentName(), subworkspace); + if (maybe_model.has_value()) { + main_model_instance = *maybe_model; + } else { + return nullptr; + } + + model_name = + ScopedName::Parse(plant->GetModelInstanceName(main_model_instance)) + .get_element(); + + interface_model_helper.TakeSnapShot(main_model_instance); } + DRAKE_DEMAND(!model_name.empty()); // Add explicit model frame to first link. auto body_indices = workspace.plant->GetBodyIndices(main_model_instance); @@ -1661,7 +1877,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( plant->GetFrameByName(canonical_link.name(), main_model_instance); plant->AddFrame( std::make_unique>( - "__model__", canonical_link_frame, RigidTransformd::Identity(), + model_frame_name, canonical_link_frame, RigidTransformd::Identity(), main_model_instance)); // Now that the model is parsed, we create interface elements to send to @@ -1669,72 +1885,9 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( // 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 = plant, model_instance, errors]( - 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); - - gz::math::Pose3d X_WL; - sdf::Errors inner_errors = graph.ResolveNestedFramePose( - X_WL, interface_link.name()); - PropagateErrors(std::move(inner_errors), errors); - 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, - ToIgnitionPose3d(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); - } - } + main_interface_model = ConvertToInterfaceModel( + plant, model_name, interface_model_helper, errors); + main_interface_model->SetParserSupportsMergeInclude(true); return main_interface_model; } @@ -1808,11 +1961,18 @@ std::optional AddModelFromSdf( SDFormatDiagnostic diagnostic(&workspace.diagnostic, &data_source); + const auto model_index_begin = + static_cast(workspace.plant->num_model_instances()); sdf::Errors errors = LoadSdf( diagnostic, &root, data_source, parser_config); if (diagnostic.PropagateErrors(errors)) { return std::nullopt; } + const auto model_index_end = + static_cast(workspace.plant->num_model_instances()); + + ModelInstanceIndexRange reusable_model_instance_range = + std::make_pair(model_index_begin, model_index_end); const sdf::Model* maybe_model = get_only_model(root); if (maybe_model == nullptr) { @@ -1832,8 +1992,8 @@ std::optional AddModelFromSdf( std::vector added_model_instances = AddModelsFromSpecification( diagnostic, model, model_name, {}, workspace.plant, - workspace.collision_resolver, - workspace.package_map, data_source.GetRootDir()); + workspace.collision_resolver, workspace.package_map, + data_source.GetRootDir(), reusable_model_instance_range); DRAKE_DEMAND(!added_model_instances.empty()); return added_model_instances.front(); @@ -1851,12 +2011,21 @@ std::vector AddModelsFromSdf( SDFormatDiagnostic diagnostic(&workspace.diagnostic, &data_source); + const auto model_index_begin = + static_cast(workspace.plant->num_model_instances()); sdf::Errors errors = LoadSdf( diagnostic, &root, data_source, parser_config); if (diagnostic.PropagateErrors(errors)) { return {}; } + const auto model_index_end = + static_cast(workspace.plant->num_model_instances()); + + // See comment on where the ModelInstanceIndexRange type is defined. + ModelInstanceIndexRange reusable_model_instance_range = + std::make_pair(model_index_begin, model_index_end); + // There either must be exactly one model, or exactly one world. #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -1888,7 +2057,7 @@ std::vector AddModelsFromSdf( AddModelsFromSpecification( diagnostic, model, model_name, {}, workspace.plant, workspace.collision_resolver, workspace.package_map, - data_source.GetRootDir()); + data_source.GetRootDir(), reusable_model_instance_range); model_instances.insert(model_instances.end(), added_model_instances.begin(), added_model_instances.end()); @@ -1920,7 +2089,7 @@ std::vector AddModelsFromSdf( AddModelsFromSpecification( diagnostic, model, model_name, {}, workspace.plant, workspace.collision_resolver, workspace.package_map, - data_source.GetRootDir()); + data_source.GetRootDir(), reusable_model_instance_range); model_instances.insert(model_instances.end(), added_model_instances.begin(), added_model_instances.end()); diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 55ee0dc41ff0..5836994403fc 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -62,11 +62,13 @@ class UrdfParser { const DataSource* data_source, const std::string& model_name, const std::optional& parent_model_name, + std::optional model_instance, const std::string& root_dir, XMLDocument* xml_doc, const ParsingWorkspace& w) : model_name_(model_name), parent_model_name_(parent_model_name), + merge_into_model_instance_(model_instance), root_dir_(root_dir), xml_doc_(xml_doc), w_(w), @@ -133,9 +135,12 @@ class UrdfParser { diagnostic_.WarnUnsupportedAttribute(node, attribute); } + const std::string& model_name() { return model_name_; } + private: - const std::string model_name_; + std::string model_name_; const std::optional parent_model_name_; + std::optional merge_into_model_instance_; const std::string root_dir_; XMLDocument* const xml_doc_; const ParsingWorkspace& w_; @@ -989,8 +994,14 @@ std::optional UrdfParser::Parse() { return {}; } - model_name = MakeModelName(model_name, parent_model_name_, w_); - model_instance_ = w_.plant->AddModelInstance(model_name); + if (!merge_into_model_instance_.has_value()) { + model_name = MakeModelName(model_name, parent_model_name_, w_); + model_instance_ = w_.plant->AddModelInstance(model_name); + } else { + model_instance_ = *merge_into_model_instance_; + } + + model_name_ = 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 @@ -1072,13 +1083,12 @@ std::optional UrdfParser::Parse() { return model_instance_; } -} // namespace - -std::optional AddModelFromUrdf( - const DataSource& data_source, - const std::string& model_name_in, - const std::optional& parent_model_name, - const ParsingWorkspace& workspace) { +std::pair, std::string> +AddOrMergeModelFromUrdf(const DataSource& data_source, + const std::string& model_name_in, + const std::optional& parent_model_name, + const ParsingWorkspace& workspace, + std::optional model_instance) { MultibodyPlant* plant = workspace.plant; DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_THROW_UNLESS(!plant->is_finalized()); @@ -1091,20 +1101,32 @@ std::optional AddModelFromUrdf( if (xml_doc.ErrorID()) { diag.Error(xml_doc, fmt::format("Failed to parse XML file: {}", xml_doc.ErrorName())); - return std::nullopt; + return std::make_pair(std::nullopt, ""); } } else { xml_doc.Parse(data_source.contents().c_str()); if (xml_doc.ErrorID()) { diag.Error(xml_doc, fmt::format("Failed to parse XML string: {}", xml_doc.ErrorName())); - return std::nullopt; + return std::make_pair(std::nullopt, ""); } } UrdfParser parser(&data_source, model_name_in, parent_model_name, - data_source.GetRootDir(), &xml_doc, workspace); - return parser.Parse(); + model_instance, data_source.GetRootDir(), &xml_doc, + workspace); + auto added_model_instance = parser.Parse(); + return std::make_pair(added_model_instance, parser.model_name()); +} +} // namespace + +std::optional AddModelFromUrdf( + const DataSource& data_source, + const std::string& model_name_in, + const std::optional& parent_model_name, + const ParsingWorkspace& workspace) { + return AddOrMergeModelFromUrdf(data_source, model_name_in, parent_model_name, + workspace, std::nullopt).first; } UrdfParserWrapper::UrdfParserWrapper() {} @@ -1119,6 +1141,14 @@ std::optional UrdfParserWrapper::AddModel( workspace); } +std::string UrdfParserWrapper::MergeModel(const DataSource& data_source, + const std::string& model_name, + ModelInstanceIndex model_instance, + const ParsingWorkspace& workspace) { + return AddOrMergeModelFromUrdf(data_source, model_name, std::nullopt, + workspace, model_instance).second; +} + std::vector UrdfParserWrapper::AddAllModels( const DataSource& data_source, const std::optional& parent_model_name, diff --git a/multibody/parsing/detail_urdf_parser.h b/multibody/parsing/detail_urdf_parser.h index 24175b76e86b..aea6f355a1ad 100644 --- a/multibody/parsing/detail_urdf_parser.h +++ b/multibody/parsing/detail_urdf_parser.h @@ -60,6 +60,11 @@ class UrdfParserWrapper final : public ParserInterface { const std::optional& parent_model_name, const ParsingWorkspace& workspace) final; + std::string MergeModel(const DataSource& data_source, + const std::string& model_name, + ModelInstanceIndex model_instance, + const ParsingWorkspace& workspace) final; + std::vector AddAllModels( const DataSource& data_source, const std::optional& parent_model_name, diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 7687ca8230ee..0db2613e49c5 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -1760,17 +1760,16 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { // Reports if the frame with the given id has a geometry with the given role // whose name is the same as what ShapeName(ShapeType{}) would produce. template -::testing::AssertionResult FrameHasShape(geometry::FrameId frame_id, - geometry::Role role, - const SceneGraph& scene_graph, - const ShapeType& shape) { +::testing::AssertionResult FrameHasShape( + geometry::FrameId frame_id, geometry::Role role, + const SceneGraph& scene_graph, const ShapeType& shape, + const std::string_view model_name = "test_robot") { const auto& inspector = scene_graph.model_inspector(); const std::string name = geometry::ShapeName(shape).name(); try { - // Note: MBP prepends the model index to the geometry name; in this case - // that model instance name is "test_robot". - const geometry::GeometryId geometry_id = - inspector.GetGeometryIdByName(frame_id, role, "test_robot::" + name); + // Note: MBP prepends the model index to the geometry name. + const geometry::GeometryId geometry_id = inspector.GetGeometryIdByName( + frame_id, role, fmt::format("{}::{}", model_name, name)); const std::string shape_type = geometry::ShapeName(inspector.GetShape(geometry_id)).name(); if (shape_type != name) { @@ -2731,13 +2730,21 @@ TEST_F(SdfParserTest, InterfaceAPI) { // default filters. { const auto& inspector = scene_graph_.model_inspector(); - static constexpr int kNumLinks = 4; + static constexpr int kNumGeometries = 5; + static constexpr int kNumProximityGeometries = 4; // Verify the number we expect and that they are all in proximity role. - ASSERT_EQ(kNumLinks, inspector.num_geometries()); - ASSERT_EQ(kNumLinks, + ASSERT_EQ(kNumGeometries , inspector.num_geometries()); + ASSERT_EQ(kNumProximityGeometries, inspector.NumGeometriesWithRole(geometry::Role::kProximity)); std::vector ids = inspector.GetAllGeometryIds(); - ASSERT_EQ(ids.size(), kNumLinks); + ASSERT_EQ(ids.size(), kNumGeometries); + + auto proxEnd = + std::remove_if(ids.begin(), ids.end(), [&inspector](auto id) { + return inspector.GetProximityProperties(id) == nullptr; + }); + ids.erase(proxEnd, ids.end()); + ASSERT_EQ(ids.size(), kNumProximityGeometries); // Make sure the plant is not finalized such that the Finalize() default // filtering has not taken into effect yet. This guarantees that the @@ -2770,6 +2777,16 @@ TEST_F(SdfParserTest, InterfaceAPI) { const RigidTransformd X_WL1 = arm_L1.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WA_expected.GetAsMatrix4(), X_WL1.GetAsMatrix4(), kEps)); + const auto frame_id = plant_.GetBodyFrameIdOrThrow(arm_L1.body().index()); + // Check that geometries have been created properly. + EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, + scene_graph_, geometry::Box{0.1, 0.1, 0.1}, + "top::arm")); + const auto& inspector = scene_graph_.model_inspector(); + EXPECT_NO_THROW(inspector.GetGeometryIdByName( + frame_id, geometry::Role::kPerception, "top::arm::Box")); + EXPECT_NO_THROW(inspector.GetGeometryIdByName( + frame_id, geometry::Role::kProximity, "top::arm::L1")); } // Test placement_frame using a table and a mug flipped upside down @@ -3172,6 +3189,231 @@ TEST_F(SdfParserTest, TestSingleModelEnforcement) { ClearDiagnostics(); } +// Verify merge-include works with Interface API. +TEST_F(SdfParserTest, BasicMergeIncludeInterfaceAPI) { + AddSceneGraph(); + const std::string full_name = FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "arm_with_gripper_merge_include.sdf"); + + // We start with the world and default model instances. + ASSERT_EQ(plant_.num_model_instances(), 2); + ASSERT_EQ(plant_.num_bodies(), 1); + ASSERT_EQ(plant_.num_joints(), 0); + + package_map_.PopulateFromFolder( + std::filesystem::path(full_name).parent_path()); + AddModelsFromSdfFile(full_name); + plant_.Finalize(); + + // We should have loaded *only* 1 more model. + EXPECT_EQ(plant_.num_model_instances(), 3); + EXPECT_EQ(plant_.num_bodies(), 4); + EXPECT_EQ(plant_.num_joints(), 3); + + const char* model_name = "arm_with_gripper"; + ASSERT_TRUE(plant_.HasModelInstanceNamed(model_name)); + ModelInstanceIndex arm_model = plant_.GetModelInstanceByName(model_name); + + // Check that the links and joints from arm.urdf are included in the model + // without introducing a nested scope, thus exercising the merge-include + // behavior through SDFormat's Interface API. + EXPECT_TRUE(plant_.HasBodyNamed("L1", arm_model)); + EXPECT_TRUE(plant_.HasBodyNamed("L2", arm_model)); + EXPECT_TRUE(plant_.HasJointNamed("J1", arm_model)); + + // Check gripper.sdf is included in the model without introducing a nested + // scope showing that both urdf and sdf files can be merge included into a + // single containing model. + EXPECT_TRUE(plant_.HasBodyNamed("gripper_link", arm_model)); +} + +// Two models are merge-included in the model loaded by this test. +// 1. In top_merge_include.sdf: arm_merge_include.forced_nesting_sdf is merge +// included. The merged model is contained in a parent model named +// "arm_urdf". The contents of arm.urdf are moved to "arm_urdf" such that +// "arm_urdf::L1" now references link "L1" from arm.urdf. +// 2. In arm_merge_include.forced_nesting_sdf: gripper.sdf is merge included +// whith puts "gripper_link" and "gripper_frame" in the scope of the parent +// model "arm". So instead of "arm::gripper::gripper_frame", we now have +// "arm::gripper_frame". +// 2. In top_merge_include.sdf: arm.urdf is merge included. The merged model is +// contained in a parent model named "arm_urdf". The contents of arm.urdf are +// moved to "arm_urdf" such that "arm_urdf::L1" now references link "L1" from +// arm.urdf. +void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant& plant, + const SceneGraph& scene_graph, + const std::string model_prefix) { + auto context = plant.CreateDefaultContext(); + EXPECT_FALSE( + plant.HasModelInstanceNamed(sdf::JoinName(model_prefix, "arm::gripper"))); + EXPECT_FALSE(plant.HasModelInstanceNamed( + sdf::JoinName(model_prefix, "arm_sdf::test_arm_sdf_name"))); + EXPECT_FALSE(plant.HasModelInstanceNamed( + sdf::JoinName(model_prefix, "arm_urdf::test_arm_urdf_name"))); + + EXPECT_FALSE(plant.HasModelInstanceNamed( + sdf::JoinName(model_prefix, "arm_sdf_name_override::test_arm_sdf_name"))); + EXPECT_FALSE(plant.HasModelInstanceNamed(sdf::JoinName( + model_prefix, "arm_urdf_name_override::test_arm_urdf_name"))); + + ASSERT_TRUE( + plant.HasModelInstanceNamed(sdf::JoinName(model_prefix, "arm_urdf"))); + const auto arm_urdf_model_instance = + plant.GetModelInstanceByName(sdf::JoinName(model_prefix, "arm_urdf")); + EXPECT_TRUE(plant.HasFrameNamed(sdf::computeMergedModelProxyFrameName("arm"), + arm_urdf_model_instance)); + + const auto arm_urdf_name_override_model_instance = + plant.GetModelInstanceByName( + sdf::JoinName(model_prefix, "arm_urdf_name_override")); + + EXPECT_TRUE(plant.HasFrameNamed( + sdf::computeMergedModelProxyFrameName("test_arm_urdf_name"), + arm_urdf_name_override_model_instance)); + + ASSERT_TRUE( + plant.HasModelInstanceNamed(sdf::JoinName(model_prefix, "arm_sdf"))); + const auto arm_sdf_model_instance = + plant.GetModelInstanceByName(sdf::JoinName(model_prefix, "arm_sdf")); + + // Pose of torso link + const RigidTransformd X_WT(RollPitchYawd(0, 0, 0), Vector3d(0, 0, 1)); + + { + // Frame G represents the frame of model top::arm_sdf::gripper_frame + const RigidTransformd X_WG_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 4)); + const auto& grasp_frame = + plant.GetFrameByName("grasp_frame", arm_sdf_model_instance); + + const RigidTransformd X_WG = grasp_frame.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WG_expected.GetAsMatrix4(), + X_WG.GetAsMatrix4(), kEps)); + } + { + // Frame A represents the model frame of model top::arm_sdf + const auto& arm_sdf_model_frame = + plant.GetFrameByName("__model__", arm_sdf_model_instance); + const RigidTransformd X_WA = arm_sdf_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE( + CompareMatrices(X_WT.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); + const RigidTransformd X_WL1_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(1, 0, 1)); + const auto& arm_L1 = plant.GetFrameByName("L1", arm_sdf_model_instance); + const RigidTransformd X_WL1 = arm_L1.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WL1_expected.GetAsMatrix4(), + X_WL1.GetAsMatrix4(), kEps)); + + // Check that geometries have been created properly. + const auto frame_id = plant.GetBodyFrameIdOrThrow(arm_L1.body().index()); + EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, + scene_graph, geometry::Box{0.1, 0.1, 0.1}, + sdf::JoinName(model_prefix, "arm_sdf"))); + EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kProximity, scene_graph, + geometry::Sphere{0.2}, + sdf::JoinName(model_prefix, "arm_sdf"))); + } + + { + // Frame E represents the model frame of model top::arm_urdf + const auto& arm_urdf_model_frame = + plant.GetFrameByName("__model__", arm_urdf_model_instance); + const RigidTransformd X_WE = arm_urdf_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE( + CompareMatrices(X_WT.GetAsMatrix4(), X_WE.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WL2_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 4, 4)); + const auto& arm_L2 = plant.GetFrameByName("L2", arm_urdf_model_instance); + const RigidTransformd X_WL2 = arm_L2.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WL2_expected.GetAsMatrix4(), + X_WL2.GetAsMatrix4(), kEps)); + + // Check that geometries have been created properly. + const auto frame_id = plant.GetBodyFrameIdOrThrow( + plant.GetBodyByName("L1", arm_urdf_model_instance).index()); + EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, + scene_graph, geometry::Box{0.1, 0.1, 0.1}, + sdf::JoinName(model_prefix, "arm_urdf"))); + } + { + // Frame F represents the model frame of model top::arm_sdf::flange + const RigidTransformd X_WF_expected(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(1, 2, 2)); + const auto flange_model_instance = plant.GetModelInstanceByName( + sdf::JoinName(model_prefix, "arm_sdf::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, 4)); + 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)); + } +} + +TEST_F(SdfParserTest, MergeIncludeInterfaceAPI1) { + AddSceneGraph(); + package_map_.AddPackageXml(FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "package.xml")); + const std::string sdf_file_path = FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "top_merge_include.sdf"); + + AddModelFromSdfFile(sdf_file_path, ""); + + plant_.Finalize(); + TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); +} + +TEST_F(SdfParserTest, MergeIncludeInterfaceAPI2) { + AddSceneGraph(); + package_map_.AddPackageXml(FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "package.xml")); + // Use AddModelsFromSdfFile (note the plural Models) + const std::string sdf_file_path = FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "top_merge_include_world.sdf"); + + AddModelsFromSdfFile(sdf_file_path); + + plant_.Finalize(); + TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); + TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "another_top"); +} + +TEST_F(SdfParserTest, MergeIncludeIntoWorld) { + AddSceneGraph(); + package_map_.AddPackageXml(FindResourceOrThrow( + "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" + "package.xml")); + const std::string sdf_string = R"""( + + + + + package://interface_api_test/top_merge_include_in_nested_model.sdf + + + + +)"""; + + AddModelsFromSdfString(sdf_string); + plant_.Finalize(); + TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); + TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "another_top"); +} } // namespace } // namespace internal } // namespace multibody 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 39ac36b7060b..c7681b1564f0 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 @@ -6,6 +6,11 @@ + + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf new file mode 100644 index 000000000000..75adf9500d8c --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf @@ -0,0 +1,57 @@ + + + + + + + + 0.1 0.1 0.1 + + + + + + + 0.2 + + + + + + 0 0 1 0 0 0 + L1 + L2 + + 0 0 1 + + + + + + + L2 + flange + + + + + 0 2 1 0 0 0 + + + 0 0 2 0.1 0.2 0.3 + + + + flange + + grasp_frame + + + + package://interface_api_test/gripper.sdf + gripper + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_with_gripper_merge_include.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_with_gripper_merge_include.sdf new file mode 100644 index 000000000000..621b51ef1d48 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_with_gripper_merge_include.sdf @@ -0,0 +1,22 @@ + + + + + + 0 2 0 0 0 0 + package://interface_api_test/arm.urdf + + + + 0 2 0 0 0 0 + package://interface_api_test/gripper.sdf + + + + L2 + gripper_link + + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf new file mode 100644 index 000000000000..96b57af0918f --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf @@ -0,0 +1,59 @@ + + + + + 0 0 1 0 0 0 + + + + + + + + + torso + arm_sdf::mount + + + + + + 1 0 0 0 0 0 + package://interface_api_test/arm_merge_include.sdf + + + + + + + + 0 2 0 0 0 0 + package://interface_api_test/arm.urdf + + + + + arm_sdf::L1 + arm_urdf::L1 + + + + + + + 1 0 0 0 0 0 + package://interface_api_test/arm_merge_include.sdf + test_arm_sdf_name + + + + + + + 0 2 0 0 0 0 + package://interface_api_test/arm.urdf + test_arm_urdf_name + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_in_nested_model.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_in_nested_model.sdf new file mode 100644 index 000000000000..b6fa5b6ccd57 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_in_nested_model.sdf @@ -0,0 +1,14 @@ + + + + + package://interface_api_test/top_merge_include.sdf + top + + + + package://interface_api_test/top_merge_include.sdf + another_top + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_world.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_world.sdf new file mode 100644 index 000000000000..df9d7b246902 --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include_world.sdf @@ -0,0 +1,14 @@ + + + + + package://interface_api_test/top_merge_include.sdf + top + + + + package://interface_api_test/top_merge_include.sdf + another_top + + + diff --git a/tools/workspace/sdformat_internal/repository.bzl b/tools/workspace/sdformat_internal/repository.bzl index dae58e475147..2bfa5ee91e7e 100644 --- a/tools/workspace/sdformat_internal/repository.bzl +++ b/tools/workspace/sdformat_internal/repository.bzl @@ -8,7 +8,7 @@ def sdformat_internal_repository( # This dependency is part of a "cohort" defined in # drake/tools/workspace/new_release.py. When practical, all members # of this cohort should be updated at the same time. - repository = "gazebosim/sdformat", + repository = "azeey/sdformat", commit = "sdformat14_14.0.0", build_file = ":package.BUILD.bazel", sha256 = "a4f38865f7e8b79559c238a1aba7bab1a4ff46c22a67bb91325ad48ba7d25728", # noqa From 082f549d66ad68d4de828b992f6bb51282b473a8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 21 Aug 2023 21:17:55 -0500 Subject: [PATCH 2/6] Address reviewer feedback. Signed-off-by: Addisu Z. Taddese --- multibody/parsing/detail_sdf_parser.cc | 41 ++++---- multibody/parsing/detail_urdf_parser.cc | 4 +- .../parsing/test/detail_sdf_parser_test.cc | 93 +++++-------------- .../interface_api_test/arm.urdf | 5 - 4 files changed, 51 insertions(+), 92 deletions(-) diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index bf523d866465..30cdc918270e 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -85,15 +85,16 @@ namespace { // populates the MultibodyPlant using the DOM objects created above (e.g. // AddModelsFromSpecification). // -// Data structure to hold model instances that were created during libsdformat's -// Interface API callback for handling merged models. -// When handling merged models during libsdformat's Interface API callback, we -// have to create model instances of parents of merged models since the parent -// models are regular SDFormat elements and will only be handled at a later -// stage of parsing. This data structure is used to store these parent model -// instances so that when we're going through Drake's parsing stage (last step -// in the parsing stage summary above), we are aware of these model instances -// and not try to recreate them. +// +// `ModelInstanceIndexRange` is a data structure to hold model instances that +// were created during libsdformat's Interface API callback for handling merged +// models. When handling merged models during libsdformat's Interface API +// callback, we have to create model instances of parents of merged models since +// the parent models are regular SDFormat elements and will only be handled at a +// later stage of parsing. This data structure is used to store these parent +// model instances so that when we're going through Drake's parsing stage (last +// step in the parsing stage summary above), we are aware of these model +// instances and not try to recreate them. using ModelInstanceIndexRange = std::pair; @@ -1386,7 +1387,9 @@ ModelInstanceIndex AddModelInstanceIfReusable( // Helper class that does bookkeeping on the frame bearing elements of a given // model instance in a MultibodyPlant. This is used to keep track of newly // created elements after a custom parser is called so that we can create -// Interface API data structures from just the newly created elements. Note: We +// Interface API data structures from just the newly created elements. +// The Interface API step is necessary so that we can construct the appropriate +// pose graph at higher levels of the hierarchy using libsdformat. Note: We // assume that Drake custom parsers do not support nested models. // TODO(azeey) If any of Drakes custom parsers start supporting nested models, // this needs to be updated to keep track of them. @@ -1753,11 +1756,10 @@ sdf::InterfaceModelPtr ConvertToInterfaceModel( const auto& model_frame = plant->GetFrameByName( interface_model_helper.model_frame_name, model_instance); - std::string canonical_link_name = + const std::string canonical_link_name = GetRelativeBodyName(model_frame.body(), model_instance, *plant); - RigidTransformd X_PM = RigidTransformd::Identity(); - RigidTransformd X_WM = GetDefaultFramePose(*plant, model_frame); - X_PM = X_WP.inverse() * X_WM; + const RigidTransformd X_WM = GetDefaultFramePose(*plant, model_frame); + const RigidTransformd X_PM = X_WP.inverse() * X_WM; auto interface_model = std::make_shared( model_name, reposture_model, false, canonical_link_name, @@ -1820,15 +1822,15 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( ModelInstanceIndex main_model_instance; // New instances will have indices starting from cur_num_models - const bool isMergeInclude = include.IsMerge().value_or(false); + const bool is_merge_include = include.IsMerge().value_or(false); - InterfaceModelHelper interface_model_helper(*plant, isMergeInclude); + InterfaceModelHelper interface_model_helper(*plant, is_merge_include); ParsingWorkspace subworkspace{options, package_map, subdiagnostic, plant, collision_resolver, parser_selector}; std::string model_frame_name = "__model__"; std::string model_name; - if (isMergeInclude) { + if (is_merge_include) { // Create the parent model instance if it hasn't been created already. // This can happen if this is the first model to be merge-included. const auto parent_model_instance = @@ -1860,6 +1862,11 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( ScopedName::Parse(plant->GetModelInstanceName(main_model_instance)) .get_element(); + // In the non-merge-include case, we don't need to compute the diff, but we + // still use the interface_model_helper to hold all the frame bearing + // elements of the newly created model instance so we can later convert + // this model instance to an Interface Model for libsdformat's Interface + // API. interface_model_helper.TakeSnapShot(main_model_instance); } DRAKE_DEMAND(!model_name.empty()); diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 5836994403fc..d0a4463b1237 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -62,13 +62,13 @@ class UrdfParser { const DataSource* data_source, const std::string& model_name, const std::optional& parent_model_name, - std::optional model_instance, + std::optional merge_into_model_instance, const std::string& root_dir, XMLDocument* xml_doc, const ParsingWorkspace& w) : model_name_(model_name), parent_model_name_(parent_model_name), - merge_into_model_instance_(model_instance), + merge_into_model_instance_(merge_into_model_instance), root_dir_(root_dir), xml_doc_(xml_doc), w_(w), diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 0db2613e49c5..01c713ef558f 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -1760,16 +1760,17 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { // Reports if the frame with the given id has a geometry with the given role // whose name is the same as what ShapeName(ShapeType{}) would produce. template -::testing::AssertionResult FrameHasShape( - geometry::FrameId frame_id, geometry::Role role, - const SceneGraph& scene_graph, const ShapeType& shape, - const std::string_view model_name = "test_robot") { +::testing::AssertionResult FrameHasShape(geometry::FrameId frame_id, + geometry::Role role, + const SceneGraph& scene_graph, + const ShapeType& shape) { const auto& inspector = scene_graph.model_inspector(); const std::string name = geometry::ShapeName(shape).name(); try { - // Note: MBP prepends the model index to the geometry name. - const geometry::GeometryId geometry_id = inspector.GetGeometryIdByName( - frame_id, role, fmt::format("{}::{}", model_name, name)); + // Note: MBP prepends the model index to the geometry name; in this case + // that model instance name is "test_robot". + const geometry::GeometryId geometry_id = + inspector.GetGeometryIdByName(frame_id, role, "test_robot::" + name); const std::string shape_type = geometry::ShapeName(inspector.GetShape(geometry_id)).name(); if (shape_type != name) { @@ -2715,7 +2716,7 @@ TEST_F(SdfParserTest, FramesAsJointParentOrChild) { // 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. -TEST_F(SdfParserTest, InterfaceAPI) { +TEST_F(SdfParserTest, InterfaceApi) { AddSceneGraph(); const std::string sdf_file_path = FindResourceOrThrow( "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" @@ -2730,21 +2731,13 @@ TEST_F(SdfParserTest, InterfaceAPI) { // default filters. { const auto& inspector = scene_graph_.model_inspector(); - static constexpr int kNumGeometries = 5; - static constexpr int kNumProximityGeometries = 4; + static constexpr int kNumLinks = 4; // Verify the number we expect and that they are all in proximity role. - ASSERT_EQ(kNumGeometries , inspector.num_geometries()); - ASSERT_EQ(kNumProximityGeometries, + ASSERT_EQ(kNumLinks, inspector.num_geometries()); + ASSERT_EQ(kNumLinks, inspector.NumGeometriesWithRole(geometry::Role::kProximity)); std::vector ids = inspector.GetAllGeometryIds(); - ASSERT_EQ(ids.size(), kNumGeometries); - - auto proxEnd = - std::remove_if(ids.begin(), ids.end(), [&inspector](auto id) { - return inspector.GetProximityProperties(id) == nullptr; - }); - ids.erase(proxEnd, ids.end()); - ASSERT_EQ(ids.size(), kNumProximityGeometries); + ASSERT_EQ(ids.size(), kNumLinks); // Make sure the plant is not finalized such that the Finalize() default // filtering has not taken into effect yet. This guarantees that the @@ -2777,16 +2770,6 @@ TEST_F(SdfParserTest, InterfaceAPI) { const RigidTransformd X_WL1 = arm_L1.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WA_expected.GetAsMatrix4(), X_WL1.GetAsMatrix4(), kEps)); - const auto frame_id = plant_.GetBodyFrameIdOrThrow(arm_L1.body().index()); - // Check that geometries have been created properly. - EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, - scene_graph_, geometry::Box{0.1, 0.1, 0.1}, - "top::arm")); - const auto& inspector = scene_graph_.model_inspector(); - EXPECT_NO_THROW(inspector.GetGeometryIdByName( - frame_id, geometry::Role::kPerception, "top::arm::Box")); - EXPECT_NO_THROW(inspector.GetGeometryIdByName( - frame_id, geometry::Role::kProximity, "top::arm::L1")); } // Test placement_frame using a table and a mug flipped upside down @@ -3190,7 +3173,7 @@ TEST_F(SdfParserTest, TestSingleModelEnforcement) { } // Verify merge-include works with Interface API. -TEST_F(SdfParserTest, BasicMergeIncludeInterfaceAPI) { +TEST_F(SdfParserTest, BasicMergeIncludeInterfaceApi) { AddSceneGraph(); const std::string full_name = FindResourceOrThrow( "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" @@ -3228,20 +3211,7 @@ TEST_F(SdfParserTest, BasicMergeIncludeInterfaceAPI) { EXPECT_TRUE(plant_.HasBodyNamed("gripper_link", arm_model)); } -// Two models are merge-included in the model loaded by this test. -// 1. In top_merge_include.sdf: arm_merge_include.forced_nesting_sdf is merge -// included. The merged model is contained in a parent model named -// "arm_urdf". The contents of arm.urdf are moved to "arm_urdf" such that -// "arm_urdf::L1" now references link "L1" from arm.urdf. -// 2. In arm_merge_include.forced_nesting_sdf: gripper.sdf is merge included -// whith puts "gripper_link" and "gripper_frame" in the scope of the parent -// model "arm". So instead of "arm::gripper::gripper_frame", we now have -// "arm::gripper_frame". -// 2. In top_merge_include.sdf: arm.urdf is merge included. The merged model is -// contained in a parent model named "arm_urdf". The contents of arm.urdf are -// moved to "arm_urdf" such that "arm_urdf::L1" now references link "L1" from -// arm.urdf. -void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant& plant, +void TestMergeIncludeWithInterfaceApi(const MultibodyPlant& plant, const SceneGraph& scene_graph, const std::string model_prefix) { auto context = plant.CreateDefaultContext(); @@ -3304,15 +3274,6 @@ void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant& plant, const RigidTransformd X_WL1 = arm_L1.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WL1_expected.GetAsMatrix4(), X_WL1.GetAsMatrix4(), kEps)); - - // Check that geometries have been created properly. - const auto frame_id = plant.GetBodyFrameIdOrThrow(arm_L1.body().index()); - EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, - scene_graph, geometry::Box{0.1, 0.1, 0.1}, - sdf::JoinName(model_prefix, "arm_sdf"))); - EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kProximity, scene_graph, - geometry::Sphere{0.2}, - sdf::JoinName(model_prefix, "arm_sdf"))); } { @@ -3329,13 +3290,6 @@ void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant& plant, const RigidTransformd X_WL2 = arm_L2.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WL2_expected.GetAsMatrix4(), X_WL2.GetAsMatrix4(), kEps)); - - // Check that geometries have been created properly. - const auto frame_id = plant.GetBodyFrameIdOrThrow( - plant.GetBodyByName("L1", arm_urdf_model_instance).index()); - EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, - scene_graph, geometry::Box{0.1, 0.1, 0.1}, - sdf::JoinName(model_prefix, "arm_urdf"))); } { // Frame F represents the model frame of model top::arm_sdf::flange @@ -3360,7 +3314,7 @@ void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant& plant, } } -TEST_F(SdfParserTest, MergeIncludeInterfaceAPI1) { +TEST_F(SdfParserTest, MergeIncludeInterfaceApi1) { AddSceneGraph(); package_map_.AddPackageXml(FindResourceOrThrow( "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" @@ -3372,10 +3326,11 @@ TEST_F(SdfParserTest, MergeIncludeInterfaceAPI1) { AddModelFromSdfFile(sdf_file_path, ""); plant_.Finalize(); - TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); + SCOPED_TRACE("MergeIncludeInterfaceApi1"); + TestMergeIncludeWithInterfaceApi(plant_, scene_graph_, "top"); } -TEST_F(SdfParserTest, MergeIncludeInterfaceAPI2) { +TEST_F(SdfParserTest, MergeIncludeInterfaceApi2) { AddSceneGraph(); package_map_.AddPackageXml(FindResourceOrThrow( "drake/multibody/parsing/test/sdf_parser_test/interface_api_test/" @@ -3388,8 +3343,9 @@ TEST_F(SdfParserTest, MergeIncludeInterfaceAPI2) { AddModelsFromSdfFile(sdf_file_path); plant_.Finalize(); - TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); - TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "another_top"); + SCOPED_TRACE("MergeIncludeInterfaceApi2"); + TestMergeIncludeWithInterfaceApi(plant_, scene_graph_, "top"); + TestMergeIncludeWithInterfaceApi(plant_, scene_graph_, "another_top"); } TEST_F(SdfParserTest, MergeIncludeIntoWorld) { @@ -3411,8 +3367,9 @@ TEST_F(SdfParserTest, MergeIncludeIntoWorld) { AddModelsFromSdfString(sdf_string); plant_.Finalize(); - TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "top"); - TestMergeIncludeWithInterfaceAPI(plant_, scene_graph_, "another_top"); + SCOPED_TRACE("MergeIncludeIntoWorld"); + TestMergeIncludeWithInterfaceApi(plant_, scene_graph_, "top"); + TestMergeIncludeWithInterfaceApi(plant_, scene_graph_, "another_top"); } } // namespace } // namespace internal 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 c7681b1564f0..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 @@ -6,11 +6,6 @@ - - - - - From 3c6ca1c502630772de89315f19fee50ede69f761 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 22 Aug 2023 15:26:42 -0500 Subject: [PATCH 3/6] Add merge-include support for the MuJoCo parser Signed-off-by: Addisu Z. Taddese --- multibody/parsing/BUILD.bazel | 1 + multibody/parsing/detail_mujoco_parser.cc | 51 ++++++++++++++----- multibody/parsing/detail_mujoco_parser.h | 5 ++ multibody/parsing/detail_parsing_workspace.h | 10 ++-- multibody/parsing/detail_sdf_parser.cc | 9 ++-- multibody/parsing/detail_urdf_parser.cc | 44 ++++++++-------- multibody/parsing/detail_urdf_parser.h | 2 +- .../parsing/test/detail_sdf_parser_test.cc | 33 +++++++++++- .../interface_api_test/arm.xml | 13 +++++ .../interface_api_test/top_merge_include.sdf | 8 +++ 10 files changed, 129 insertions(+), 47 deletions(-) create mode 100644 multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel index 613aee74d063..1e72f3c76751 100644 --- a/multibody/parsing/BUILD.bazel +++ b/multibody/parsing/BUILD.bazel @@ -529,6 +529,7 @@ drake_cc_googletest( "//multibody/benchmarks/acrobot:models", ], deps = [ + ":detail_mujoco_parser", ":detail_sdf_parser", "//common:find_resource", "//common/test_utilities", diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc index 1239ac809ec0..3b3c6a1ec022 100644 --- a/multibody/parsing/detail_mujoco_parser.cc +++ b/multibody/parsing/detail_mujoco_parser.cc @@ -1277,10 +1277,11 @@ class MujocoParser { // Assets without an absolute path are referenced relative to the "main MJCF // model file" path, `main_mjcf_path`. - std::optional Parse(const std::string& model_name_in, - const std::optional& parent_model_name, - XMLDocument* xml_doc, - const std::filesystem::path& main_mjcf_path) { + std::pair, std::string> Parse( + const std::string& model_name_in, + const std::optional& parent_model_name, + std::optional merge_into_model_instance, + XMLDocument* xml_doc, const std::filesystem::path& main_mjcf_path) { main_mjcf_path_ = main_mjcf_path; XMLElement* node = xml_doc->FirstChildElement("mujoco"); @@ -1297,8 +1298,13 @@ class MujocoParser { "must be specified."); return {}; } - model_name = MakeModelName(model_name, parent_model_name, workspace_); - model_instance_ = plant_->AddModelInstance(model_name); + + if (!merge_into_model_instance.has_value()) { + model_name = MakeModelName(model_name, parent_model_name, workspace_); + model_instance_ = plant_->AddModelInstance(model_name); + } else { + model_instance_ = *merge_into_model_instance; + } // Parse the compiler parameters. for (XMLElement* compiler_node = node->FirstChildElement("compiler"); @@ -1362,7 +1368,7 @@ class MujocoParser { WarnUnsupportedElement(*node, "sensor"); WarnUnsupportedElement(*node, "keyframe"); - return model_instance_; + return std::make_pair(model_instance_, model_name); } void Warning(const XMLNode& location, std::string message) const { @@ -1402,12 +1408,12 @@ class MujocoParser { std::map> mesh_inertia_; }; -} // namespace - -std::optional AddModelFromMujocoXml( +std::pair, std::string> +AddOrMergeModelFromMujocoXml( const DataSource& data_source, const std::string& model_name_in, const std::optional& parent_model_name, - const ParsingWorkspace& workspace) { + const ParsingWorkspace& workspace, + std::optional merge_into_model_instance) { DRAKE_THROW_UNLESS(!workspace.plant->is_finalized()); TinyXml2Diagnostic diag(&workspace.diagnostic, &data_source); @@ -1434,7 +1440,18 @@ std::optional AddModelFromMujocoXml( } MujocoParser parser(workspace, data_source); - return parser.Parse(model_name_in, parent_model_name, &xml_doc, path); + return parser.Parse(model_name_in, parent_model_name, + merge_into_model_instance, &xml_doc, path); +} +} // namespace + +std::optional AddModelFromMujocoXml( + const DataSource& data_source, const std::string& model_name_in, + const std::optional& parent_model_name, + const ParsingWorkspace& workspace) { + return AddOrMergeModelFromMujocoXml(data_source, model_name_in, + parent_model_name, workspace, + std::nullopt).first; } MujocoParserWrapper::MujocoParserWrapper() {} @@ -1449,6 +1466,16 @@ std::optional MujocoParserWrapper::AddModel( workspace); } +std::string MujocoParserWrapper::MergeModel( + const DataSource& data_source, const std::string& model_name, + ModelInstanceIndex merge_into_model_instance, + const ParsingWorkspace& workspace) { + return AddOrMergeModelFromMujocoXml(data_source, model_name, std::nullopt, + workspace, merge_into_model_instance) + .second; +} + + std::vector MujocoParserWrapper::AddAllModels( const DataSource& data_source, const std::optional& parent_model_name, diff --git a/multibody/parsing/detail_mujoco_parser.h b/multibody/parsing/detail_mujoco_parser.h index a944ee7851ed..41bace892b7e 100644 --- a/multibody/parsing/detail_mujoco_parser.h +++ b/multibody/parsing/detail_mujoco_parser.h @@ -58,6 +58,11 @@ class MujocoParserWrapper final : public ParserInterface { const std::optional& parent_model_name, const ParsingWorkspace& workspace) final; + std::string MergeModel(const DataSource& data_source, + const std::string& model_name, + ModelInstanceIndex merge_into_model_instance, + const ParsingWorkspace& workspace) final; + std::vector AddAllModels( const DataSource& data_source, const std::optional& parent_model_name, diff --git a/multibody/parsing/detail_parsing_workspace.h b/multibody/parsing/detail_parsing_workspace.h index d22d99c170a6..fce82af52264 100644 --- a/multibody/parsing/detail_parsing_workspace.h +++ b/multibody/parsing/detail_parsing_workspace.h @@ -60,7 +60,7 @@ class ParserInterface { // @param model_name // The name given to the newly created instance of this model. If // empty, the model name found within the model data will be used. - // @param model_instance + // @param merge_into_model_instance // The model instance into which the contents of the model will be added. // @param workspace // The ParsingWorkspace. @@ -68,10 +68,10 @@ class ParserInterface { // Note, this is not the name of the @p model_instance. Instead, it's the // name of the original model that has now been merged into @p // model_instance. - virtual std::string MergeModel(const DataSource& /* data_source */, - const std::string& /* model_name */, - ModelInstanceIndex /* model_instance */, - const ParsingWorkspace& /* workspace */) { + virtual std::string MergeModel( + const DataSource& /* data_source */, const std::string& /* model_name */, + ModelInstanceIndex /* merge_into_model_instance*/, + const ParsingWorkspace& /* workspace */) { throw std::runtime_error( fmt::format("MergeModel is not implemented for this input type")); } diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 30cdc918270e..8cbbafae8a2f 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -1,6 +1,5 @@ #include "drake/multibody/parsing/detail_sdf_parser.h" -#include #include #include #include @@ -1661,6 +1660,7 @@ RigidTransformd GetDefaultFramePose( // For the libsdformat API, see: // http://sdformat.org/tutorials?tut=composition_proposal constexpr char kExtUrdf[] = ".urdf"; +constexpr char kExtXml[] = ".xml"; void AddBodiesToInterfaceModel(const MultibodyPlant& plant, const sdf::InterfaceModelPtr& interface_model, @@ -1792,9 +1792,10 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( collision_resolver, parser_selector] = workspace; const std::string resolved_filename{include.ResolvedFileName()}; - // Do not attempt to parse anything other than URDF files. + // Do not attempt to parse anything other than URDF and MuJoCo xml files. const bool is_urdf = EndsWithCaseInsensitive(resolved_filename, kExtUrdf); - if (!is_urdf) { + const bool is_xml = EndsWithCaseInsensitive(resolved_filename, kExtXml); + if (!is_urdf && !is_xml) { // TODO(rpoyner-tri): implement nesting of mujoco files; saved for another // day since it requires some study of mujoco scene composition semantics. return nullptr; @@ -1875,7 +1876,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( auto body_indices = workspace.plant->GetBodyIndices(main_model_instance); if (body_indices.empty()) { errors->emplace_back(sdf::ErrorCode::ELEMENT_INVALID, - "URDF must have at least one link."); + "Model must have at least one link."); return nullptr; } const auto& canonical_link = workspace.plant->get_body(body_indices[0]); diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index d0a4463b1237..91caa5acc8c3 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -77,11 +77,12 @@ class UrdfParser { DRAKE_DEMAND(xml_doc != nullptr); } - // @return a model instance index, if one was created during parsing. + // @return a model instance index, if one was created during parsing, and name + // of model either passed in or parsed from document. // @throw std::exception on parse error. // @note: see AddModelFromUrdf for a full account of diagnostics, error // reporting, and return values. - std::optional Parse(); + std::pair, std::string> Parse(); void ParseBushing(XMLElement* node); void ParseBallConstraint(XMLElement* node); void ParseFrame(XMLElement* node); @@ -138,9 +139,9 @@ class UrdfParser { const std::string& model_name() { return model_name_; } private: - std::string model_name_; + const std::string model_name_; const std::optional parent_model_name_; - std::optional merge_into_model_instance_; + const std::optional merge_into_model_instance_; const std::string root_dir_; XMLDocument* const xml_doc_; const ParsingWorkspace& w_; @@ -977,7 +978,7 @@ void UrdfParser::ParseBallConstraint(XMLElement* node) { internal::ParseBallConstraint(read_vector, read_body, w_.plant); } -std::optional UrdfParser::Parse() { +std::pair, std::string> UrdfParser::Parse() { XMLElement* node = xml_doc_->FirstChildElement("robot"); if (!node) { Error(*xml_doc_, "URDF does not contain a robot tag."); @@ -1001,8 +1002,6 @@ std::optional UrdfParser::Parse() { model_instance_ = *merge_into_model_instance_; } - model_name_ = 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. @@ -1054,7 +1053,7 @@ std::optional UrdfParser::Parse() { if (node->FirstChildElement("loop_joint")) { Error(*node, "loop joints are not supported in MultibodyPlant"); - return model_instance_; + return std::make_pair(model_instance_, model_name); } // Parses the model's Drake frame elements. @@ -1080,15 +1079,15 @@ std::optional UrdfParser::Parse() { ParseBallConstraint(ball_constraint_node); } - return model_instance_; + return std::make_pair(model_instance_, model_name); } std::pair, std::string> -AddOrMergeModelFromUrdf(const DataSource& data_source, - const std::string& model_name_in, - const std::optional& parent_model_name, - const ParsingWorkspace& workspace, - std::optional model_instance) { +AddOrMergeModelFromUrdf( + const DataSource& data_source, const std::string& model_name_in, + const std::optional& parent_model_name, + const ParsingWorkspace& workspace, + std::optional merge_into_model_instance) { MultibodyPlant* plant = workspace.plant; DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_THROW_UNLESS(!plant->is_finalized()); @@ -1113,10 +1112,9 @@ AddOrMergeModelFromUrdf(const DataSource& data_source, } UrdfParser parser(&data_source, model_name_in, parent_model_name, - model_instance, data_source.GetRootDir(), &xml_doc, - workspace); - auto added_model_instance = parser.Parse(); - return std::make_pair(added_model_instance, parser.model_name()); + merge_into_model_instance, data_source.GetRootDir(), + &xml_doc, workspace); + return parser.Parse();; } } // namespace @@ -1141,12 +1139,12 @@ std::optional UrdfParserWrapper::AddModel( workspace); } -std::string UrdfParserWrapper::MergeModel(const DataSource& data_source, - const std::string& model_name, - ModelInstanceIndex model_instance, - const ParsingWorkspace& workspace) { +std::string UrdfParserWrapper::MergeModel( + const DataSource& data_source, const std::string& model_name, + ModelInstanceIndex merge_into_model_instance, + const ParsingWorkspace& workspace) { return AddOrMergeModelFromUrdf(data_source, model_name, std::nullopt, - workspace, model_instance).second; + workspace, merge_into_model_instance).second; } std::vector UrdfParserWrapper::AddAllModels( diff --git a/multibody/parsing/detail_urdf_parser.h b/multibody/parsing/detail_urdf_parser.h index aea6f355a1ad..4affeadbd7fd 100644 --- a/multibody/parsing/detail_urdf_parser.h +++ b/multibody/parsing/detail_urdf_parser.h @@ -62,7 +62,7 @@ class UrdfParserWrapper final : public ParserInterface { std::string MergeModel(const DataSource& data_source, const std::string& model_name, - ModelInstanceIndex model_instance, + ModelInstanceIndex merge_into_model_instance, const ParsingWorkspace& workspace) final; std::vector AddAllModels( diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 01c713ef558f..5c26dec2cd6b 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -22,6 +22,7 @@ #include "drake/geometry/shape_specification.h" #include "drake/math/rigid_transform.h" #include "drake/math/roll_pitch_yaw.h" +#include "drake/multibody/parsing/detail_mujoco_parser.h" #include "drake/multibody/parsing/detail_path_utils.h" #include "drake/multibody/parsing/detail_urdf_parser.h" #include "drake/multibody/plant/multibody_plant.h" @@ -72,10 +73,18 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ } static ParserInterface& TestingSelect(const DiagnosticPolicy&, - const std::string&) { + const std::string& filename) { // TODO(rpoyner-tri): add more formats here, as tests use them. static never_destroyed urdf; - return urdf.access(); + static never_destroyed mujoco; + if (EndsWithCaseInsensitive(filename, ".urdf")) { + return urdf.access(); + } + if (EndsWithCaseInsensitive(filename, ".xml")) { + return mujoco.access(); + } + throw std::runtime_error(fmt::format( + "Unsupported file format in unittest for file ({})", filename)); } @@ -3247,6 +3256,11 @@ void TestMergeIncludeWithInterfaceApi(const MultibodyPlant& plant, const auto arm_sdf_model_instance = plant.GetModelInstanceByName(sdf::JoinName(model_prefix, "arm_sdf")); + ASSERT_TRUE( + plant.HasModelInstanceNamed(sdf::JoinName(model_prefix, "arm_mjcf"))); + const auto arm_mjcf_model_instance = + plant.GetModelInstanceByName(sdf::JoinName(model_prefix, "arm_mjcf")); + // Pose of torso link const RigidTransformd X_WT(RollPitchYawd(0, 0, 0), Vector3d(0, 0, 1)); @@ -3312,6 +3326,21 @@ void TestMergeIncludeWithInterfaceApi(const MultibodyPlant& plant, EXPECT_TRUE(CompareMatrices(X_WM_expected.GetAsMatrix4(), X_WM.GetAsMatrix4(), kEps)); } + { + // Frame G represents the model frame of model top::arm_mjcf + const auto& arm_mjcf_model_frame = + plant.GetFrameByName("__model__", arm_mjcf_model_instance); + const RigidTransformd X_WG = arm_mjcf_model_frame.CalcPoseInWorld(*context); + EXPECT_TRUE( + CompareMatrices(X_WT.GetAsMatrix4(), X_WG.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WL2_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(11, 4, 4)); + const auto& arm_L2 = plant.GetFrameByName("L2", arm_mjcf_model_instance); + const RigidTransformd X_WL2 = arm_L2.CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WL2_expected.GetAsMatrix4(), + X_WL2.GetAsMatrix4(), kEps)); + } } TEST_F(SdfParserTest, MergeIncludeInterfaceApi1) { diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml new file mode 100644 index 000000000000..a27e604a1d8b --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf index 96b57af0918f..0c8e592ce6a6 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/top_merge_include.sdf @@ -55,5 +55,13 @@ test_arm_urdf_name + + + + + 10 2 0 0 0 0 + package://interface_api_test/arm.xml + + From df146dddb692d45d401f6648102608c78360b82f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 23 Aug 2023 10:14:27 -0500 Subject: [PATCH 4/6] Add SCOPED_TRACE in TestMergeIncludeWithInterfaceApi --- multibody/parsing/test/detail_sdf_parser_test.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 5c26dec2cd6b..9e55e10d6560 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -3223,6 +3223,7 @@ TEST_F(SdfParserTest, BasicMergeIncludeInterfaceApi) { void TestMergeIncludeWithInterfaceApi(const MultibodyPlant& plant, const SceneGraph& scene_graph, const std::string model_prefix) { + SCOPED_TRACE(model_prefix); auto context = plant.CreateDefaultContext(); EXPECT_FALSE( plant.HasModelInstanceNamed(sdf::JoinName(model_prefix, "arm::gripper"))); From f1731d74923b7250add659f2b9124570de085e28 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 Sep 2023 10:13:32 -0500 Subject: [PATCH 5/6] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- multibody/parsing/detail_parsing_workspace.h | 2 +- multibody/parsing/detail_sdf_parser.cc | 5 +---- .../parsing/test/sdf_parser_test/interface_api_test/arm.xml | 1 - .../sdf_parser_test/interface_api_test/arm_merge_include.sdf | 2 +- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/multibody/parsing/detail_parsing_workspace.h b/multibody/parsing/detail_parsing_workspace.h index fce82af52264..9ee7cb775211 100644 --- a/multibody/parsing/detail_parsing_workspace.h +++ b/multibody/parsing/detail_parsing_workspace.h @@ -51,7 +51,7 @@ class ParserInterface { const std::optional& parent_model_name, const ParsingWorkspace& workspace) = 0; - // Parses a model from the input file specified by @p data_source add all of + // Parses a model from the input file specified by @p data_source adds all of // its contents into an existing model instance specified by @p // model_instance. @p model_instance must already exist in @p plant. // diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 8cbbafae8a2f..bfe42c2415f3 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -83,8 +83,7 @@ namespace { // * `sdf::Root::Load` returns and Drake starts its parsing stage where it // populates the MultibodyPlant using the DOM objects created above (e.g. // AddModelsFromSpecification). -// -// + // `ModelInstanceIndexRange` is a data structure to hold model instances that // were created during libsdformat's Interface API callback for handling merged // models. When handling merged models during libsdformat's Interface API @@ -1796,8 +1795,6 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( const bool is_urdf = EndsWithCaseInsensitive(resolved_filename, kExtUrdf); const bool is_xml = EndsWithCaseInsensitive(resolved_filename, kExtXml); if (!is_urdf && !is_xml) { - // TODO(rpoyner-tri): implement nesting of mujoco files; saved for another - // day since it requires some study of mujoco scene composition semantics. return nullptr; } diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml index a27e604a1d8b..05d063410758 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm.xml @@ -10,4 +10,3 @@ - diff --git a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf index 75adf9500d8c..4022af2c9f45 100644 --- a/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf +++ b/multibody/parsing/test/sdf_parser_test/interface_api_test/arm_merge_include.sdf @@ -1,5 +1,5 @@ - + From e3afd5deb23f809aa53c99dfacb3a98cd7a42c08 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 4 Oct 2023 16:02:51 -0500 Subject: [PATCH 6/6] Make `InterfaceModelHelper` a class to prevent breaking invariants Signed-off-by: Addisu Z. Taddese --- multibody/parsing/detail_sdf_parser.cc | 97 +++++++++++-------- .../sdformat_internal/repository.bzl | 2 +- 2 files changed, 57 insertions(+), 42 deletions(-) diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index bfe42c2415f3..5518b29949a8 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -1391,29 +1391,18 @@ ModelInstanceIndex AddModelInstanceIfReusable( // assume that Drake custom parsers do not support nested models. // TODO(azeey) If any of Drakes custom parsers start supporting nested models, // this needs to be updated to keep track of them. -struct InterfaceModelHelper { - const MultibodyPlant& plant; - bool is_merged; - ModelInstanceIndex model_instance; - std::string name; - std::vector body_indices; - std::vector frame_indices; - std::vector joint_indices; - std::vector model_instance_indices; - std::string model_frame_name = "__model__"; - - bool have_snapshot{false}; - - InterfaceModelHelper(const MultibodyPlant& pt, bool merged) - : plant(pt), is_merged(merged) {} +class InterfaceModelHelper { + public: + explicit InterfaceModelHelper(const MultibodyPlant& pt) + : plant_(pt) {} std::vector GetChildModelInstanceIndices() const { std::vector output; - for (ModelInstanceIndex mi(model_instance + 1); - mi < plant.num_model_instances(); ++mi) { - if (multibody::ScopedName::Parse(plant.GetModelInstanceName(mi)) - .get_element() == name) { + for (ModelInstanceIndex mi(model_instance_ + 1); + mi < plant_.num_model_instances(); ++mi) { + if (multibody::ScopedName::Parse(plant_.GetModelInstanceName(mi)) + .get_element() == name_) { output.push_back(mi); } } @@ -1423,12 +1412,12 @@ struct InterfaceModelHelper { // Take a snapshot of the current contents of a model instance so that we // later on do a diff void TakeSnapShot(ModelInstanceIndex model_instance_index) { - model_instance = model_instance_index; - name = plant.GetModelInstanceName(model_instance); - body_indices = plant.GetBodyIndices(model_instance); - frame_indices = plant.GetFrameIndices(model_instance); - joint_indices = plant.GetJointIndices(model_instance); - model_instance_indices = GetChildModelInstanceIndices(); + model_instance_ = model_instance_index; + name_ = plant_.GetModelInstanceName(model_instance_); + body_indices_ = plant_.GetBodyIndices(model_instance_); + frame_indices_ = plant_.GetFrameIndices(model_instance_); + joint_indices_ = plant_.GetJointIndices(model_instance_); + model_instance_indices_ = GetChildModelInstanceIndices(); have_snapshot = true; } @@ -1445,15 +1434,41 @@ struct InterfaceModelHelper { // snapshot. void ComputeDiffFromSnapshot() { DRAKE_DEMAND(have_snapshot); - body_indices = - GetVectorDiff(plant.GetBodyIndices(model_instance), body_indices); - frame_indices = - GetVectorDiff(plant.GetFrameIndices(model_instance), frame_indices); - joint_indices = - GetVectorDiff(plant.GetJointIndices(model_instance), joint_indices); - model_instance_indices = - GetVectorDiff(GetChildModelInstanceIndices(), model_instance_indices); + body_indices_ = + GetVectorDiff(plant_.GetBodyIndices(model_instance_), body_indices_); + frame_indices_ = + GetVectorDiff(plant_.GetFrameIndices(model_instance_), frame_indices_); + joint_indices_ = + GetVectorDiff(plant_.GetJointIndices(model_instance_), joint_indices_); + model_instance_indices_ = + GetVectorDiff(GetChildModelInstanceIndices(), model_instance_indices_); + } + + ModelInstanceIndex model_instance() const { return model_instance_; } + + const std::vector& joint_indices() const { + return joint_indices_; + } + const std::vector& frame_indices() const { + return frame_indices_; + } + const std::vector& body_indices() const { return body_indices_; } + const std::string& model_frame_name() const { return model_frame_name_; } + void set_model_frame_name(const std::string& model_frame_name) { + model_frame_name_ = model_frame_name; } + + private: + const MultibodyPlant& plant_; + ModelInstanceIndex model_instance_; + std::string name_; + std::vector body_indices_; + std::vector frame_indices_; + std::vector joint_indices_; + std::vector model_instance_indices_; + std::string model_frame_name_ = "__model__"; + + bool have_snapshot{false}; }; // Helper method to add a model to a MultibodyPlant given an sdf::Model @@ -1737,7 +1752,7 @@ sdf::InterfaceModelPtr ConvertToInterfaceModel( MultibodyPlant* plant, std::string model_name, const InterfaceModelHelper& interface_model_helper, sdf::Errors* errors, RigidTransformd X_WP = RigidTransformd::Identity()) { - auto model_instance = interface_model_helper.model_instance; + auto model_instance = interface_model_helper.model_instance(); sdf::RepostureFunction reposture_model = [plant, model_instance, errors](const sdf::InterfaceModelPoseGraph& graph) { @@ -1754,7 +1769,7 @@ sdf::InterfaceModelPtr ConvertToInterfaceModel( }; const auto& model_frame = plant->GetFrameByName( - interface_model_helper.model_frame_name, model_instance); + interface_model_helper.model_frame_name(), model_instance); const std::string canonical_link_name = GetRelativeBodyName(model_frame.body(), model_instance, *plant); const RigidTransformd X_WM = GetDefaultFramePose(*plant, model_frame); @@ -1765,15 +1780,15 @@ sdf::InterfaceModelPtr ConvertToInterfaceModel( ToIgnitionPose3d(X_PM)); AddBodiesToInterfaceModel(*plant, interface_model, - interface_model_helper.body_indices, + interface_model_helper.body_indices(), X_WM.inverse()); AddFramesToInterfaceModel(*plant, model_instance, interface_model, - interface_model_helper.frame_indices, - interface_model_helper.model_frame_name); + interface_model_helper.frame_indices(), + interface_model_helper.model_frame_name()); AddJointsToInterfaceModel(*plant, interface_model, - interface_model_helper.joint_indices); + interface_model_helper.joint_indices()); return interface_model; } @@ -1822,7 +1837,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( // New instances will have indices starting from cur_num_models const bool is_merge_include = include.IsMerge().value_or(false); - InterfaceModelHelper interface_model_helper(*plant, is_merge_include); + InterfaceModelHelper interface_model_helper(*plant); ParsingWorkspace subworkspace{options, package_map, subdiagnostic, plant, collision_resolver, parser_selector}; @@ -1842,7 +1857,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( interface_model_helper.ComputeDiffFromSnapshot(); model_frame_name = sdf::computeMergedModelProxyFrameName(model_name); - interface_model_helper.model_frame_name = model_frame_name; + interface_model_helper.set_model_frame_name(model_frame_name); main_model_instance = parent_model_instance; } else { diff --git a/tools/workspace/sdformat_internal/repository.bzl b/tools/workspace/sdformat_internal/repository.bzl index 2bfa5ee91e7e..dae58e475147 100644 --- a/tools/workspace/sdformat_internal/repository.bzl +++ b/tools/workspace/sdformat_internal/repository.bzl @@ -8,7 +8,7 @@ def sdformat_internal_repository( # This dependency is part of a "cohort" defined in # drake/tools/workspace/new_release.py. When practical, all members # of this cohort should be updated at the same time. - repository = "azeey/sdformat", + repository = "gazebosim/sdformat", commit = "sdformat14_14.0.0", build_file = ":package.BUILD.bazel", sha256 = "a4f38865f7e8b79559c238a1aba7bab1a4ff46c22a67bb91325ad48ba7d25728", # noqa