From ed8b63f5fe5dbe60e6cf81f0af6f57592010e43a Mon Sep 17 00:00:00 2001 From: ggould-tri Date: Wed, 2 Sep 2020 16:39:41 -0400 Subject: [PATCH 1/2] Move model directives mechanism out of dev. --- .../README.md => README_model_directives.md} | 0 multibody/parsing/dev/BUILD.bazel | 64 ------------------ .../parsing/{dev => }/model_directives.h | 0 .../{dev => }/process_model_directives.cc | 0 .../{dev => }/process_model_directives.h | 0 .../{dev => }/test/model_directives_test.cc | 0 .../test/process_model_directives_test.cc | 0 .../add_backreference.yaml | 0 .../add_scoped_mid.yaml | 0 .../add_scoped_sub.yaml | 0 .../add_scoped_top.yaml | 0 .../process_model_directives_test/circle.png | Bin .../process_model_directives_test/package.xml | 0 .../simple_model.sdf | 0 14 files changed, 64 deletions(-) rename multibody/parsing/{dev/README.md => README_model_directives.md} (100%) delete mode 100644 multibody/parsing/dev/BUILD.bazel rename multibody/parsing/{dev => }/model_directives.h (100%) rename multibody/parsing/{dev => }/process_model_directives.cc (100%) rename multibody/parsing/{dev => }/process_model_directives.h (100%) rename multibody/parsing/{dev => }/test/model_directives_test.cc (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test.cc (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/add_backreference.yaml (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/add_scoped_mid.yaml (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/add_scoped_sub.yaml (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/add_scoped_top.yaml (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/circle.png (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/package.xml (100%) rename multibody/parsing/{dev => }/test/process_model_directives_test/simple_model.sdf (100%) diff --git a/multibody/parsing/dev/README.md b/multibody/parsing/README_model_directives.md similarity index 100% rename from multibody/parsing/dev/README.md rename to multibody/parsing/README_model_directives.md diff --git a/multibody/parsing/dev/BUILD.bazel b/multibody/parsing/dev/BUILD.bazel deleted file mode 100644 index 3e986ec91088..000000000000 --- a/multibody/parsing/dev/BUILD.bazel +++ /dev/null @@ -1,64 +0,0 @@ -# -*- python -*- - -load( - "@drake//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", - "drake_cc_library", -) -load("//tools/lint:lint.bzl", "add_lint_tests") - -drake_cc_library( - name = "process_model_directives", - srcs = ["process_model_directives.cc"], - hdrs = ["process_model_directives.h"], - data = [ - "//manipulation/models/jaco_description:models", - ], - deps = [ - ":model_directives", - "//common:filesystem", - "//common:find_resource", - "//common/yaml:yaml_read_archive", - "//multibody/parsing", - "//multibody/plant", - ], -) - -filegroup( - name = "process_model_directives_test_models", - testonly = True, - data = glob(["test/process_model_directives_test/**"]), -) - -drake_cc_googletest( - name = "process_model_directives_test", - data = [ - ":process_model_directives_test_models", - ], - deps = [ - ":process_model_directives", - ], -) - -drake_cc_library( - name = "model_directives", - hdrs = [ - "model_directives.h", - ], - deps = [ - "//common:essential", - "//common:name_value", - "//common/schema:transform", - "//math:geometric_transform", - ], -) - -drake_cc_googletest( - name = "model_directives_test", - deps = [ - ":model_directives", - "//common/yaml:yaml_read_archive", - ], -) - -add_lint_tests() diff --git a/multibody/parsing/dev/model_directives.h b/multibody/parsing/model_directives.h similarity index 100% rename from multibody/parsing/dev/model_directives.h rename to multibody/parsing/model_directives.h diff --git a/multibody/parsing/dev/process_model_directives.cc b/multibody/parsing/process_model_directives.cc similarity index 100% rename from multibody/parsing/dev/process_model_directives.cc rename to multibody/parsing/process_model_directives.cc diff --git a/multibody/parsing/dev/process_model_directives.h b/multibody/parsing/process_model_directives.h similarity index 100% rename from multibody/parsing/dev/process_model_directives.h rename to multibody/parsing/process_model_directives.h diff --git a/multibody/parsing/dev/test/model_directives_test.cc b/multibody/parsing/test/model_directives_test.cc similarity index 100% rename from multibody/parsing/dev/test/model_directives_test.cc rename to multibody/parsing/test/model_directives_test.cc diff --git a/multibody/parsing/dev/test/process_model_directives_test.cc b/multibody/parsing/test/process_model_directives_test.cc similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test.cc rename to multibody/parsing/test/process_model_directives_test.cc diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_backreference.yaml b/multibody/parsing/test/process_model_directives_test/add_backreference.yaml similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/add_backreference.yaml rename to multibody/parsing/test/process_model_directives_test/add_backreference.yaml diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_mid.yaml b/multibody/parsing/test/process_model_directives_test/add_scoped_mid.yaml similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/add_scoped_mid.yaml rename to multibody/parsing/test/process_model_directives_test/add_scoped_mid.yaml diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_sub.yaml b/multibody/parsing/test/process_model_directives_test/add_scoped_sub.yaml similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/add_scoped_sub.yaml rename to multibody/parsing/test/process_model_directives_test/add_scoped_sub.yaml diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_top.yaml b/multibody/parsing/test/process_model_directives_test/add_scoped_top.yaml similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/add_scoped_top.yaml rename to multibody/parsing/test/process_model_directives_test/add_scoped_top.yaml diff --git a/multibody/parsing/dev/test/process_model_directives_test/circle.png b/multibody/parsing/test/process_model_directives_test/circle.png similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/circle.png rename to multibody/parsing/test/process_model_directives_test/circle.png diff --git a/multibody/parsing/dev/test/process_model_directives_test/package.xml b/multibody/parsing/test/process_model_directives_test/package.xml similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/package.xml rename to multibody/parsing/test/process_model_directives_test/package.xml diff --git a/multibody/parsing/dev/test/process_model_directives_test/simple_model.sdf b/multibody/parsing/test/process_model_directives_test/simple_model.sdf similarity index 100% rename from multibody/parsing/dev/test/process_model_directives_test/simple_model.sdf rename to multibody/parsing/test/process_model_directives_test/simple_model.sdf From d1eb95bfcca9aa38f4b30fa9781b29708131a275 Mon Sep 17 00:00:00 2001 From: ggould-tri Date: Wed, 2 Sep 2020 16:40:01 -0400 Subject: [PATCH 2/2] This commit applies all of the post-dev changes to model directives: * Sync with upstream changes * Rewrite to not reference dev * Removing invalid geometry from the jaco (as mentioning it in a data= line causes it to be validity checked) * Factor the scoped names mechanism out of model directives * Rewrite the weld error API to be future-proof against a future refactor. --- multibody/parsing/BUILD.bazel | 64 +++++++ multibody/parsing/README_model_directives.md | 94 +++++----- multibody/parsing/model_directives.h | 82 +++++---- multibody/parsing/process_model_directives.cc | 169 +++--------------- multibody/parsing/process_model_directives.h | 123 ++++--------- multibody/parsing/scoped_names.cc | 71 ++++++++ multibody/parsing/scoped_names.h | 65 +++++++ .../parsing/test/model_directives_test.cc | 8 +- multibody/parsing/test/package_map_test.cc | 3 + .../test/process_model_directives_test.cc | 15 +- .../process_model_directives_test/package.xml | 4 +- .../simple_model.sdf | 1 + 12 files changed, 380 insertions(+), 319 deletions(-) create mode 100644 multibody/parsing/scoped_names.cc create mode 100644 multibody/parsing/scoped_names.h diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel index 10b1eb20f204..7d0be2ccb021 100644 --- a/multibody/parsing/BUILD.bazel +++ b/multibody/parsing/BUILD.bazel @@ -34,8 +34,11 @@ drake_cc_package_library( ":detail_scene_graph", ":detail_sdf_parser", ":detail_urdf_parser", + ":model_directives", ":package_map", ":parser", + ":process_model_directives", + ":scoped_names", ], ) @@ -166,6 +169,43 @@ drake_cc_library( ], ) +drake_cc_library( + name = "model_directives", + hdrs = [ + "model_directives.h", + ], + deps = [ + "//common:essential", + "//common:name_value", + "//common/schema:transform", + "//math:geometric_transform", + ], +) + +drake_cc_library( + name = "process_model_directives", + srcs = ["process_model_directives.cc"], + hdrs = ["process_model_directives.h"], + deps = [ + ":model_directives", + ":parser", + ":scoped_names", + "//common:filesystem", + "//common:find_resource", + "//common/yaml:yaml_read_archive", + "//multibody/plant", + ], +) + +drake_cc_library( + name = "scoped_names", + srcs = ["scoped_names.cc"], + hdrs = ["scoped_names.h"], + deps = [ + "//multibody/plant", + ], +) + drake_cc_library( name = "test_loaders", testonly = 1, @@ -224,6 +264,30 @@ drake_cc_googletest( ], ) +filegroup( + name = "process_model_directives_test_models", + testonly = True, + data = glob(["test/process_model_directives_test/**"]), +) + +drake_cc_googletest( + name = "process_model_directives_test", + data = [ + ":process_model_directives_test_models", + ], + deps = [ + ":process_model_directives", + ], +) + +drake_cc_googletest( + name = "model_directives_test", + deps = [ + ":model_directives", + "//common/yaml:yaml_read_archive", + ], +) + drake_cc_googletest( name = "detail_common_test", deps = [ diff --git a/multibody/parsing/README_model_directives.md b/multibody/parsing/README_model_directives.md index 15a563c496b6..07d0a0ec0756 100644 --- a/multibody/parsing/README_model_directives.md +++ b/multibody/parsing/README_model_directives.md @@ -1,41 +1,29 @@ 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. +"Model Directives" is a small YAML-based language for building a complex +MultibodyPlant-based scene out of numerous `.sdf` and `.urdf` files. For +instance in the TRI dish-loading demo we have individual SDFormat files for +the counter, sink, cameras, pedestal, arm, gripper, and each manipuland. A +single SDFormat for this would be unwieldy and difficult to maintain and +collaborate on, but SDFormat'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. +We expect that this mechanism will be temporary and will be removed +[when SDFormat adds similar functionality](#conditions-for-deprecation). +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`. This directive is due for deprecation soon and - should generally be avoided. - * `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. +The easiest syntax reference is the unit test files in +`test/process_model_directives_test/*.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, which are described in detail in +`model_directives.h`. These directives can add model instances and add frames +to them and welds between them. ## Use @@ -52,6 +40,18 @@ A simple example of a use would be: plant.Finalize(); ``` +Or, if you are connecting a scene graph: + +```cpp + DiagramBuilder builder; + ModelDirectives station_directives = LoadModelDirectives( + FindResourceOrThrow("my_exciting_project/models/my_scene.yaml")); + auto [plant, scene_graph] = + AddMultibodyPlantSceneGraph(&builder, 0.0);; + 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. @@ -59,31 +59,45 @@ 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 `::`. +To acknowledge this, and support "faux" composition, we introduce scoping via +names. + +Names can be scoped implicitly or explicitly, and this scoping supports +multiple levels of "nesting" -- insofar as model instances in MultibodyPlant +can denote hierarchy (see #14043 for more info). -Examples: +Names are delimited by `::`; the last name portion indicates the element name, +and all preceding portions indicate the model instance name. For this reason, +element names themselves should not contain `::`. -- `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`. +(The `::` convention is used for consistency with the emerging namespace +syntax for SDFormat, soon to be required in SDFormat 1.8.) + +Thus: + +- `my_frame`: No explicit model instance, the frame is `my_frame`. +- `my_model::my_frame`: The model instance is `my_model`, the frame is + `my_frame`. +- `top_level::my_model::my_frame`: The model instance is + `top_level::my_model`, the frame is `my_frame`. ## Conditions for deprecation We expect and hope to deprecate this mechanism when either: -SDF format properly specifies, and Drake supports, the following: +SDFormat properly specifies, and Drake supports, the following: * What `` statements should *really* do (e.g. namespacing models, joints, etc.) without kludging Drake's parsing + * See for the progress of the proposed formal semantics. * 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`) + workaround using a wrapper script to inject the appropriate drake source + or resource path) diff --git a/multibody/parsing/model_directives.h b/multibody/parsing/model_directives.h index ce9c8fa300cd..e0d4a31dd6c8 100644 --- a/multibody/parsing/model_directives.h +++ b/multibody/parsing/model_directives.h @@ -1,7 +1,13 @@ #pragma once /// @file -/// Provides directives for building scenes (*not* scenarios). -/// See `common/schema/README.md` for more info. +/// Defines the YAML schema for the model directives language, which is used +/// to assemble multiple SDF and URDF files in a single MultibodyPlant. +/// +/// For more information on how structures are converted to and from YAML via +/// the Serialize/Archive mechanism, see `common/name_value.h` and +/// drake::yaml::YamlReadArchive. +/// +/// See `multibody/parsing/README_model_directives.md` for more info. #include #include @@ -20,6 +26,7 @@ namespace drake { namespace multibody { namespace parsing { +/// Directive to add a weld between two named frames, a parent and a child. struct AddWeld { bool IsValid() const { if (parent.empty()) { @@ -38,12 +45,14 @@ struct AddWeld { a->Visit(DRAKE_NVP(child)); } - // Parent frame. Can specify scope. + /// Parent frame. Can specify scope. std::string parent; - // Child frame. Can (and should) specify scope. + /// Child frame. Can (and should) specify scope. std::string child; }; +/// Directive to add a model from a URDF or SDFormat file to a scene, using a +/// given name for the added instance. struct AddModel { bool IsValid() const { if (file.empty()) { @@ -62,11 +71,13 @@ struct AddModel { a->Visit(DRAKE_NVP(name)); } + /// The `package://` URI of the file to add. std::string file; - // Model instance name. + /// The model instance name. std::string name; }; +/// Directive to add an empty, named model instance to a scene. struct AddModelInstance { bool IsValid() const { if (name.empty()) { @@ -80,9 +91,14 @@ struct AddModelInstance { a->Visit(DRAKE_NVP(name)); } + /// The model instance name. std::string name; }; +/// Directive to add a path in the filesystem to the resolution of +/// `package://` URIs during the processing of model directives and the +/// models they load. +/// @warning: This is expected to be deprecated soon. struct AddPackagePath { bool IsValid() const { if (name.empty()) { @@ -101,10 +117,14 @@ struct AddPackagePath { a->Visit(DRAKE_NVP(path)); } + /// The model instance name. std::string name; + /// The filesystem path to the package root. std::string path; }; +/// Directive to add a Frame to the scene. The added frame must have a name +/// and a transform with a base frame and offset. struct AddFrame { bool IsValid() const { if (name.empty()) { @@ -123,14 +143,16 @@ struct AddFrame { a->Visit(DRAKE_NVP(X_PF)); } - // Name of frame to be added. If scope is specified, will override model - // instance; otherwise, will use `X_PF.base_frame`s instance. + /// Name of frame to be added. If scope is specified, will override model + /// instance; otherwise, will use `X_PF.base_frame`s instance. std::string name; - // Pose of frame to be added, `F`, w.r.t. parent frame `P` (as defined by - // `X_PF.base_frame`). + /// Pose of frame to be added, `F`, w.r.t. parent frame `P` (as defined by + /// `X_PF.base_frame`). drake::schema::Transform X_PF; }; +/// Directive to incorporate another model directives file, optionally with +/// its elements prefixed with a namespace. struct AddDirectives { bool IsValid() const { if (file.empty()) { @@ -146,20 +168,27 @@ struct AddDirectives { a->Visit(DRAKE_NVP(model_namespace)); } + /// The `package://` URI of the file to add. std::string file; - // Namespaces base model instance for processing directive files. - // Affects scoping (i.e. the following members): - // - AddModel::name - // - AddModelInstance::name - // - AddFrame::name - // - AddWeld::parent - // - AddWeld::child - // - AddFrame::X_PF::base_frame - // - AddDirectives::model_namespace - // See `README.md` for example references and namespacing. + /// Namespaces base model instance for processing directive files. + /// Affects scoping (i.e. the following members): + /// - AddModel::name + /// - AddModelInstance::name + /// - AddFrame::name + /// - AddWeld::parent + /// - AddWeld::child + /// - AddFrame::X_PF::base_frame + /// - AddDirectives::model_namespace + /// See `README.md` for example references and namespacing. std::optional model_namespace; }; +// TODO(eric.cousineau): Change this (and relevant YAML files) to use tags. +/// Union structure for model directives. +/// +/// @note This was designed before support for `std::variant<>` was around, +/// and thus we used a parent field, rather than a YAML tag, to designate the +/// intended type for the directive. struct ModelDirective { bool IsValid() const { const bool unique = @@ -204,6 +233,7 @@ struct ModelDirective { std::optional add_directives; }; +/// Top-level structure for a model directives yaml file schema. struct ModelDirectives { bool IsValid() const { for (auto& directive : directives) { @@ -220,20 +250,6 @@ struct ModelDirectives { std::vector directives; }; -/// Syntactic sugar for a common idiom: Construct an add_package_path -/// directive and insert it at the beginning of a ModelDirectives. -inline void AddPackageToModelDirectives(const std::string& package_name, - const std::string& package_path, - ModelDirectives* directives) { - AddPackagePath add_package_path; - add_package_path.name = package_name; - add_package_path.path = package_path; - ModelDirective directive; - directive.add_package_path = add_package_path; - directives->directives.insert( - directives->directives.begin(), directive); -} - } // namespace parsing } // namespace multibody } // namespace drake diff --git a/multibody/parsing/process_model_directives.cc b/multibody/parsing/process_model_directives.cc index b0434bd3c2b6..0fbfcf25340b 100644 --- a/multibody/parsing/process_model_directives.cc +++ b/multibody/parsing/process_model_directives.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/parsing/dev/process_model_directives.h" +#include "drake/multibody/parsing/process_model_directives.h" #include #include @@ -10,6 +10,7 @@ #include "drake/common/schema/transform.h" #include "drake/common/yaml/yaml_read_archive.h" #include "drake/multibody/parsing/parser.h" +#include "drake/multibody/parsing/scoped_names.h" namespace drake { namespace multibody { @@ -45,41 +46,7 @@ std::unique_ptr ConstructIfNullAndReassign(T** ptr, Args&&... args) { } // namespace -namespace internal { - -ScopedName ParseScopedName(const std::string& full_name) { - const std::string delim = "::"; - size_t pos = full_name.rfind(delim); - ScopedName result; - if (pos == std::string::npos) { - result.name = full_name; - } else { - result.instance_name = full_name.substr(0, pos); - // "Global scope" (e.g. "::my_frame") not supported. - DRAKE_DEMAND(!result.instance_name.empty()); - result.name = full_name.substr(pos + delim.size()); - } - return result; -} - -std::string PrefixName(const std::string& namespace_, const std::string& name) { - if (namespace_.empty()) - return name; - else if (name.empty()) - return namespace_; - else - return namespace_ + "::" + name; -} - -std::string GetInstanceScopeName( - const MultibodyPlant& plant, - ModelInstanceIndex instance) { - if (instance != plant.world_body().model_instance()) { - return plant.GetModelInstanceName(instance); - } else { - return ""; - } -} +namespace { // Add a new weld joint to @p plant from @p parent_frame as indicated by @p // weld (as resolved relative to @p model_namespace) and update the @p info @@ -92,12 +59,18 @@ void AddWeldWithOptionalError( const Frame& child_frame, ModelWeldErrorFunction error_func, MultibodyPlant* plant, - std::vector* added_models) { - // TODO(eric.cousineau): This hack really shouldn't belong in model + std::vector* added_models) { + // TODO(#14084): This hack really shouldn't belong in model // directives. Instead, it should live externally as a transformation on // ModelDirectives (either flattened or recursive). + std::string parent_full_name = + PrefixName(GetInstanceScopeName(*plant, parent_frame.model_instance()), + parent_frame.name()); + std::string child_full_name = + PrefixName(GetInstanceScopeName(*plant, child_frame.model_instance()), + child_frame.name()); std::optional X_PCe = - error_func ? error_func(parent_frame, child_frame) : std::nullopt; + error_func ? error_func(parent_full_name, child_full_name) : std::nullopt; if (X_PCe.has_value()) { // N.B. Since this will belong in the child_frame's model instance, we // shouldn't worry about name collisions. @@ -114,12 +87,12 @@ void AddWeldWithOptionalError( plant->WeldFrames(parent_frame, child_frame); } if (added_models) { - // Record weld info into crappy ModelInfo struct. + // Record weld info into crappy ModelInstanceInfo struct. bool found = false; for (auto& info : *added_models) { if (info.model_instance == child_frame.model_instance()) { found = true; - // See warning in ModelInfo about these members. + // See warning in ModelInstanceInfo about these members. info.parent_frame_name = parent_frame.name(); info.child_frame_name = child_frame.name(); } @@ -130,7 +103,7 @@ void AddWeldWithOptionalError( void ProcessModelDirectivesImpl( const ModelDirectives& directives, MultibodyPlant* plant, - std::vector* added_models, Parser* parser, + std::vector* added_models, Parser* parser, const std::string& model_namespace, ModelWeldErrorFunction error_func) { drake::log()->debug("ProcessModelDirectives(MultibodyPlant)"); @@ -147,19 +120,9 @@ void ProcessModelDirectivesImpl( return GetScopedFrameByName(*plant, PrefixName(model_namespace, name)); }; - // Add `package://jaco_description` URIs even though that package lacks a - // `package.xml` file. - // TODO(ggould) ...which is bad and wrong. - if (!parser->package_map().Contains("jaco_description")) { - const fs::path path_inside_jaco = drake::FindResourceOrThrow( - "drake/manipulation/models/jaco_description/LICENSE.TXT"); - parser->package_map().Add("jaco_description", - path_inside_jaco.parent_path().string()); - } - for (auto& directive : directives.directives) { if (directive.add_model) { - ModelInfo info; + ModelInstanceInfo info; auto& model = *directive.add_model; const std::string name = PrefixName(model_namespace, model.name); drake::log()->debug(" add_model: {}\n {}", name, model.file); @@ -181,6 +144,11 @@ void ProcessModelDirectivesImpl( } else if (directive.add_frame) { auto& frame = *directive.add_frame; drake::log()->debug(" add_frame: {}", frame.name); + if (!frame.X_PF.base_frame) { + // This would be caught elsewhere, but it is clearer to throw here. + throw std::logic_error( + "add_frame directive with empty base frame is ambiguous"); + } // Only override instance if scope is explicitly specified. std::optional instance; ScopedName parsed = ParseScopedName(frame.name); @@ -241,7 +209,7 @@ void ProcessModelDirectivesImpl( } } -} // namespace internal +} // namespace std::string ResolveModelDirectiveUri(const std::string& uri, const PackageMap& package_map) { @@ -276,42 +244,15 @@ std::string ResolveModelDirectiveUri(const std::string& uri, return package_map.GetPath(package_name) + "/" + path_in_package; } -const drake::multibody::Frame* -GetScopedFrameByNameMaybe( - const drake::multibody::MultibodyPlant& plant, - const std::string& full_name) { - if (full_name == "world") - return &plant.world_frame(); - auto result = internal::ParseScopedName(full_name); - if (!result.instance_name.empty()) { - auto instance = plant.GetModelInstanceByName(result.instance_name); - if (plant.HasFrameNamed(result.name, instance)) { - return &plant.GetFrameByName(result.name, instance); - } - } else if (plant.HasFrameNamed(result.name)) { - return &plant.GetFrameByName(result.name); - } - return nullptr; -} - -const std::string GetScopedFrameName( - const drake::multibody::MultibodyPlant& plant, - const drake::multibody::Frame& frame) { - if (&frame == &plant.world_frame()) - return "world"; - return internal::PrefixName(internal::GetInstanceScopeName( - plant, frame.model_instance()), frame.name()); -} - void ProcessModelDirectives( const ModelDirectives& directives, MultibodyPlant* plant, - std::vector* added_models, Parser* parser, + std::vector* added_models, Parser* parser, ModelWeldErrorFunction error_func) { auto tmp_parser = ConstructIfNullAndReassign(&parser, plant); auto tmp_added_model = - ConstructIfNullAndReassign>(&added_models); + ConstructIfNullAndReassign>(&added_models); const std::string model_namespace = ""; - internal::ProcessModelDirectivesImpl( + ProcessModelDirectivesImpl( directives, plant, added_models, parser, model_namespace, error_func); } @@ -346,66 +287,6 @@ void FlattenModelDirectives( } } -ModelInfo MakeModelInfo(const std::string& model_name, - const std::string& model_path, - const std::string& parent_frame_name, - const std::string& child_frame_name, - const drake::math::RigidTransformd& X_PC) { - ModelInfo info; - info.model_name = model_name; - info.model_path = model_path; - info.parent_frame_name = parent_frame_name; - info.child_frame_name = child_frame_name; - info.X_PC = X_PC; - return info; -} - -// TODO(eric.cousineau): Do we *really* need this function? This seems like -// it'd be better handled as an MBP subgraph. -ModelDirectives MakeModelsAttachedToFrameDirectives( - const std::vector& models_to_add) { - ModelDirectives directives; - - // One for add frame, one for add model, one for add weld. - directives.directives.resize(models_to_add.size() * 3); - - int index = 0; - for (size_t i = 0; i < models_to_add.size(); i++) { - const ModelInfo& model_to_add = models_to_add.at(i); - std::string attachment_frame_name = model_to_add.parent_frame_name; - - // Add frame first. - if (!model_to_add.X_PC.IsExactlyIdentity()) { - AddFrame frame_dir; - attachment_frame_name = model_to_add.model_name + "_attachment_frame"; - frame_dir.name = attachment_frame_name; - 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 = - drake::schema::Rotation{model_to_add.X_PC.rotation()}; - - directives.directives.at(index++).add_frame = frame_dir; - } - - // Add model - AddModel model_dir; - model_dir.file = model_to_add.model_path; - model_dir.name = model_to_add.model_name; - - directives.directives.at(index++).add_model = model_dir; - - AddWeld weld_dir; - weld_dir.parent = attachment_frame_name; - weld_dir.child = - model_to_add.model_name + "::" + model_to_add.child_frame_name; - directives.directives.at(index++).add_weld = weld_dir; - } - directives.directives.resize(index); - - return directives; -} - } // namespace parsing } // namespace multibody } // namespace drake diff --git a/multibody/parsing/process_model_directives.h b/multibody/parsing/process_model_directives.h index 78aec2d2a6b4..d6e59530bbc5 100644 --- a/multibody/parsing/process_model_directives.h +++ b/multibody/parsing/process_model_directives.h @@ -4,7 +4,7 @@ #include #include -#include "drake/multibody/parsing/dev/model_directives.h" +#include "drake/multibody/parsing/model_directives.h" #include "drake/multibody/parsing/package_map.h" #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" @@ -16,114 +16,65 @@ namespace parsing { ModelDirectives LoadModelDirectives(const std::string& filename); +/// Converts URIs into filesystem absolute paths. +/// /// ModelDirectives refer to their resources by URIs like /// `package://somepackage/somepath/somefile.sdf`, where somepackage refers to -/// the ROS-style package.xml system. This method converts those URIs into -/// filesystem absolute paths. +/// the ROS-style package.xml system. std::string ResolveModelDirectiveUri( const std::string& uri, const drake::multibody::PackageMap& package_map); -// TODO(eric.cousineau): Rename this to `ModelInstanceInfo` to deconflict with -// `model_info.h`. -// TODO(eric.cousineau): Burn this in a dumpster fire pending real model -// composition / extraction in Drake. This is just dumb. -struct ModelInfo { - // Model name (possibly scoped). +// TODO(#13074): Burn this in a dumpster fire pending real model +// composition / extraction in Drake. +// TODO(#14084): This structure might combine with the implementation of 14084: +// https://github.com/RobotLocomotion/drake/issues/14084#issuecomment-694394869 +/// Convenience structure to hold all of the information to add a model +/// instance from a file. +struct ModelInstanceInfo { + /// Model name (possibly scoped). std::string model_name; - // File path. + /// File path. std::string model_path; - // WARNING: This is the *unscoped* parent frame, assumed to be unique. + /// WARNING: This is the *unscoped* parent frame, assumed to be unique. std::string parent_frame_name; - // This is the unscoped frame name belonging to `model_instance`. + /// This is the unscoped frame name belonging to `model_instance`. std::string child_frame_name; drake::math::RigidTransformd X_PC; drake::multibody::ModelInstanceIndex model_instance; }; -ModelInfo MakeModelInfo( - const std::string& model_name, const std::string& model_path, - const std::string& parent_frame_name, const std::string& child_frame_name, - const drake::math::RigidTransformd& X_PC = drake::math::RigidTransformd()); - -/** - * If `X_PC` is not identity, a AddFrame directive is made to insert a - * new frame named "model_name_attachment_frame" that's offset from - * `parent_frame_name` by `X_PC`, and the subsequent AddModel directive is - * made to weld `model_name`'s `child_frame_name` to this new frame. - * Otherwise, a AddModel directive is made to weld `model_name`'s - * `child_frame_name` to `parent_frame_name` directly. The `model_instance` - * field is ignored for this purposes. - */ -ModelDirectives MakeModelsAttachedToFrameDirectives( - const std::vector& models_to_add); - -// Flatten model directives. +/// Flatten model directives. void FlattenModelDirectives(const ModelDirectives& directives, const drake::multibody::PackageMap& package_map, ModelDirectives* out); -/// Provides a magical way to inject error (randomization) for synthesis. -/// Maps from (parent_frame, child_frame) -> X_PCe, where Ce is the perturbed -/// child frame pose w.r.t. parent frame P. If there is no error, then nullopt -/// should be returned. +// TODO(#13520) This rather ugly mechanism was added because a caller cannot +// add model error after `ProcessModelDirectives` and so needs to pass any +// requested error in beforehand. However: +// TODO(#14084) It is likely that we will remove this mechanism in the future +// and replace it with a separate model directives transform stage. +// +/// (Advanced) Provides a magical way to inject error into model directives, +/// for instance if the caller has modeling error to add that is not reflected +/// in the directives file. Maps from (parent_frame, child_frame) -> X_PCe, +/// where Ce is the perturbed child frame pose w.r.t. parent frame P. If there +/// is no error, then nullopt should be returned. using ModelWeldErrorFunction = std::function( - const drake::multibody::Frame&, - const drake::multibody::Frame&)>; + const std::string& parent, + const std::string& child)>; /// Processes model directives for a given MultibodyPlant. /// -/// Note: The passed-in parser will be mutated to add the jaco_description -/// package to its package map (since model directives and their contents are -/// allowed to refer to the package directly for workaround reasons). -void ProcessModelDirectives(const ModelDirectives& directives, - drake::multibody::MultibodyPlant* plant, - std::vector* added_models = nullptr, - drake::multibody::Parser* parser = nullptr, - ModelWeldErrorFunction = nullptr); - -/// Finds an optionally model-scoped frame according to -/// `internal::ScopedNameParser::Parse`. -const drake::multibody::Frame* -GetScopedFrameByNameMaybe( - const drake::multibody::MultibodyPlant& plant, - const std::string& full_name); - -/// Required version of `GetScopedFrameByNameMaybe`. -inline const drake::multibody::Frame& -GetScopedFrameByName( - const drake::multibody::MultibodyPlant& plant, - const std::string& full_name) { - auto* frame = GetScopedFrameByNameMaybe(plant, full_name); - if (frame == nullptr) { - throw std::runtime_error("Could not find frame: " + full_name); - } - return *frame; -} - -const std::string GetScopedFrameName( - const drake::multibody::MultibodyPlant& plant, - const drake::multibody::Frame& frame); - -namespace internal { - -// TODO(eric.cousineau): Consider hoisting this. -std::string FindDirectiveResource(const std::string& name); - -struct ScopedName { - // If empty, implies no scope. - std::string instance_name; - std::string name; -}; - -// Attempts to find a name using the following scoping rules: -// - The delimiter "::" may appear zero or more times. -// - If one more delimiters are present, the full name is split by the *last* -// delimiter. The provided model instance name must exist. -ScopedName ParseScopedName(const std::string& full_name); - -} // namespace internal +/// @note the ModelWeldErrorFunction argument, described above, is likely to +/// go away when a cleaner mechanism is developed. +void ProcessModelDirectives( + const ModelDirectives& directives, + drake::multibody::MultibodyPlant* plant, + std::vector* added_models = nullptr, + drake::multibody::Parser* parser = nullptr, + ModelWeldErrorFunction = nullptr); } // namespace parsing } // namespace multibody diff --git a/multibody/parsing/scoped_names.cc b/multibody/parsing/scoped_names.cc new file mode 100644 index 000000000000..656b951c2150 --- /dev/null +++ b/multibody/parsing/scoped_names.cc @@ -0,0 +1,71 @@ +#include "drake/multibody/parsing/scoped_names.h" + +namespace drake { +namespace multibody { +namespace parsing { + +constexpr char const* kDelim = "::"; + +const drake::multibody::Frame* +GetScopedFrameByNameMaybe( + const drake::multibody::MultibodyPlant& plant, + const std::string& full_name) { + if (full_name == "world") + return &plant.world_frame(); + auto result = ParseScopedName(full_name); + if (!result.instance_name.empty()) { + auto instance = plant.GetModelInstanceByName(result.instance_name); + if (plant.HasFrameNamed(result.name, instance)) { + return &plant.GetFrameByName(result.name, instance); + } + } else if (plant.HasFrameNamed(result.name)) { + return &plant.GetFrameByName(result.name); + } + return nullptr; +} + +const std::string GetScopedFrameName( + const drake::multibody::MultibodyPlant& plant, + const drake::multibody::Frame& frame) { + if (&frame == &plant.world_frame()) + return "world"; + return PrefixName(GetInstanceScopeName( + plant, frame.model_instance()), frame.name()); +} + +ScopedName ParseScopedName(const std::string& full_name) { + size_t pos = full_name.rfind(kDelim); + ScopedName result; + if (pos == std::string::npos) { + result.name = full_name; + } else { + result.instance_name = full_name.substr(0, pos); + // "Global scope" (e.g. "::my_frame") not supported. + DRAKE_DEMAND(!result.instance_name.empty()); + result.name = full_name.substr(pos + std::string(kDelim).size()); + } + return result; +} + +std::string PrefixName(const std::string& namespace_, const std::string& name) { + if (namespace_.empty()) + return name; + else if (name.empty()) + return namespace_; + else + return namespace_ + kDelim + name; +} + +std::string GetInstanceScopeName( + const MultibodyPlant& plant, + ModelInstanceIndex instance) { + if (instance != plant.world_body().model_instance()) { + return plant.GetModelInstanceName(instance); + } else { + return ""; + } +} + +} // namespace parsing +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/scoped_names.h b/multibody/parsing/scoped_names.h new file mode 100644 index 000000000000..60e17af81197 --- /dev/null +++ b/multibody/parsing/scoped_names.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include "drake/multibody/plant/multibody_plant.h" + +/// @file Implements the namespace scoping semantics described in +/// `README_model_directives.md` for multibody plants built from model +/// directives (and possibly future SDFormat files). + +namespace drake { +namespace multibody { +namespace parsing { + +/// Finds an optionally model-scoped frame, using the naming rules of +/// `ParseScopedName`. +/// +/// Returns `nullptr` if the frame is not found, as well as all the error +/// cases of `MultibodyPlant::HasFrameByName(std::string)`. +const drake::multibody::Frame* +GetScopedFrameByNameMaybe( + const drake::multibody::MultibodyPlant& plant, + const std::string& full_name); + +/// Equivalent to `GetScopedFrameByNameMaybe`, but throws if the frame +/// is not found. +inline const drake::multibody::Frame& +GetScopedFrameByName( + const drake::multibody::MultibodyPlant& plant, + const std::string& full_name) { + auto* frame = GetScopedFrameByNameMaybe(plant, full_name); + if (frame == nullptr) { + throw std::runtime_error("Could not find frame: " + full_name); + } + return *frame; +} + +/// Convenience class for a scoped name. +struct ScopedName { + /// The name of the multibody instance part of a scoped name. If empty, + /// implies no multibody instance scope. + std::string instance_name; + + /// The model-instance-specific part of the name, ie the name of the frame, + /// body, etc. within the instance. + std::string name; +}; + +/// Attempts to find a name using the following scoping rules: +/// - The delimiter "::" may appear zero or more times. +/// - If one more delimiters are present, the full name is split by the *last* +/// delimiter. The provided model instance name must exist. +ScopedName ParseScopedName(const std::string& full_name); + +/// Composes a "namespace::name" name from its components. +std::string PrefixName(const std::string& namespace_, const std::string& name); + +/// Gets the namespace prefix for a given model instance. +std::string GetInstanceScopeName( + const drake::multibody::MultibodyPlant&, + drake::multibody::ModelInstanceIndex); + +} // namespace parsing +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/test/model_directives_test.cc b/multibody/parsing/test/model_directives_test.cc index 4de1b3a33206..c6cf3d6937d3 100644 --- a/multibody/parsing/test/model_directives_test.cc +++ b/multibody/parsing/test/model_directives_test.cc @@ -1,5 +1,5 @@ // Minimal test to make sure stuff doesn't explode. -#include "drake/multibody/parsing/dev/model_directives.h" +#include "drake/multibody/parsing/model_directives.h" #include @@ -12,10 +12,6 @@ namespace multibody { namespace parsing { namespace { -// TODO(jeremy.nimmer) We can remove this argument from the call sites below -// once Drake's YamlReadArchive constructor default changes to be strict. -constexpr YamlReadArchive::Options kStrict; - GTEST_TEST(ModelDirectivesTest, Success) { const char* contents = R"""( directives: @@ -44,7 +40,7 @@ GTEST_TEST(ModelDirectivesTest, Success) { model_namespace: right )"""; ModelDirectives directives; - YamlReadArchive(YAML::Load(contents), kStrict).Accept(&directives); + YamlReadArchive(YAML::Load(contents)).Accept(&directives); EXPECT_TRUE(directives.IsValid()); } diff --git a/multibody/parsing/test/package_map_test.cc b/multibody/parsing/test/package_map_test.cc index eaf68d3de65d..e52aa8bd091c 100644 --- a/multibody/parsing/test/package_map_test.cc +++ b/multibody/parsing/test/package_map_test.cc @@ -52,6 +52,9 @@ void VerifyMatchWithTestDataRoot(const PackageMap& package_map) { "package_map_test_package_d/"}, {"box_model", root_path + "box_package/"}, + // TODO(#10531) This should use `package://drake`. + {"process_model_directives_test", root_path + + "process_model_directives_test/"}, }; VerifyMatch(package_map, expected_packages); } diff --git a/multibody/parsing/test/process_model_directives_test.cc b/multibody/parsing/test/process_model_directives_test.cc index 831a47d0752f..f9dbe5edd10f 100644 --- a/multibody/parsing/test/process_model_directives_test.cc +++ b/multibody/parsing/test/process_model_directives_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/parsing/dev/process_model_directives.h" +#include "drake/multibody/parsing/process_model_directives.h" #include @@ -7,6 +7,7 @@ #include "drake/common/filesystem.h" #include "drake/common/find_resource.h" #include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/scoped_names.h" #include "drake/multibody/plant/multibody_plant.h" namespace drake { @@ -23,7 +24,7 @@ using drake::multibody::Parser; using drake::systems::DiagramBuilder; const char* const kTestDir = - "drake/multibody/parsing/dev/test/process_model_directives_test"; + "drake/multibody/parsing/test/process_model_directives_test"; // Our unit test's package is not normally loaded; construct a parser that // has it and can resolve package://process_model_directives_test urls. @@ -111,13 +112,11 @@ GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) { // frame. MultibodyPlant plant(0.0); - auto error = [&](const Frame& parent, const Frame& child) { - auto& error_parent = plant.GetFrameByName( - "frame", plant.GetModelInstanceByName("simple_model")); - auto& error_child = plant.GetFrameByName( - "base", plant.GetModelInstanceByName("extra_model")); + auto error = [&](const std::string& parent, const std::string& child) { + const std::string error_parent = "simple_model::frame"; + const std::string error_child = "extra_model::base"; optional out; - if (&parent == &error_parent && &child == &error_child) + if (parent == error_parent && child == error_child) out = error_transform; return out; }; diff --git a/multibody/parsing/test/process_model_directives_test/package.xml b/multibody/parsing/test/process_model_directives_test/package.xml index 61c989b9ab4f..99b7a0a5640e 100644 --- a/multibody/parsing/test/process_model_directives_test/package.xml +++ b/multibody/parsing/test/process_model_directives_test/package.xml @@ -5,7 +5,7 @@ Unit test data for `process_model_directives.cc` - John Doe - Jane Doe + Grant Gould + Grant Gould N/A diff --git a/multibody/parsing/test/process_model_directives_test/simple_model.sdf b/multibody/parsing/test/process_model_directives_test/simple_model.sdf index 90e709ee4775..40daf97ff099 100644 --- a/multibody/parsing/test/process_model_directives_test/simple_model.sdf +++ b/multibody/parsing/test/process_model_directives_test/simple_model.sdf @@ -17,6 +17,7 @@ package:// URI resolution. --> + package://process_model_directives_test/circle.png