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,202 @@ 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};
+}
+
+RigidTransformd GetDefaultFramePose(
+    const MultibodyPlant<double>& plant,
+    const Frame<double>& frame) {
+  const RigidTransformd X_WB =
+      plant->GetDefaultFreeBodyPose(frame.body());
+  const RigidTransformd X_WF =
+      X_WB * frame.GetFixedPoseInBodyFrame();
+  return X_WF;
+}
+
+// For the libsdformat API, see:
+// http://sdformat.org/tutorials?tut=composition_proposal&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) {
+    // TOOD(eric.cousineau): Handle URDF and SetDefaultFreeBodyPose() (#13692)
+    // 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];
+
+  // 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 =
+      plant->GetFrameByName("__model__", main_model_instance);
+
+  // 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) {
+    const RigidTransformd X_WM = GetDefaultFramePose(*plant, main_model_frame);
+    RigidTransformd X_ML =
+        X_WM.inverse() * plant->GetDefaultFreeBodyPose(link);
+    return make_shared<sdf::InterfaceLink>(link->name(), ToPose3d(X_ML));
+  };
+
+  // Converts an MBP Frame to sdf::InterfaceFrame.
+  auto frame_to_interface_frame = [&](const Frame<double>& frame) {
+    const std::string body_name = frame->body().name();
+    const std::string attached_to = body_name;
+    sdf::InterfacePose pose(frame->GetFixedPoseInBodyFrame(), body_name);
+    if (name == "" || name == body_name) {
+      // Do not register un-named frames or body frames.
+      return nullptr;
+    }
+    return make_shared<sdf::InterfaceFrame>(name, body_name, ToPose3d(X_BF));
+  };
+
+  // N.B. For hierarchy, this assumes that "parent" models are defined before
+  // their "child" models.
+  for (auto model_instance : new_model_instances) {
+    sdf::RepostureFunction reposture_model = [plant, model_instance](
+        sdf::InterfaceModelPtr interface_model,
+        sdf::NestedModelFrameGraphPtr graph) {
+      // N.B. This should also posture the model appropriately.
+      for (auto interface_link : interface_model.GetLinks()) {
+        const auto& body =
+            plant->GetBodyByName(interface_link.GetName(), model_instance);
+        RigidTransformd X_WL =
+            graph.ResolveNestedFramePose(interface_link.GetName());
+        plant->SetFreeBodyPose();
+      }
+    };
+
+    const std::string absolute_name =
+        plant->GetModelInstanceName(model_instance);
+    const auto [absolute_parent_name, local_name] = SplitName(absolute_name);
+
+    sdf::InterfaceModelPtr interface_model =
+        std::make_shared<sdf::InterfaceModel>(
+            local_name,
+            reposture_model,
+            model_frame.body().name(),
+            model_frame.GetFixedPoseInBodyFrame());
+
+    // Record all frames.
+    for (auto* link : GetModelLinks(plant, model_instance)) {
+      interface_model->AddLink(body_to_interface_link(*body));
+    }
+    for (auto* frame : GetModelFrames(plant, model_instance)) {
+      if (frame.name() == "__model__") {
+        // 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 +673,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 +821,15 @@ bool AreWelded(
   return false;
 }
 
+void AddNestedModelsFromSpecification(
+    auto model, model_name, auto plant, X_WM, package_map, root_dir) {
+  Parser parser(plant);
+  for (sdf::Model nested_model : model.GetParsedNestedModels()) {
+    // libsdformat parsed the model fully. Add the new elements directly.
+    AddModelFromSpecification(nested_model.AsSdfModel(), ...);
+  }
+}
+
 // Helper method to add a model to a MultibodyPlant given an sdf::Model
 // specification object.
 ModelInstanceIndex AddModelFromSpecification(
@@ -649,6 +847,11 @@ 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.
+  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);
@@ -705,6 +908,9 @@ ModelInstanceIndex AddModelFromSpecification(
   }
 
   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 +948,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 +961,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 +978,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) {

multibody/parsing/test/nested_models/psuedo_file.sdf

@@ -0,0 +1,73 @@
+<!-- top.sdf -->
+<sdf>
+  <model name="top">
+    <link name="torso"/>
+
+    <include name="random_crap">
+      <pose relative_to="right_arm::gripper::grasp_frame"/>
+      <uri>...</uri>
+    </include>
+
+    <include name="arm">
+      <pose relative_to="torso">X_TArm</pose>
+      <uri>file://.../arm.forced_nesting_sdf</uri>
+    </include>
+
+    <joint name="arm_attach">
+      <parent>torso</parent>
+      <child>arm::mount</child>
+    </joint>
+
+    <include name="extra_arm">
+      <pose relative_to="arm">X_ArmExtra</pose>
+      <uri>file://.../extra_arm.urdf</uri>
+    </include>
+
+    <joint name="extra_arm_attach">
+      <parent>arm::L1</parent>
+      <child>extra_arm::L2</child>
+    </joint>
+
+  </model>
+</sdf>
+
+<!-- left.urdf -->
+<robot name="arm">
+  <link name="L1"/>
+  <joint name="J1" type="revolute">
+    <origin xyz="{xyz_L1J1}" rpy="{rpy_L1J1}"/>
+    <parent link="L1"/>
+    <child link="L2"/>
+  </joint>
+  <link name="L2"/>
+</robot>
+
+<!-- right.forced_nesting_sdf -->
+<sdf>
+  <model name="arm">
+    <link name="L1"/>
+    <joint name="J1" type="revolute">
+      <pose relative_to="L1">{X_L1J1}</pose>
+      <parent>L1</parent>
+      <child>L2</child>
+    </joint>
+    <link name="L2"/>
+    <frame name="gripper_mount" attached_to="L2">
+      <pose>{X_L2Gm}</pose>
+    </frame>
+    <include name="gripper">
+      <pose relative_to="gripper_mount"/>
+      <uri>file://.../gripper.sdf</uri>
+    </include>
+  </model>
+</sdf>
+
+<!-- gripper.sdf -->
+<sdf>
+  <model name="gripper">
+    <link name="body"/>
+    <frame name="grasp_frame">
+      <pose>{X_GGrasp}</pose>
+    </frame>
+  </model>
+</sdf>