detail_sdf_parser: Example of composition interface phantom API from libsdformat

README.md

@@ -1,6 +1,8 @@
-# Drake
+# HACK Drake
 
-Model-Based Design and Verification for Robotics.
+Hacking to test out an API.
 
-Please see the [Drake Documentation](https://drake.mit.edu) for more
-information.
+~Model-Based Design and Verification for Robotics.~
+
+~Please see the [Drake Documentation](https://drake.mit.edu) for more
+information.~

WORKSPACE

@@ -1,5 +1,7 @@
 # -*- python -*-
 
+BREAK_CI_SO_WE_DONT_WASTE_RESOURCES
+
 # This file marks a workspace root for the Bazel build system. see
 # https://bazel.build/ .
 

multibody/parsing/detail_sdf_parser.cc

@@ -416,15 +416,213 @@ void AddJointFromSpecification(
   joint_types->insert(joint_spec.Type());
 }
 
+std::set<ModelInstanceIndex> GetModelInstanceSet(
+    const MultibodyPlant<double>& plant) {
+  std::set<ModelInstanceIndex> out;
+  for (int i = 0; i < plant.num_model_instances(): ++i) {
+    out.add(ModelInstanceIndex(i));
+  }
+  return out;
+}
+
+std::range GetModelLinks(
+    const MultibodyPlant<double>& plant, ModelInstanceIndex model) {
+  ...
+}
+
+std::range GetModelFrames(
+    const MultibodyPlant<double>& plant, ModelInstanceIndex model) {
+  ...
+}
+
+std::pair<std::string, std::string> SplitName(std::string full) {
+  const std::string delim = "::";  // get from libsdformat?
+  auto pos = full.rfind(delim);
+  std::string first = full.substr(0, pos);
+  std::string second = full.substr(pos + delim.size());
+  return {first, second};
+}
+
+// For the libsdformat API, see:
+// http://sdformat.org/tutorials?tut=composition_proposal&cat=pose_semantics_docs&branch=feature-composition-interface-api&repo=https%3A%2F%2Fgithub.com%2FEricCousineau-TRI%2Fsdf_tutorials-1#1-5-minimal-libsdformat-interface-types-for-non-sdformat-models
+
+constexpr char kExtUrdf[] = ".urdf";
+// To test re-parsing an SDFormat document, but in complete isolation. Tests
+// out separate model formats.
+constexpr char kExtForcedNesting[] = ".forced_nesting_sdf";
+
+// This assumes that parent models will have their parsing start before child
+// models!
+sdf::InterfaceModelPtr ParseNestedInterfaceModel(
+    MultibodyPlant<double>* plant,
+    sdf::NestedInclude include,
+    sdf::Error& errors) {
+  // Do not attempt to parse anything other than URDF or forced nesting files.
+  const bool is_urdf = EndsWith(include.resolved_file_name, kExtUrdf);
+  const bool is_forced_nesting =
+      EndsWith(include.resolved_file_name, kExtForcedNesting);
+  if (!is_urdf && !is_forced_nesting) {
+    return nullptr;
+  }
+
+  if (include.is_static) {
+    throw std::runtime_error(
+        "Drake does not yet support //include/static for custom nesting.");
+  }
+
+  // WARNING: Most of these hacks (remembering model instances, getting silly
+  // name hierarchy) come about since MultibodyPlant does not have any
+  // mechanism for easy composition by itself (e.g. copying subgraphs, or
+  // getting a subgraph, etc.).
+  // If this ever happens (see #12703), this logic will be *greatly*
+  // simplified.
+
+  std::vector<ModelInstanceIndex> new_model_instances;
+  if (is_urdf) {
+    // WARNING: URDF parsing should possibly add a `__model__` frame?
+
+    // We should be good without the URDF parser using `SetDefaultFreeBodyPose`:
+    // - In URDF, all bodies should be connected; if there's a floating body,
+    // it should only be the first one.
+    // - In MBP, if you SetDefaultFreeBodyPose on a floating body with other
+    // bodies attached to it via joints, all other bodies' positions don't
+    // matter
+
+    // N.B. Errors will just happen via thrown exceptions.
+    new_model_instances = {
+        AddModelFromUrdfFile(
+            plant, include.file_path, include.absolute_model_name)};
+    // Add explicit model frame to first link..
+    auto& canonical_link = GetModelLinks(plant, new_model_instances[0]).at(0);
+    plant.AddFrame(FixedOffsetFrame("__model__", canonical_link));
+  } else {
+    // N.B. Errors will just happen via thrown exceptions.
+    DRAKE_DEMAND(is_forced_nesting);
+    new_model_instances = AddModelsFromSdfFile(
+        plant, include.file_path, include.absolute_model_name);
+  }
+  DRAKE_DEMAND(new_model_instances.size() > 0);
+  const ModelInstanceIndex main_model_instance = new_model_instances[0];
+
+  auto& get_model_frame = [&](ModelInstanceIndex model) {
+    if (plant->HasFrameNamed("__model__", model)) {
+      return plant->GetFrameByName("__model__", model);
+    } else {
+      auto* link = GetModelLinks(plant, model)[0];
+      return link->body_frame();
+    }
+  };
+
+  // And because we don't have great composition, I need to go through and do
+  // hacks to override default initial poses.
+  // Process:
+  //  * Remember parsed pose from top-level model's pose.
+  //  * Measure delta between that and the overriding pose.
+  //  * Apply delta to each individual body's default pose :(
+  RigidTransformd X_WMdesired = ToRigidTransform(include.pose_WM);
+  const Frame<double>& main_model_frame = get_model_frame(main_model_instance);
+  RigidTransformd X_WMparsed =
+      plant->GetDefaultFreeBodyPose(main_model_frame.body()) *
+      main_model_frame.GetFixedPoseInBodyFrame();
+  RigidTransformd X_ParsedDesired = X_WMparsed.inverse() * X_WMdesired;
+
+  // This will be populated for the first model instance.
+  sdf::InterfaceModelPtr main_interface_model;
+  // Record by name to remember local hierarchy.
+  // Related this comment:
+  // https://github.com/RobotLocomotion/drake/issues/12270#issuecomment-606757766
+  std::map<std::string, sdf::InterfaceModelPtr> interface_model_hierarchy;
+
+  // Converts an MBP Body to sdf::InterfaceLink.
+  auto body_to_interface_link = [&](const Body<double>& link) {
+    // Pre-transform default pose, if specified.
+    RigidTransform X_WLparsed = plant->GetDefaultPose(*link);
+    RigidTransform X_WLdesired = X_WLparsed * X_ParsedDesired;
+    plant->SetDefaultPose(*link, X_WLdesired);
+    RigidTransformd X_ML = X_WMdesired.inverse() * X_WLdesired;
+    // Register link as grounding frame.
+    sdf::InterfacePose pose(X_ML);
+    return make_shared<sdf::InterfaceLink>(link->name(), pose);
+  };
+
+  // Converts an MBP Frame to sdf::InterfaceFrame.
+  auto frame_to_interface_frame = [&](
+      const Frame<double>& frame, std::string name = "") {
+    const std::string body_name = frame->body().name();
+    sdf::InterfacePose pose(
+        frame->GetFixedPoseInBodyFrame(), body_name);
+    if (name == "") {
+      name = frame->name();
+    }
+    if (name == "" || name == body_name) {
+      // Do not register un-named frames or body frames.
+      return nullptr;
+    }
+    return make_shared<sdf::InterfaceFrame>(name, pose);
+  };
+
+  // N.B. For hierarchy, this assumes that "parent" models are defined before
+  // their "child" models.
+  for (auto model : new_model_instances) {
+    const std::string absolute_name = plant->GetModelInstanceName(model);
+    const auto [absolute_parent_name, local_name] = SplitName(absolute_name);
+
+    auto& model_frame = get_model_frame(model);
+    auto& canonical_link = model_frame.body();
+
+    auto interface_model = std::make_shared<sdf::InterfaceModel>(
+        local_name,
+        body_to_interface_link(canonical_link),
+        // TODO(eric.cousineau): What if the model already has a __model__
+        // frame?
+        frame_to_interface_frame(model_frame, "__model__"));
+
+    // Record all frames.
+    for (auto* link : GetModelLinks(plant, model)) {
+      if (link == &canonical_link) {
+        // Already added.
+        continue;
+      }
+      interface_model->AddLink(body_to_interface_link(*body));
+    }
+    for (auto* frame : GetModelFrames(plant, model)) {
+      if (frame == model_frame) {
+        // Already added.
+        continue;
+      }
+      interface_model->AddFrame(frame_to_interface_frame(*frame));
+    }
+
+    if (!main_interface_model) {
+      main_interface_model = interface_model;
+      interface_model_hierarchy[absolute_name] = main_interface_model;
+    } else {
+      // Register with its parent model.
+      sdf::InterfaceModelPtr parent_interface_model =
+          interface_model_hierarchy.at(absolute_parent_name);
+      parent_interface_model->AddNestedModel(interface_model);
+    }
+  }
+
+  return main_interface_model;
+}
+
 // Helper method to load an SDF file and read the contents into an sdf::Root
 // object.
 std::string LoadSdf(
     sdf::Root* root,
-    const std::string& file_name) {
+    const std::string& file_name,
+    MultibodyPlant<double>* plant) {
 
   const std::string full_path = GetFullPath(file_name);
-
   // Load the SDF file.
+  // WARNING: This will "invalidate" a plant if there is attempted recovery
+  // after an exception is thrown. However, this is probably the case for
+  // normal URDF and SDFormat parsing.
+  root->registerCustomModelParser(
+    [plant](sdf::NestedInclude include, sdf::Errors& errors) {
+      return ParseNestedInterfaceModel(plant, include, errors);
+    });
   ThrowAnyErrors(root->Load(full_path));
 
   // Uses the directory holding the SDF to be the root directory
@@ -486,6 +684,8 @@ std::vector<LinkInfo> AddLinksFromSpecification(
     plant->SetDefaultFreeBodyPose(body, X_WL);
 
     if (plant->geometry_source_is_registered()) {
+      // TODO(eric.cousineau): Make this the primary thing to pass around. Do
+      // not pass around (package_map, root_dir).
       ResolveFilename resolve_filename =
         [&package_map, &root_dir](std::string uri) {
           const std::string resolved_name =
@@ -632,6 +832,28 @@ bool AreWelded(
   return false;
 }
 
+using VoidFunction = std::function<void()>;
+
+VoidFunction AddNestedModelsFromSpecification(
+    auto model, model_name, auto plant, X_WM, package_map, root_dir) {
+  std::vector<VoidFunction> post_inits;
+  Parser parser(plant);
+  for (sdf::Model nested_model : model.GetParsedNestedModels()) {
+    // libsdformat parsed the model fully. Add the new elements directly.
+    // All other models have already been added.
+    auto model_instance = AddModelFromSpecification(
+        nested_model.AsSdfModel(), ...);
+    post_inits.append([model_instance, plant]() {
+      // Weld frames or posture stuff???
+    });
+  }
+  return [std::move(post_inits)]() {
+    for (auto f : post_inits) {
+      f();
+    }
+  };
+}
+
 // Helper method to add a model to a MultibodyPlant given an sdf::Model
 // specification object.
 ModelInstanceIndex AddModelFromSpecification(
@@ -649,6 +871,12 @@ ModelInstanceIndex AddModelFromSpecification(
   ThrowIfPoseFrameSpecified(model.Element());
   const RigidTransformd X_WM = ToRigidTransform(model.RawPose());
 
+  // First, add nested models, since they are encapsulated, so that the MBP
+  // frames can be referenced explicitly.
+  std::function<()> nested_models_post_init =
+      AddNestedModelsFromSpecification(
+          model, model_name, plant, X_WM, package_map, root_dir);
+
   drake::log()->trace("sdf_parser: Add links");
   std::vector<LinkInfo> added_link_infos = AddLinksFromSpecification(
       model_instance, model, X_WM, plant, package_map, root_dir);
@@ -704,7 +932,13 @@ ModelInstanceIndex AddModelFromSpecification(
     }
   }
 
+  // Finalize connections for nested model posturing, etc.
+  nested_models_post_init();
+
   if (model.Static()) {
+    // NOTE: This should work for nested models, as SDFormat should ensure that
+    // any nested model's are also defined as static.
+
     // Only weld / fixed joints are permissible.
     // TODO(eric.cousineau): Consider "freezing" non-weld joints, as is
     // permissible in Bullet and DART via Gazebo (#12227).
@@ -742,7 +976,11 @@ ModelInstanceIndex AddModelFromSdfFile(
 
   sdf::Root root;
 
-  std::string root_dir = LoadSdf(&root, file_name);
+  if (scene_graph != nullptr && !plant->geometry_source_is_registered()) {
+    plant->RegisterAsSourceForSceneGraph(scene_graph);
+  }
+
+  std::string root_dir = LoadSdf(&root, file_name, plant);
 
   if (root.ModelCount() != 1) {
     throw std::runtime_error("File must have a single <model> element.");
@@ -751,10 +989,6 @@ ModelInstanceIndex AddModelFromSdfFile(
   // Get the only model in the file.
   const sdf::Model& model = *root.ModelByIndex(0);
 
-  if (scene_graph != nullptr && !plant->geometry_source_is_registered()) {
-    plant->RegisterAsSourceForSceneGraph(scene_graph);
-  }
-
   const std::string model_name =
       model_name_in.empty() ? model.Name() : model_name_in;
 
@@ -772,7 +1006,7 @@ std::vector<ModelInstanceIndex> AddModelsFromSdfFile(
 
   sdf::Root root;
 
-  std::string root_dir = LoadSdf(&root, file_name);
+  std::string root_dir = LoadSdf(&root, file_name, plant);
 
   // Throw an error if there are no models or worlds.
   if (root.ModelCount() == 0 && root.WorldCount() == 0) {