Skip to content

Commit

Permalink
fixup! Model Directives mechanism for scene assembly.
Browse files Browse the repository at this point in the history
  • Loading branch information
ggould-tri committed Aug 18, 2020
1 parent 0af7888 commit 3435c08
Show file tree
Hide file tree
Showing 15 changed files with 35 additions and 56 deletions.
18 changes: 9 additions & 9 deletions multibody/parsing/dev/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,15 @@ load(
"@drake//tools/skylark:drake_cc.bzl",
"drake_cc_googletest",
"drake_cc_library",
"drake_cc_package_library",
)
load("//tools/lint:lint.bzl", "add_lint_tests")
load("//tools/install:install_data.bzl", "install_data")

drake_cc_library(
name = "process_model_directives",
srcs = ["process_model_directives.cc"],
hdrs = ["process_model_directives.h"],
data = [
"@drake//manipulation/models/jaco_description:models",
"//manipulation/models/jaco_description:models",
],
deps = [
":model_directives",
Expand All @@ -26,10 +24,16 @@ drake_cc_library(
],
)

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 = [
":test_models",
":process_model_directives_test_models",
],
deps = [
":process_model_directives",
Expand All @@ -45,7 +49,7 @@ drake_cc_library(
"//common:essential",
"//common:name_value",
"//common/schema/dev:transform",
"//math",
"//math:geometric_transform",
],
)

Expand All @@ -57,8 +61,4 @@ drake_cc_googletest(
],
)

install_data(
extra_tags = ["nolint"], # `dev` content is never actually installed.
)

