Model Directives mechanism for scene assembly.

* Resolves #13282 * A domain-specific language for assembling MultibodyPlant scenes from multiple SDF files. * Helpful for assembling large scenes without huge unwieldy sdf/xacro files. * A temporary accomodation until sdformat adds similar functionality. * Ported from TRI Project Anzu Authors: Eric Cousineau <[email protected]> Jeremy Nimmer <[email protected]> Grant Gould <[email protected]> Calder Phillips-Grafflin <[email protected]> Siyuan Feng <[email protected]>
RobotLocomotion · Aug 18, 2020 · c077ee9 · c077ee9
1 parent 4d1b6dc
commit c077ee9
Show file tree

Hide file tree

Showing 15 changed files with 260 additions and 65 deletions.
diff --git a/common/schema/dev/BUILD.bazel b/common/schema/dev/BUILD.bazel
@@ -39,6 +39,7 @@ drake_cc_library(
     name = "transform",
     srcs = ["transform.cc"],
     hdrs = ["transform.h"],
+    visibility = ["//multibody/parsing/dev:__pkg__"],
     deps = [
         ":rotation",
         ":stochastic",

diff --git a/common/schema/dev/rotation.h b/common/schema/dev/rotation.h
@@ -34,7 +34,7 @@ struct Rotation {
     Identity() = default;
 
     template <typename Archive>
-    void Serialize(Archive* a) {}
+    void Serialize(Archive*) {}
   };
 
   /// A roll-pitch-yaw rotation, using the angle conventions of Drake's
@@ -72,7 +72,7 @@ struct Rotation {
     Uniform() = default;
 
     template <typename Archive>
-    void Serialize(Archive* a) {}
+    void Serialize(Archive*) {}
   };
 
   /// Returns true iff this is fully deterministic.

diff --git a/multibody/parsing/dev/BUILD.bazel b/multibody/parsing/dev/BUILD.bazel
@@ -0,0 +1,64 @@
+# -*- python -*-
+
+load(
+    "@drake//tools/skylark:drake_cc.bzl",
+    "drake_cc_googletest",
+    "drake_cc_library",
+    "drake_cc_package_library",
+)
+load("//tools/lint:lint.bzl", "add_lint_tests")
+load("//tools/install:install_data.bzl", "install_data")
+
+drake_cc_library(
+    name = "process_model_directives",
+    srcs = ["process_model_directives.cc"],
+    hdrs = ["process_model_directives.h"],
+    data = [
+        "@drake//manipulation/models/jaco_description:models",
+    ],
+    deps = [
+        ":model_directives",
+        "//common:filesystem",
+        "//common:find_resource",
+        "//common/yaml:yaml_read_archive",
+        "//multibody/parsing",
+        "//multibody/plant",
+    ],
+)
+
+drake_cc_googletest(
+    name = "process_model_directives_test",
+    data = [
+        ":test_models",
+    ],
+    deps = [
+        ":process_model_directives",
+    ],
+)
+
+drake_cc_library(
+    name = "model_directives",
+    hdrs = [
+        "model_directives.h",
+    ],
+    deps = [
+        "//common:essential",
+        "//common:name_value",
+        "//common/schema/dev:transform",
+        "//math",
+    ],
+)
+
+drake_cc_googletest(
+    name = "model_directives_test",
+    deps = [
+        ":model_directives",
+        "//common/yaml:yaml_read_archive",
+    ],
+)
+
+install_data(
+    extra_tags = ["nolint"],  # `dev` content is never actually installed.
+)
+
+add_lint_tests()
diff --git a/multibody/parsing/dev/README.md b/multibody/parsing/dev/README.md
@@ -0,0 +1,88 @@
+The Model Directives mechanism
+==============================
+
+Model Directives is a small yaml-based language for building a complex
+MultibodyPlant-based scene out of numerous SDFs.  For instance in the TRI
+dish-loading demo we have individual SDF files for the counter, sink, cameras,
+pedestal, arm, gripper, and each manipuland.  A single SDF for this would be
+unwieldy and difficult to maintain and collaborate on, but SDF's file
+inclusion mechanisms have not yet proven adequate to this task.
+
+We expect that this mechanism will be temporary and will be removed when
+sdformat adds similar functionality.  Users should be aware that this library
+will be deprecated if/when sdformat reaches feature parity with it.
+
+
+## Syntax
+
+The easiest syntax reference is the unit test files in `test/models/*.yaml` of
+this directory.
+
+A model directives file is a yaml file with a top level `directives:` group.
+Within this group are a series of directives:
+
+ * `AddModel` takes a `file` and `name` and loads the SDF/URDF file indicated
+   as a new model instance with the given name.
+ * `AddModelInstance` Creates a new, empty model instance in the plant with
+   the indicated `name`.
+ * `AddPackagePath` takes `name` and `path` and makes `package://name` URIs
+   be resolved to `path`.
+ * `AddFrame` takes a `name` and a `X_PF` transform and adds a new frame to
+   the model.  Note that the transform is as specified in the `Transform`
+   scenario schema and can reference an optional base frame, in which case
+   the frame will be added to the base frame's model instance.
+ * `AddDirectives` takes a `file` naming a model directives file and an
+   optional `model_namespace`; it loads the model directives from that file
+   with the namespace prefixed to them (see Scoping, below).
+ * `AddWeld` takes a `parent` and `child` frame and welds them together.
+
+
+## Use
+
+The easiest use reference is the unit test `process_model_directives_test.cc`.
+
+A simple example of a use would be:
+
+```cpp
+  ModelDirectives station_directives = LoadModelDirectives(
+      FindResourceOrThrow("my_exciting_project/models/my_scene.yaml"));
+  MultibodyPlant<double> plant;
+  ProcessModelDirectives(station_directives, &plant);
+  plant.Finalize();
+```
+
+This loads the model directives from that filename, constructs a plant, and
+uses the model directives to populate the plant.
+
+
+## Scoping
+
+Elements (frames, bodies, etc.) in `MultibodyPlant` belong to model instances.
+Model instances can have any name specifiers, and can contain the "namespace"
+delimiter `::`. Element names should not contain `::`.
+
+Examples:
+
+- `my_frame` implies no explicit model instance.
+- `my_model::my_frame` implies the model instance `my_model`, the frame
+`my_frame`.
+- `top_level::my_model::my_frame` implies the model instance
+`top_level::my_model`, the frame `my_frame`.
+
+
+## Conditions for deprecation
+
+We expect and hope to deprecate this mechanism when either:
+
+SDF format properly specifies, and Drake supports, the following:
+
+ * What `<include/>` statements should *really* do (e.g. namespacing models,
+   joints, etc.) without kludging Drake's parsing
+ * How to weld models together with joints external to the models
+
+OR if we find a mechanism whereby xacro could accomplish the same thing:
+
+ * Drake's `package://` / `model://` mechanism were mature and correct, and
+   `sdformat` didn't use singletons for search paths.
+ * It was easier for one xacro to locate other xacros (possibly via a
+   workaround using a wrapper script to inject `DRAKE_PATH`)
diff --git a/multibody/parsing/dev/model_directives.h b/multibody/parsing/dev/model_directives.h
@@ -11,13 +11,15 @@
 
 #include "drake/common/eigen_types.h"
 #include "drake/common/name_value.h"
+#include "drake/common/schema/dev/transform.h"
 #include "drake/common/text_logging.h"
 #include "drake/math/rigid_transform.h"
 #include "drake/math/roll_pitch_yaw.h"
-#include "common/schema/transform.h"
 
-namespace anzu {
-namespace common {
+namespace drake {
+namespace multibody {
+namespace parsing {
+namespace dev {
 namespace schema {
 
 struct AddWeld {
@@ -128,7 +130,7 @@ struct AddFrame {
   std::string name;
   // Pose of frame to be added, `F`, w.r.t. parent frame `P` (as defined by
   // `X_PF.base_frame`).
-  Transform X_PF;
+  drake::schema::Transform X_PF;
 };
 
 struct AddDirectives {
@@ -225,15 +227,17 @@ struct ModelDirectives {
 inline void AddPackageToModelDirectives(const std::string& package_name,
                                         const std::string& package_path,
                                         ModelDirectives* directives) {
-  common::schema::AddPackagePath add_package_path;
+  AddPackagePath add_package_path;
   add_package_path.name = package_name;
   add_package_path.path = package_path;
-  common::schema::ModelDirective directive;
+  ModelDirective directive;
   directive.add_package_path = add_package_path;
   directives->directives.insert(
       directives->directives.begin(), directive);
 }
 
 }  // namespace schema
-}  // namespace common
-}  // namespace anzu
+}  // namespace dev
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake
diff --git a/multibody/parsing/dev/process_model_directives.cc b/multibody/parsing/dev/process_model_directives.cc
@@ -1,24 +1,25 @@
-#include "common/process_model_directives.h"
+#include "drake/multibody/parsing/dev/process_model_directives.h"
 
 #include <memory>
 #include <optional>
+#include <string>
 #include <utility>
 
+#include "drake/common/filesystem.h"
 #include "drake/common/find_resource.h"
+#include "drake/common/schema/dev/transform.h"
 #include "drake/common/yaml/yaml_read_archive.h"
 #include "drake/multibody/parsing/parser.h"
-#include "common/filesystem.h"
-#include "common/find_resource.h"
-#include "common/starts_with.h"
-#include "common/yaml_load.h"
 
-namespace anzu {
-namespace common {
+namespace drake {
+namespace multibody {
+namespace parsing {
+namespace dev {
 
 using std::make_unique;
 using Eigen::Isometry3d;
 
-namespace fs = anzu::filesystem;
+namespace fs = drake::filesystem;
 using drake::FindResourceOrThrow;
 using drake::math::RigidTransformd;
 using drake::multibody::FixedOffsetFrame;
@@ -319,7 +320,15 @@ void ProcessModelDirectives(
 
 ModelDirectives LoadModelDirectives(const std::string& filename) {
   drake::log()->debug("LoadModelDirectives: {}", filename);
-  auto directives = common::YamlLoadWithDefaults<ModelDirectives>(filename);
+
+  // TODO(ggould-tri) This should use the YamlLoadWithDefaults mechanism
+  // instead once that is ported to drake.
+  ModelDirectives directives;
+  YAML::Node root = YAML::LoadFile(filename);
+  drake::yaml::YamlReadArchive::Options options;
+  options.allow_cpp_with_no_yaml = true;
+  drake::yaml::YamlReadArchive(root, options).Accept(&directives);
+
   DRAKE_DEMAND(directives.IsValid());
   return directives;
 }
@@ -376,7 +385,8 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
       frame_dir.X_PF.base_frame = model_to_add.parent_frame_name;
       frame_dir.X_PF.translation =
           drake::Vector<double, 3>(model_to_add.X_PC.translation());
-      frame_dir.X_PF.rotation = schema::Rotation{model_to_add.X_PC.rotation()};
+      frame_dir.X_PF.rotation =
+          drake::schema::Rotation{model_to_add.X_PC.rotation()};
 
       directives.directives.at(index++).add_frame = frame_dir;
     }
@@ -388,7 +398,7 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
 
     directives.directives.at(index++).add_model = model_dir;
 
-    common::schema::AddWeld weld_dir;
+    schema::AddWeld weld_dir;
     weld_dir.parent = attachment_frame_name;
     weld_dir.child =
         model_to_add.model_name + "::" + model_to_add.child_frame_name;
@@ -399,5 +409,8 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
   return directives;
 }
 
-}  // namespace common
-}  // namespace anzu
+
+}  // namespace dev
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake
diff --git a/multibody/parsing/dev/process_model_directives.h b/multibody/parsing/dev/process_model_directives.h
@@ -7,14 +7,16 @@
 #include <string>
 #include <vector>
 
+#include "drake/multibody/parsing/dev/model_directives.h"
 #include "drake/multibody/parsing/package_map.h"
 #include "drake/multibody/parsing/parser.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/multibody/tree/multibody_tree_indexes.h"
-#include "common/schema/model_directives.h"
 
-namespace anzu {
-namespace common {
+namespace drake {
+namespace multibody {
+namespace parsing {
+namespace dev {
 
 schema::ModelDirectives LoadModelDirectives(const std::string& filename);
 
@@ -127,5 +129,7 @@ ScopedName ParseScopedName(const std::string& full_name);
 
 }  // namespace internal
 
-}  // namespace common
-}  // namespace anzu
+}  // namespace dev
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake
diff --git a/multibody/parsing/dev/test/model_directives_test.cc b/multibody/parsing/dev/test/model_directives_test.cc
@@ -1,14 +1,16 @@
 // Minimal test to make sure stuff doesn't explode.
-#include "common/schema/model_directives.h"
+#include "drake/multibody/parsing/dev/model_directives.h"
 
 #include <gtest/gtest.h>
 
 #include "drake/common/yaml/yaml_read_archive.h"
 
 using drake::yaml::YamlReadArchive;
 
-namespace anzu {
-namespace common {
+namespace drake {
+namespace multibody {
+namespace parsing {
+namespace dev {
 namespace schema {
 namespace {
 
@@ -50,5 +52,7 @@ GTEST_TEST(ModelDirectivesTest, Success) {
 
 }  // namespace
 }  // namespace schema
-}  // namespace common
-}  // namespace anzu
+}  // namespace dev
+}  // namespace parsing
+}  // namespace multibody
+}  // namespace drake
diff --git a/multibody/parsing/dev/test/models/add_scoped_mid.yaml b/multibody/parsing/dev/test/models/add_scoped_mid.yaml
@@ -12,11 +12,11 @@ directives:
       base_frame: world
       translation: [10, 0, 0]
 - add_directives:
-    file: package://anzu/common/test/models/add_scoped_sub.yaml
+    file: package://process_model_directives_test/add_scoped_sub.yaml
     model_namespace: nested
 
 # Include a test for model directives backreferences (which are possibly a bug;
 # see the included file for details)
 - add_directives:
-    file: package://anzu/common/test/models/add_backreference.yaml
+    file: package://process_model_directives_test/add_backreference.yaml
     model_namespace: nested