[parsing] Add support for using merge-includes with custom parsed mod…

…els (#16727)

SDFormat models can now merge-include URDF and MJCF models using `<include merge="true"/>`

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",

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<ModelInstanceIndex> Parse(const std::string& model_name_in,
- const std::optional<std::string>& parent_model_name,
- XMLDocument* xml_doc,
- const std::filesystem::path& main_mjcf_path) {
+ std::pair<std::optional<ModelInstanceIndex>, std::string> Parse(
+ const std::string& model_name_in,
+ const std::optional<std::string>& parent_model_name,
+ std::optional<ModelInstanceIndex> 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<std::string, SpatialInertia<double>> mesh_inertia_;
 };
 
-} // namespace
-
-std::optional<ModelInstanceIndex> AddModelFromMujocoXml(
+std::pair<std::optional<ModelInstanceIndex>, std::string>
+AddOrMergeModelFromMujocoXml(
  const DataSource& data_source, const std::string& model_name_in,
  const std::optional<std::string>& parent_model_name,
- const ParsingWorkspace& workspace) {
+ const ParsingWorkspace& workspace,
+ std::optional<ModelInstanceIndex> merge_into_model_instance) {
  DRAKE_THROW_UNLESS(!workspace.plant->is_finalized());
 
  TinyXml2Diagnostic diag(&workspace.diagnostic, &data_source);
@@ -1434,7 +1440,18 @@ std::optional<ModelInstanceIndex> 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<ModelInstanceIndex> AddModelFromMujocoXml(
+ const DataSource& data_source, const std::string& model_name_in,
+ const std::optional<std::string>& 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<ModelInstanceIndex> 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<ModelInstanceIndex> MujocoParserWrapper::AddAllModels(
  const DataSource& data_source,
  const std::optional<std::string>& parent_model_name,

multibody/parsing/detail_mujoco_parser.h

@@ -58,6 +58,11 @@ class MujocoParserWrapper final : public ParserInterface {
  const std::optional<std::string>& 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<ModelInstanceIndex> AddAllModels(
  const DataSource& data_source,
  const std::optional<std::string>& parent_model_name,

multibody/parsing/detail_parsing_workspace.h

@@ -51,6 +51,31 @@ class ParserInterface {
  const std::optional<std::string>& parent_model_name,
  const ParsingWorkspace& workspace) = 0;
 
+ // 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.
+ //
+ // @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 merge_into_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 /* merge_into_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.
  //