add_lint_tests()
3 changes: 2 additions & 1 deletion multibody/parsing/dev/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@ Within this group are a series of directives:
* `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`.
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
Expand Down
4 changes: 0 additions & 4 deletions multibody/parsing/dev/model_directives.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,6 @@
namespace drake {
namespace multibody {
namespace parsing {
namespace dev {
namespace schema {

struct AddWeld {
bool IsValid() const {
Expand Down Expand Up @@ -236,8 +234,6 @@ inline void AddPackageToModelDirectives(const std::string& package_name,
directives->directives.begin(), directive);
}

} // namespace schema
} // namespace dev
} // namespace parsing
} // namespace multibody
} // namespace drake
19 changes: 7 additions & 12 deletions multibody/parsing/dev/process_model_directives.cc
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
namespace drake {
namespace multibody {
namespace parsing {
namespace dev {

using std::make_unique;
using Eigen::Isometry3d;
Expand All @@ -30,8 +29,6 @@ using drake::multibody::PackageMap;
using drake::multibody::Parser;
using drake::yaml::YamlReadArchive;

using schema::ModelDirectives;

namespace {

// If `*ptr` is null, construct `T(args...)` and reassign the pointer.
Expand Down Expand Up @@ -334,9 +331,9 @@ ModelDirectives LoadModelDirectives(const std::string& filename) {
}

void FlattenModelDirectives(
const schema::ModelDirectives& directives,
const ModelDirectives& directives,
const PackageMap& package_map,
schema::ModelDirectives* out) {
ModelDirectives* out) {
// NOTE: Does not handle scoping!
for (auto& directive : directives.directives) {
if (directive.add_directives) {
Expand Down Expand Up @@ -365,9 +362,9 @@ ModelInfo MakeModelInfo(const std::string& model_name,

// TODO(eric.cousineau): Do we *really* need this function? This seems like
// it'd be better handled as an MBP subgraph.
schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
ModelDirectives MakeModelsAttachedToFrameDirectives(
const std::vector<ModelInfo>& models_to_add) {
schema::ModelDirectives directives;
ModelDirectives directives;

// One for add frame, one for add model, one for add weld.
directives.directives.resize(models_to_add.size() * 3);
Expand All @@ -379,7 +376,7 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(

// Add frame first.
if (!model_to_add.X_PC.IsExactlyIdentity()) {
schema::AddFrame frame_dir;
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;
Expand All @@ -392,13 +389,13 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
}

// Add model
schema::AddModel model_dir;
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;

schema::AddWeld weld_dir;
AddWeld weld_dir;
weld_dir.parent = attachment_frame_name;
weld_dir.child =
model_to_add.model_name + "::" + model_to_add.child_frame_name;
Expand All @@ -409,8 +406,6 @@ schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
return directives;
}


} // namespace dev
} // namespace parsing
} // namespace multibody
} // namespace drake
15 changes: 5 additions & 10 deletions multibody/parsing/dev/process_model_directives.h
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
#pragma once

/// Processes stuff from `common/schema/model_directives.h`. See
// `common/schema/README.md` for more info.

#include <optional>
#include <string>
#include <vector>
Expand All @@ -16,9 +13,8 @@
namespace drake {
namespace multibody {
namespace parsing {
namespace dev {

schema::ModelDirectives LoadModelDirectives(const std::string& filename);
ModelDirectives LoadModelDirectives(const std::string& filename);

/// ModelDirectives refer to their resources by URIs like
/// `package://somepackage/somepath/somefile.sdf`, where somepackage refers to
Expand Down Expand Up @@ -59,13 +55,13 @@ ModelInfo MakeModelInfo(
* `child_frame_name` to `parent_frame_name` directly. The `model_instance`
* field is ignored for this purposes.
*/
schema::ModelDirectives MakeModelsAttachedToFrameDirectives(
ModelDirectives MakeModelsAttachedToFrameDirectives(
const std::vector<ModelInfo>& models_to_add);

// Flatten model directives.
void FlattenModelDirectives(const schema::ModelDirectives& directives,
void FlattenModelDirectives(const ModelDirectives& directives,
const drake::multibody::PackageMap& package_map,
schema::ModelDirectives* out);
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
Expand All @@ -81,7 +77,7 @@ using ModelWeldErrorFunction =
/// 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 schema::ModelDirectives& directives,
void ProcessModelDirectives(const ModelDirectives& directives,
drake::multibody::MultibodyPlant<double>* plant,
std::vector<ModelInfo>* added_models = nullptr,
drake::multibody::Parser* parser = nullptr,
Expand Down Expand Up @@ -129,7 +125,6 @@ ScopedName ParseScopedName(const std::string& full_name);

} // namespace internal

} // namespace dev
} // namespace parsing
} // namespace multibody
} // namespace drake
4 changes: 0 additions & 4 deletions multibody/parsing/dev/test/model_directives_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,6 @@ using drake::yaml::YamlReadArchive;
namespace drake {
namespace multibody {
namespace parsing {
namespace dev {
namespace schema {
namespace {

// TODO(jeremy.nimmer) We can remove this argument from the call sites below
Expand Down Expand Up @@ -51,8 +49,6 @@ GTEST_TEST(ModelDirectivesTest, Success) {
}

} // namespace
} // namespace schema
} // namespace dev
} // namespace parsing
} // namespace multibody
} // namespace drake
17 changes: 7 additions & 10 deletions multibody/parsing/dev/test/process_model_directives_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
namespace drake {
namespace multibody {
namespace parsing {
namespace dev {
namespace {

using std::optional;
Expand All @@ -22,23 +21,24 @@ using drake::multibody::Frame;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using drake::systems::DiagramBuilder;
using schema::ModelDirectives;

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<Parser> make_parser(MultibodyPlant<double>* plant) {
auto parser = std::make_unique<Parser>(plant);
const drake::filesystem::path abspath_xml = FindResourceOrThrow(
"drake/multibody/parsing/dev/test/models/package.xml");
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(
"drake/multibody/parsing/dev/test/models/add_scoped_sub.yaml"));
FindResourceOrThrow(std::string(kTestDir) + "/add_scoped_sub.yaml"));
const MultibodyPlant<double> empty_plant(0.0);

MultibodyPlant<double> plant(0.0);
Expand All @@ -62,8 +62,7 @@ GTEST_TEST(ProcessModelDirectivesTest, BasicSmokeTest) {
// testing its interaction with SceneGraph.
GTEST_TEST(ProcessModelDirectivesTest, AddScopedSmokeTest) {
ModelDirectives directives = LoadModelDirectives(
FindResourceOrThrow(
"drake/multibody/parsing/dev/test/models/add_scoped_top.yaml"));
FindResourceOrThrow(std::string(kTestDir) + "/add_scoped_top.yaml"));

// Ensure that we have a SceneGraph present so that we test relevant visual
// pieces.
Expand Down Expand Up @@ -105,8 +104,7 @@ GTEST_TEST(ProcessModelDirectivesTest, AddScopedSmokeTest) {
GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) {
const RigidTransformd error_transform({0.1, 0., 0.1}, {2, 3, 4});
ModelDirectives directives = LoadModelDirectives(
FindResourceOrThrow(
"drake/multibody/parsing/dev/test/models/add_scoped_sub.yaml"));
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`
Expand Down Expand Up @@ -155,7 +153,6 @@ GTEST_TEST(ProcessModelDirectivesTest, SmokeTestInjectWeldError) {
}

} // namespace
} // namespace dev
} // namespace parsing
} // namespace multibody
} // namespace drake
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,8 @@
</geometry>
<material>
<!--
Test usage of diffuse map from a non-neighboring Anzu resource.
Test usage of diffuse map from a separate resource that requires
package:// URI resolution.
-->
<drake:diffuse_map>
package://process_model_directives_test/circle.png
Expand Down
8 changes: 3 additions & 5 deletions tools/install/install_data.bzl
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,7 @@ def install_data(
name = "models",
prod_models_prefix = "prod_",
test_models_prefix = "test_",
extra_prod_models = [],
extra_tags = []):
extra_prod_models = []):
"""Install data
This macro creates 3 filegroups:
Expand Down Expand Up @@ -45,9 +44,8 @@ def install_data(
"vtm",
"vtp",
"xml",
"yaml",
]
exclude_patterns = ["**/test/*", "**/test/models/*", "**/test*"]
exclude_patterns = ["**/test/*", "**/test*"]
prod_models_include = ["**/*.{}".format(x) for x in models_extensions]
test_models_include = [
p + m
Expand Down Expand Up @@ -83,6 +81,6 @@ def install_data(
name = "install_data",
data = [prod_models_target],
data_dest = "share/drake/" + native.package_name(),
tags = ["install"] + extra_tags,
tags = ["install"],
visibility = ["//visibility:public"],
)

0 comments on commit 3435c08

Please sign in to comment.