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..8270b45d3a6a --- /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", +) +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/dev: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/README.md b/multibody/parsing/dev/README.md new file mode 100644 index 000000000000..15a563c496b6 --- /dev/null +++ b/multibody/parsing/dev/README.md @@ -0,0 +1,89 @@ +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`. 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. + + +## 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 new file mode 100644 index 000000000000..5679c01edb2a --- /dev/null +++ b/multibody/parsing/dev/model_directives.h @@ -0,0 +1,239 @@ +#pragma once +/// @file +/// Provides directives for building scenes (*not* scenarios). +/// See `common/schema/README.md` for more info. + +#include +#include +#include + +#include + +#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" + +namespace drake { +namespace multibody { +namespace parsing { + +struct AddWeld { + bool IsValid() const { + if (parent.empty()) { + drake::log()->error("add_weld: `parent` must be non-empty"); + return false; + } else if (child.empty()) { + drake::log()->error("add_weld: `child` must be non-empty"); + return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(parent)); + a->Visit(DRAKE_NVP(child)); + } + + // Parent frame. Can specify scope. + std::string parent; + // Child frame. Can (and should) specify scope. + std::string child; +}; + +struct AddModel { + bool IsValid() const { + if (file.empty()) { + drake::log()->error("add_model: `file` must be non-empty"); + return false; + } else if (name.empty()) { + drake::log()->error("add_model: `name` must be non-empty"); + return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(file)); + a->Visit(DRAKE_NVP(name)); + } + + std::string file; + // Model instance name. + std::string name; +}; + +struct AddModelInstance { + bool IsValid() const { + if (name.empty()) { + drake::log()->error("add_model_instance: `name` must be non-empty"); + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(name)); + } + + std::string name; +}; + +struct AddPackagePath { + bool IsValid() const { + if (name.empty()) { + drake::log()->error("add_package_path: `name` must be non-empty."); + return false; + } else if (path.empty()) { + drake::log()->error("add_package_path: `path` must be non-empty."); + return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(name)); + a->Visit(DRAKE_NVP(path)); + } + + std::string name; + std::string path; +}; + +struct AddFrame { + bool IsValid() const { + if (name.empty()) { + drake::log()->error("add_frame: `name` must be non-empty"); + return false; + } else if (!X_PF.base_frame || X_PF.base_frame->empty()) { + drake::log()->error("add_frame: `X_PF.base_frame` must be defined"); + return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(name)); + 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. + std::string name; + // 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; +}; + +struct AddDirectives { + bool IsValid() const { + if (file.empty()) { + drake::log()->error("add_directives: `file` must be non-empty"); + return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(file)); + a->Visit(DRAKE_NVP(model_namespace)); + } + + 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. + std::optional model_namespace; +}; + +struct ModelDirective { + bool IsValid() const { + const bool unique = + (add_model.has_value() + add_model_instance.has_value() + + add_frame.has_value() + add_weld.has_value() + + add_package_path.has_value() + add_directives.has_value()) == 1; + if (!unique) { + drake::log()->error( + "directive: Specify one of `add_model`, `add_model_instance`, " + "`add_frame`, `add_package_path`, or `add_directives`"); + return false; + } else if (add_model) { + return add_model->IsValid(); + } else if (add_model_instance) { + return add_model_instance->IsValid(); + } else if (add_frame) { + return add_frame->IsValid(); + } else if (add_weld) { + return add_weld->IsValid(); + } else if (add_package_path) { + return add_package_path->IsValid(); + } else { + return add_directives->IsValid(); + } + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(add_model)); + a->Visit(DRAKE_NVP(add_model_instance)); + a->Visit(DRAKE_NVP(add_frame)); + a->Visit(DRAKE_NVP(add_weld)); + a->Visit(DRAKE_NVP(add_package_path)); + a->Visit(DRAKE_NVP(add_directives)); + } + + std::optional add_model; + std::optional add_model_instance; + std::optional add_frame; + std::optional add_weld; + std::optional add_package_path; + std::optional add_directives; +}; + +struct ModelDirectives { + bool IsValid() const { + for (auto& directive : directives) { + if (!directive.IsValid()) return false; + } + return true; + } + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(directives)); + } + + 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/dev/process_model_directives.cc b/multibody/parsing/dev/process_model_directives.cc new file mode 100644 index 000000000000..c8d7fc3c47c2 --- /dev/null +++ b/multibody/parsing/dev/process_model_directives.cc @@ -0,0 +1,411 @@ +#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" + +namespace drake { +namespace multibody { +namespace parsing { + +using std::make_unique; +using Eigen::Isometry3d; + +namespace fs = drake::filesystem; +using drake::FindResourceOrThrow; +using drake::math::RigidTransformd; +using drake::multibody::FixedOffsetFrame; +using drake::multibody::Frame; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::PackageMap; +using drake::multibody::Parser; +using drake::yaml::YamlReadArchive; + +namespace { + +// If `*ptr` is null, construct `T(args...)` and reassign the pointer. +template +std::unique_ptr ConstructIfNullAndReassign(T** ptr, Args&&... args) { + DRAKE_DEMAND(ptr != nullptr); + std::unique_ptr out; + if (!*ptr) { + out = std::make_unique(std::forward(args)...); + *ptr = out.get(); + } + return out; +} + +} // 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 ""; + } +} + +// 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 +// for the child model accordingly. +// +// If @p error_func is provided (non-empty), use it to compute an offset X_PCe +// for the weld. +void AddWeldWithOptionalError( + const Frame& parent_frame, + const Frame& child_frame, + ModelWeldErrorFunction error_func, + MultibodyPlant* plant, + std::vector* added_models) { + // TODO(eric.cousineau): This hack really shouldn't belong in model + // directives. Instead, it should live externally as a transformation on + // ModelDirectives (either flattened or recursive). + std::optional X_PCe = + error_func ? error_func(parent_frame, child_frame) : 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. + const std::string weld_error_name = + parent_frame.name() + "_weld_error_to_" + child_frame.name(); + drake::log()->debug("ProcessAddWeld adding error frame {}", + weld_error_name); + const auto& error_frame = + plant->AddFrame(make_unique>( + weld_error_name, parent_frame, *X_PCe, + child_frame.model_instance())); + plant->WeldFrames(error_frame, child_frame); + } else { + plant->WeldFrames(parent_frame, child_frame); + } + if (added_models) { + // Record weld info into crappy ModelInfo 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. + info.parent_frame_name = parent_frame.name(); + info.child_frame_name = child_frame.name(); + } + } + DRAKE_DEMAND(found); + } +} + +void ProcessModelDirectivesImpl( + const ModelDirectives& directives, MultibodyPlant* plant, + std::vector* added_models, Parser* parser, + const std::string& model_namespace, + ModelWeldErrorFunction error_func) { + drake::log()->debug("ProcessModelDirectives(MultibodyPlant)"); + DRAKE_DEMAND(plant != nullptr); + DRAKE_DEMAND(added_models != nullptr); + // TODO(eric.cousineau): Somehow assert that our `parser` doesn't have a + // different plant? + DRAKE_DEMAND(parser != nullptr); + auto get_scoped_frame = [&](const std::string& name) -> const Frame& { + // TODO(eric.cousineau): Simplify logic? + if (name == "world") { + return plant->world_frame(); + } + 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; + auto& model = *directive.add_model; + const std::string name = PrefixName(model_namespace, model.name); + drake::log()->debug(" add_model: {}\n {}", name, model.file); + const std::string file = + ResolveModelDirectiveUri(model.file, parser->package_map()); + drake::multibody::ModelInstanceIndex child_model_instance_id = + parser->AddModelFromFile(file, name); + info.model_instance = child_model_instance_id; + info.model_name = name; + info.model_path = file; + if (added_models) added_models->push_back(info); + + } else if (directive.add_model_instance) { + auto& instance = *directive.add_model_instance; + const std::string name = PrefixName(model_namespace, instance.name); + drake::log()->debug(" add_model_instance: {}", name); + plant->AddModelInstance(name); + + } else if (directive.add_frame) { + auto& frame = *directive.add_frame; + drake::log()->debug(" add_frame: {}", frame.name); + // Only override instance if scope is explicitly specified. + std::optional instance; + ScopedName parsed = ParseScopedName(frame.name); + if (!parsed.instance_name.empty()) { + parsed.instance_name = PrefixName( + model_namespace, parsed.instance_name); + instance = plant->GetModelInstanceByName(parsed.instance_name); + } + auto& added = plant->AddFrame(make_unique>( + parsed.name, get_scoped_frame(*frame.X_PF.base_frame), + frame.X_PF.GetDeterministicValue(), instance)); + const std::string resolved_name = PrefixName( + GetInstanceScopeName(*plant, added.model_instance()), added.name()); + drake::log()->debug(" resolved_name: {}", resolved_name); + + } else if (directive.add_weld) { + AddWeldWithOptionalError( + get_scoped_frame(directive.add_weld->parent), + get_scoped_frame(directive.add_weld->child), + error_func, plant, added_models); + + } else if (directive.add_package_path) { + auto& package_path = *directive.add_package_path; + drake::log()->debug(" add_package_path: {}", package_path.name); + const fs::path abspath_xml = + drake::FindResourceOrThrow(package_path.path + "/package.xml"); + const std::string path = abspath_xml.parent_path().string(); + + // It is possible to get the same `add_package_path` directive twice, + // e.g. by including the same directives yaml twice. That's fine so + // long as they agree. + if (parser->package_map().Contains(package_path.name)) { + DRAKE_DEMAND(parser->package_map().GetPath(package_path.name) == path); + } else { + parser->package_map().Add(package_path.name, path); + } + } else { + // Recurse. + auto& sub = *directive.add_directives; + std::string new_model_namespace = PrefixName( + model_namespace, sub.model_namespace.value_or("")); + // Ensure we have a model instance for this namespace. + drake::log()->debug(" add_directives: {}", sub.file); + drake::log()->debug(" new_model_namespace: {}", new_model_namespace); + if (!new_model_namespace.empty() && + !plant->HasModelInstanceNamed(new_model_namespace)) { + throw std::runtime_error(fmt::format( + "Namespace '{}' does not exist as model instance", + new_model_namespace)); + } + auto sub_directives = + LoadModelDirectives( + ResolveModelDirectiveUri(sub.file, parser->package_map())); + ProcessModelDirectivesImpl( + sub_directives, plant, added_models, parser, new_model_namespace, + error_func); + } + } +} + +} // namespace internal + +std::string ResolveModelDirectiveUri(const std::string& uri, + const PackageMap& package_map) { + const std::string scheme_separator = "://"; + const size_t scheme_end = uri.find(scheme_separator); + if (scheme_end == std::string::npos) { + drake::log()->error("Model resource '{}' is not a valid URI.", + uri); + std::abort(); + } + + const std::string scheme = uri.substr(0, scheme_end); + const size_t package_end = uri.find("/", scheme_end + 3); + if (package_end == std::string::npos) { + drake::log()->error("Model resource '{}' has no path in package.", + uri); + std::abort(); + } + + const std::string package_name = + uri.substr(scheme_end + scheme_separator.size(), + package_end - scheme_end - scheme_separator.size()); + const std::string path_in_package = uri.substr(package_end + 1); + + DRAKE_DEMAND(scheme == "package"); // No other schemes supported for now. + if (!package_map.Contains(package_name)) { + drake::log()->error( + "Unable to resolve package '{}' for URI '{}' using package map: '{}'", + package_name, uri, package_map); + std::abort(); + } + 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, + ModelWeldErrorFunction error_func) { + auto tmp_parser = ConstructIfNullAndReassign(&parser, plant); + auto tmp_added_model = + ConstructIfNullAndReassign>(&added_models); + const std::string model_namespace = ""; + internal::ProcessModelDirectivesImpl( + directives, plant, added_models, parser, model_namespace, error_func); +} + +ModelDirectives LoadModelDirectives(const std::string& filename) { + drake::log()->debug("LoadModelDirectives: {}", 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; +} + +void FlattenModelDirectives( + const ModelDirectives& directives, + const PackageMap& package_map, + ModelDirectives* out) { + // NOTE: Does not handle scoping! + for (auto& directive : directives.directives) { + if (directive.add_directives) { + auto& sub = *directive.add_directives; + const auto sub_file = ResolveModelDirectiveUri(sub.file, package_map); + FlattenModelDirectives(LoadModelDirectives(sub_file), package_map, out); + } else { + out->directives.push_back(directive); + } + } +} + +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/dev/process_model_directives.h b/multibody/parsing/dev/process_model_directives.h new file mode 100644 index 000000000000..78aec2d2a6b4 --- /dev/null +++ b/multibody/parsing/dev/process_model_directives.h @@ -0,0 +1,130 @@ +#pragma once + +#include +#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" + +namespace drake { +namespace multibody { +namespace parsing { + +ModelDirectives LoadModelDirectives(const std::string& filename); + +/// 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. +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). + std::string model_name; + // File path. + std::string model_path; + // 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`. + 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. +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. +using ModelWeldErrorFunction = + std::function( + const drake::multibody::Frame&, + const drake::multibody::Frame&)>; + +/// 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 + +} // 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 new file mode 100644 index 000000000000..4de1b3a33206 --- /dev/null +++ b/multibody/parsing/dev/test/model_directives_test.cc @@ -0,0 +1,54 @@ +// Minimal test to make sure stuff doesn't explode. +#include "drake/multibody/parsing/dev/model_directives.h" + +#include + +#include "drake/common/yaml/yaml_read_archive.h" + +using drake::yaml::YamlReadArchive; + +namespace drake { +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: +- add_model: + name: new_model + file: base.sdf +- add_weld: + parent: parent_frame + child: child_frame +- add_frame: + name: new_frame_a + X_PF: + base_frame: world + translation: [1, 2, 3] + rotation: !Rpy { deg: [5, 6, 7] } +- add_frame: + name: new_frame_b + X_PF: + base_frame: world + translation: [1, 2, 3] + rotation: !Rpy { deg: [5, 6, 7] } +- add_directives: + file: child.yaml +- add_directives: + file: child.yaml + model_namespace: right +)"""; + ModelDirectives directives; + YamlReadArchive(YAML::Load(contents), kStrict).Accept(&directives); + EXPECT_TRUE(directives.IsValid()); +} + +} // namespace +} // namespace parsing +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/dev/test/process_model_directives_test.cc b/multibody/parsing/dev/test/process_model_directives_test.cc new file mode 100644 index 000000000000..831a47d0752f --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test.cc @@ -0,0 +1,158 @@ +#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" + +namespace drake { +namespace multibody { +namespace parsing { +namespace { + +using std::optional; +using drake::math::RigidTransformd; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Frame; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; + +const char* const kTestDir = + "drake/multibody/parsing/dev/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. +std::unique_ptr make_parser(MultibodyPlant* plant) { + auto parser = std::make_unique(plant); + const drake::filesystem::path abspath_xml = FindResourceOrThrow( + std::string(kTestDir) + "/package.xml"); + parser->package_map().AddPackageXml(abspath_xml.string()); + return parser; +} + +// Simple smoke test of the most basic model directives. +GTEST_TEST(ProcessModelDirectivesTest, BasicSmokeTest) { + ModelDirectives station_directives = LoadModelDirectives( + FindResourceOrThrow(std::string(kTestDir) + "/add_scoped_sub.yaml")); + const MultibodyPlant empty_plant(0.0); + + MultibodyPlant plant(0.0); + ProcessModelDirectives(station_directives, &plant, + nullptr, make_parser(&plant).get()); + plant.Finalize(); + + // Expect the two model instances added by the directives. + EXPECT_EQ(plant.num_model_instances() - empty_plant.num_model_instances(), 2); + + // Expect the two bodies added by the directives. + EXPECT_EQ(plant.num_bodies() - empty_plant.num_bodies(), 2); + + // A great many frames are added in model directives processing, but we + // should at least expect that our named ones are present. + EXPECT_TRUE(plant.HasFrameNamed("sub_added_frame")); + EXPECT_TRUE(plant.HasFrameNamed("sub_added_frame_explicit")); +} + +// Acceptance tests for the ModelDirectives name scoping, including acceptance +// testing its interaction with SceneGraph. +GTEST_TEST(ProcessModelDirectivesTest, AddScopedSmokeTest) { + ModelDirectives directives = LoadModelDirectives( + FindResourceOrThrow(std::string(kTestDir) + "/add_scoped_top.yaml")); + + // Ensure that we have a SceneGraph present so that we test relevant visual + // pieces. + DiagramBuilder builder; + MultibodyPlant& plant = AddMultibodyPlantSceneGraph(&builder, 0.); + ProcessModelDirectives(directives, &plant, + nullptr, make_parser(&plant).get()); + plant.Finalize(); + auto diagram = builder.Build(); + + // Query information and ensure we have expected results. + // - Manually spell out one example. + ASSERT_EQ( + &GetScopedFrameByName(plant, "left::simple_model::frame"), + &plant.GetFrameByName( + "frame", plant.GetModelInstanceByName("left::simple_model"))); + // - Automate other stuff. + auto check_frame = [&plant]( + const std::string instance, const std::string frame) { + const std::string scoped_frame = instance + "::" + frame; + drake::log()->debug("Check: {}", scoped_frame); + ASSERT_EQ( + &GetScopedFrameByName(plant, scoped_frame), + &plant.GetFrameByName(frame, plant.GetModelInstanceByName(instance))); + }; + for (const std::string prefix : {"", "left::", "right::", "mid::nested::"}) { + const std::string simple_model = prefix + "simple_model"; + check_frame(simple_model, "base"); + check_frame(simple_model, "frame"); + check_frame(simple_model, "sub_added_frame"); + check_frame(simple_model, "top_added_frame"); + const std::string extra_model = prefix + "extra_model"; + check_frame(extra_model, "base"); + check_frame(extra_model, "frame"); + } +} + +// Test the model error mechanism. +GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) { + const RigidTransformd error_transform({0.1, 0., 0.1}, {2, 3, 4}); + ModelDirectives directives = LoadModelDirectives( + FindResourceOrThrow(std::string(kTestDir) + "/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` + // 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")); + optional out; + if (&parent == &error_parent && &child == &error_child) + out = error_transform; + return out; + }; + + ProcessModelDirectives(directives, &plant, + nullptr, make_parser(&plant).get(), error); + plant.Finalize(); + + // This should have created an error frame for the relevant weld. + const std::string expected_error_frame_name = "frame_weld_error_to_base"; + EXPECT_TRUE(plant.HasFrameNamed(expected_error_frame_name)); + const auto& frame = plant.GetFrameByName(expected_error_frame_name); + EXPECT_TRUE( + dynamic_cast*>(&frame)); + const RigidTransformd expected_error = + (plant + .GetFrameByName("frame", plant.GetModelInstanceByName("simple_model")) + .GetFixedPoseInBodyFrame()) + * error_transform; + + EXPECT_TRUE( + frame.GetFixedPoseInBodyFrame().IsExactlyEqualTo(expected_error)); + + // This should not have created an error frame for other welds. + for (drake::multibody::FrameIndex frame_id(0); + frame_id < plant.num_frames(); + frame_id++) { + const std::string frame_name = plant.get_frame(frame_id).name(); + if (frame_name != expected_error_frame_name) { + EXPECT_TRUE(frame_name.find("error") == std::string::npos); + } + } +} + +} // namespace +} // namespace parsing +} // namespace multibody +} // namespace drake diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_backreference.yaml b/multibody/parsing/dev/test/process_model_directives_test/add_backreference.yaml new file mode 100644 index 000000000000..6a2de4ff06c8 --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/add_backreference.yaml @@ -0,0 +1,10 @@ +directives: + +# Add a weld that references a frame in another model directives file +# TODO(ggould-tri) this violates the scoping principle discussed in +# https://github.com/RobotLocomotion/drake/issues/13282#issuecomment-627687411 +# and is arguably a bug, but as long as it is current behaviour we test it. +- add_weld: + parent: simple_model_origin + # N.B. Implicitly added to current model instance. + child: simple_model::base diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_mid.yaml b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_mid.yaml new file mode 100644 index 000000000000..e75456c5f702 --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_mid.yaml @@ -0,0 +1,22 @@ +# The trio of `add_scoped_*.yaml` files are a test yaml structure that uses +# each feature of the model directives system. This file is the middle level +# of a three-file inclusion hierarcy of model directives, and is also used as +# a smoke test of the inclusion mechanism. + +directives: +- add_model_instance: + name: nested +- add_frame: + name: nested::simple_model_origin + X_PF: + base_frame: world + translation: [10, 0, 0] +- add_directives: + 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://process_model_directives_test/add_backreference.yaml + model_namespace: nested diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_sub.yaml b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_sub.yaml new file mode 100644 index 000000000000..d62a394045fb --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_sub.yaml @@ -0,0 +1,32 @@ +# The trio of `add_scoped_*.yaml` files are a test yaml structure that uses +# each feature of the model directives system. This file has only simple +# directives with no inclusion and is also used as a smoke test of the basic +# directives. + +directives: +- add_model: + # This name will be prefixed by `model_namespace`. + name: simple_model + file: package://process_model_directives_test/simple_model.sdf + +- add_frame: + # This will implicitly resolve to the `simple_model` instance. + name: sub_added_frame + X_PF: + base_frame: simple_model::frame + translation: [10, 20, 30] + +- add_frame: + # This will explicitly resolve to the `simple_model` instance. + name: simple_model::sub_added_frame_explicit + X_PF: + base_frame: simple_model::frame + translation: [10, 20, 30] + +- add_model: + name: extra_model + file: package://process_model_directives_test/simple_model.sdf + +- add_weld: + parent: simple_model::frame + child: extra_model::base diff --git a/multibody/parsing/dev/test/process_model_directives_test/add_scoped_top.yaml b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_top.yaml new file mode 100644 index 000000000000..e81f60b19415 --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/add_scoped_top.yaml @@ -0,0 +1,70 @@ +# The trio of `add_scoped_*.yaml` files are a test yaml structure that uses +# each feature of the model directives system. This file is the top-level of +# a three-file inclusion hierarchy of model directives. + +directives: + +# N.B. The namespacing features are only available for RBT. + +# No namespace. +# N.B. If this were added after the namespaced instances, `simple_model_origin` +# would cause a failure because it appears in multiple model instances. +- add_frame: + name: simple_model_origin + X_PF: + base_frame: world +- add_directives: + file: package://process_model_directives_test/add_scoped_sub.yaml +- add_frame: + name: simple_model::top_added_frame + X_PF: + base_frame: simple_model::sub_added_frame + translation: [5, 10, 15] + +# Left. +- add_model_instance: + # `model_namespace` requires a corresponding model instance so that frames + # can be added to it. + name: left +- add_frame: + name: left::simple_model_origin + X_PF: + base_frame: world + translation: [0, 1, 0] +- add_directives: + file: package://process_model_directives_test/add_scoped_sub.yaml + model_namespace: left +- add_frame: + name: left::simple_model::top_added_frame + X_PF: + base_frame: left::simple_model::sub_added_frame + translation: [5, 10, 15] + +# Right. +- add_model_instance: + name: right +- add_frame: + name: right::simple_model_origin + X_PF: + base_frame: world + translation: [0, -1, 0] +- add_directives: + file: package://process_model_directives_test/add_scoped_sub.yaml + model_namespace: right +- add_frame: + name: right::simple_model::top_added_frame + X_PF: + base_frame: right::simple_model::sub_added_frame + translation: [5, 10, 15] + +# Mid (nested). +- add_model_instance: + name: mid +- add_directives: + file: package://process_model_directives_test/add_scoped_mid.yaml + model_namespace: mid +- add_frame: + name: mid::nested::simple_model::top_added_frame + X_PF: + base_frame: mid::nested::simple_model::sub_added_frame + translation: [5, 10, 15] diff --git a/multibody/parsing/dev/test/process_model_directives_test/circle.png b/multibody/parsing/dev/test/process_model_directives_test/circle.png new file mode 100644 index 000000000000..5808248b3142 Binary files /dev/null and b/multibody/parsing/dev/test/process_model_directives_test/circle.png differ diff --git a/multibody/parsing/dev/test/process_model_directives_test/package.xml b/multibody/parsing/dev/test/process_model_directives_test/package.xml new file mode 100644 index 000000000000..61c989b9ab4f --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/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/process_model_directives_test/simple_model.sdf b/multibody/parsing/dev/test/process_model_directives_test/simple_model.sdf new file mode 100644 index 000000000000..90e709ee4775 --- /dev/null +++ b/multibody/parsing/dev/test/process_model_directives_test/simple_model.sdf @@ -0,0 +1,29 @@ + + + + + + 1 + + + + + 0.2 0.2 2.2 + + + + + + package://process_model_directives_test/circle.png + + + + + + 1 2 3 0 0 0 + + +