From 84f70bf7b308e2928e8fff0f70319b234b0aec8f Mon Sep 17 00:00:00 2001 From: ggould-tri Date: Mon, 17 Aug 2020 16:22:11 -0400 Subject: [PATCH] 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 Jeremy Nimmer Grant Gould Calder Phillips-Grafflin Siyuan Feng --- common/schema/dev/BUILD.bazel | 1 + common/schema/dev/rotation.h | 4 +- multibody/parsing/dev/BUILD.bazel | 64 ++++++++++++++ multibody/parsing/dev/README.md | 88 +++++++++++++++++++ multibody/parsing/dev/model_directives.h | 20 +++-- .../parsing/dev/process_model_directives.cc | 39 +++++--- .../parsing/dev/process_model_directives.h | 14 +-- .../parsing/dev/test/model_directives_test.cc | 14 +-- .../dev/test/models/add_scoped_mid.yaml | 4 +- .../dev/test/models/add_scoped_sub.yaml | 4 +- .../dev/test/models/add_scoped_top.yaml | 8 +- multibody/parsing/dev/test/models/package.xml | 11 +++ .../parsing/dev/test/models/simple_model.sdf | 2 +- .../dev/test/process_model_directives_test.cc | 42 +++++---- tools/install/install_data.bzl | 8 +- 15 files changed, 259 insertions(+), 64 deletions(-) create mode 100644 multibody/parsing/dev/BUILD.bazel create mode 100644 multibody/parsing/dev/README.md create mode 100644 multibody/parsing/dev/test/models/package.xml diff --git a/common/schema/dev/BUILD.bazel b/common/schema/dev/BUILD.bazel index 4590dcef5690..6fb0478424a9 100644 --- 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 index e7094e87cfae..d476f16b4f04 100644 --- a/common/schema/dev/rotation.h +++ b/common/schema/dev/rotation.h @@ -34,7 +34,7 @@ struct Rotation { Identity() = default; template - 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 - 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 new file mode 100644 index 000000000000..8529874f40d2 --- /dev/null +++ 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 new file mode 100644 index 000000000000..49203d470aff --- /dev/null +++ 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 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 `` 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 index 084032e27316..a8c0308a61ff 100644 --- 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 index 86132ba6ec44..7e11a12980c5 100644 --- 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 #include +#include #include +#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(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(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 index 2b74e8fb6c3a..3b6eeaca3e4f 100644 --- a/multibody/parsing/dev/process_model_directives.h +++ b/multibody/parsing/dev/process_model_directives.h @@ -7,14 +7,16 @@ #include #include +#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 index da5e045ea447..402427e2fcad 100644 --- a/multibody/parsing/dev/test/model_directives_test.cc +++ b/multibody/parsing/dev/test/model_directives_test.cc @@ -1,5 +1,5 @@ // Minimal test to make sure stuff doesn't explode. -#include "common/schema/model_directives.h" +#include "drake/multibody/parsing/dev/model_directives.h" #include @@ -7,8 +7,10 @@ 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 index e66c48cc50dd..e75456c5f702 100644 --- 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 diff --git a/multibody/parsing/dev/test/models/add_scoped_sub.yaml b/multibody/parsing/dev/test/models/add_scoped_sub.yaml index f1246e1e2a1f..d62a394045fb 100644 --- a/multibody/parsing/dev/test/models/add_scoped_sub.yaml +++ b/multibody/parsing/dev/test/models/add_scoped_sub.yaml @@ -7,7 +7,7 @@ directives: - add_model: # This name will be prefixed by `model_namespace`. name: simple_model - file: package://anzu/common/test/models/simple_model.sdf + file: package://process_model_directives_test/simple_model.sdf - add_frame: # This will implicitly resolve to the `simple_model` instance. @@ -25,7 +25,7 @@ directives: - add_model: name: extra_model - file: package://anzu/common/test/models/simple_model.sdf + file: package://process_model_directives_test/simple_model.sdf - add_weld: parent: simple_model::frame diff --git a/multibody/parsing/dev/test/models/add_scoped_top.yaml b/multibody/parsing/dev/test/models/add_scoped_top.yaml index b8b9d314212a..e81f60b19415 100644 --- a/multibody/parsing/dev/test/models/add_scoped_top.yaml +++ b/multibody/parsing/dev/test/models/add_scoped_top.yaml @@ -14,7 +14,7 @@ directives: X_PF: base_frame: world - add_directives: - file: package://anzu/common/test/models/add_scoped_sub.yaml + file: package://process_model_directives_test/add_scoped_sub.yaml - add_frame: name: simple_model::top_added_frame X_PF: @@ -32,7 +32,7 @@ directives: base_frame: world translation: [0, 1, 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: left - add_frame: name: left::simple_model::top_added_frame @@ -49,7 +49,7 @@ directives: base_frame: world translation: [0, -1, 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: right - add_frame: name: right::simple_model::top_added_frame @@ -61,7 +61,7 @@ directives: - add_model_instance: name: mid - add_directives: - file: package://anzu/common/test/models/add_scoped_mid.yaml + file: package://process_model_directives_test/add_scoped_mid.yaml model_namespace: mid - add_frame: name: mid::nested::simple_model::top_added_frame diff --git a/multibody/parsing/dev/test/models/package.xml b/multibody/parsing/dev/test/models/package.xml new file mode 100644 index 000000000000..61c989b9ab4f --- /dev/null +++ b/multibody/parsing/dev/test/models/package.xml @@ -0,0 +1,11 @@ + + + process_model_directives_test + 0.0.0 + + Unit test data for `process_model_directives.cc` + + John Doe + Jane Doe + N/A + diff --git a/multibody/parsing/dev/test/models/simple_model.sdf b/multibody/parsing/dev/test/models/simple_model.sdf index b8f60a69455e..b38bc16b63ea 100644 --- a/multibody/parsing/dev/test/models/simple_model.sdf +++ b/multibody/parsing/dev/test/models/simple_model.sdf @@ -16,7 +16,7 @@ Test usage of diffuse map from a non-neighboring Anzu resource. --> - package://anzu/common/test/models_data/circle.png + package://process_model_directives_test/circle.png diff --git a/multibody/parsing/dev/test/process_model_directives_test.cc b/multibody/parsing/dev/test/process_model_directives_test.cc index a6a2ec699ec0..2a2e4885218f 100644 --- a/multibody/parsing/dev/test/process_model_directives_test.cc +++ b/multibody/parsing/dev/test/process_model_directives_test.cc @@ -1,16 +1,18 @@ -#include "common/process_model_directives.h" +#include "drake/multibody/parsing/dev/process_model_directives.h" #include #include +#include "drake/common/filesystem.h" +#include "drake/common/find_resource.h" #include "drake/math/rigid_transform.h" #include "drake/multibody/plant/multibody_plant.h" -#include "common/filesystem.h" -#include "common/find_resource.h" -namespace anzu { -namespace common { +namespace drake { +namespace multibody { +namespace parsing { +namespace dev { namespace { using std::optional; @@ -22,22 +24,21 @@ using drake::multibody::Parser; using drake::systems::DiagramBuilder; using schema::ModelDirectives; -// Because this test is meant to test ProcessModelDirectives without using -// anzu features, but because the test data is in anzu (for now), provide -// a bespoke way to get a multibody::Parser that knows about anzu. -// TODO(ggould) Remove this method when the test data moves to drake. +// Our unit test's package is not normally loaded; construct a parser that +// has it and can resolve package://process_model_directives_test urls. std::unique_ptr make_parser(MultibodyPlant* plant) { auto parser = std::make_unique(plant); - const anzu::filesystem::path abspath_xml = - FindAnzuResourceOrThrow("package.xml"); - parser->package_map().Add("anzu", abspath_xml.parent_path().string()); + const drake::filesystem::path abspath_xml = FindResourceOrThrow( + "drake/multibody/parsing/dev/test/models/package.xml"); + parser->package_map().AddPackageXml(abspath_xml.string()); return std::move(parser); } // Simple smoke test of the most basic model directives. GTEST_TEST(ProcessModelDirectivesTest, BasicSmokeTest) { ModelDirectives station_directives = LoadModelDirectives( - FindAnzuResourceOrThrow("common/test/models/add_scoped_sub.yaml")); + FindResourceOrThrow( + "drake/multibody/parsing/dev/test/models/add_scoped_sub.yaml")); const MultibodyPlant empty_plant(0.0); MultibodyPlant plant(0.0); @@ -61,7 +62,8 @@ GTEST_TEST(ProcessModelDirectivesTest, BasicSmokeTest) { // testing its interaction with SceneGraph. GTEST_TEST(ProcessModelDirectivesTest, AddScopedSmokeTest) { ModelDirectives directives = LoadModelDirectives( - "common/test/models/add_scoped_top.yaml"); + FindResourceOrThrow( + "drake/multibody/parsing/dev/test/models/add_scoped_top.yaml")); // Ensure that we have a SceneGraph present so that we test relevant visual // pieces. @@ -103,7 +105,8 @@ GTEST_TEST(ProcessModelDirectivesTest, AddScopedSmokeTest) { GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) { const RigidTransformd error_transform({0.1, 0., 0.1}, {2, 3, 4}); ModelDirectives directives = LoadModelDirectives( - FindAnzuResourceOrThrow("common/test/models/add_scoped_sub.yaml")); + FindResourceOrThrow( + "drake/multibody/parsing/dev/test/models/add_scoped_sub.yaml")); // This error function should add model error to exactly one weld, the // attachment of the `first_instance` sdf model to the `smoke_test_origin` @@ -122,8 +125,7 @@ GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) { }; ProcessModelDirectives(directives, &plant, - nullptr, make_parser(&plant).get(), - error); + nullptr, make_parser(&plant).get(), error); plant.Finalize(); // This should have created an error frame for the relevant weld. @@ -153,5 +155,7 @@ GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) { } } // namespace -} // namespace common -} // namespace anzu +} // namespace dev +} // namespace parsing +} // namespace multibody +} // namespace drake diff --git a/tools/install/install_data.bzl b/tools/install/install_data.bzl index 7cc2f0f68e82..8281b79fcfb2 100644 --- a/tools/install/install_data.bzl +++ b/tools/install/install_data.bzl @@ -6,7 +6,8 @@ def install_data( name = "models", prod_models_prefix = "prod_", test_models_prefix = "test_", - extra_prod_models = []): + extra_prod_models = [], + extra_tags = []): """Install data This macro creates 3 filegroups: @@ -44,8 +45,9 @@ def install_data( "vtm", "vtp", "xml", + "yaml", ] - exclude_patterns = ["**/test/*", "**/test*"] + exclude_patterns = ["**/test/*", "**/test/models/*", "**/test*"] prod_models_include = ["**/*.{}".format(x) for x in models_extensions] test_models_include = [ p + m @@ -81,6 +83,6 @@ def install_data( name = "install_data", data = [prod_models_target], data_dest = "share/drake/" + native.package_name(), - tags = ["install"], + tags = ["install"] + extra_tags, visibility = ["//visibility:public"], )