Skip to content

Commit

Permalink
Add mujoco_menagerie as a test-only dependency
Browse files Browse the repository at this point in the history
And use it for testing the mujoco parser.
Towards #20444 and #21648.
  • Loading branch information
RussTedrake committed Jun 30, 2024
1 parent 27d462a commit 36492c2
Show file tree
Hide file tree
Showing 8 changed files with 173 additions and 8 deletions.
18 changes: 17 additions & 1 deletion multibody/parsing/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,10 @@ load(
"//tools/workspace/dm_control_internal:files.bzl",
"dm_control_mujoco_files",
)
load(
"//tools/workspace/mujoco_menagerie_internal:files.bzl",
"mujoco_menagerie_files",
)

filegroup(
name = "test_models",
Expand Down Expand Up @@ -44,6 +48,18 @@ _DM_CONTROL_MUJOCO_FILES = forward_files(
visibility = ["//visibility:private"],
)

_MUJOCO_MENAGERIE_FILES = forward_files(
srcs = [
"@mujoco_menagerie_internal//:" +
x
for x in mujoco_menagerie_files()
],
dest_prefix = "mujoco_menagerie/",
strip_prefix = "@mujoco_menagerie_internal//:",
tags = ["manual"],
visibility = ["//visibility:private"],
)

drake_cc_package_library(
name = "parsing",
visibility = ["//visibility:public"],
Expand Down Expand Up @@ -652,7 +668,7 @@ drake_cc_googletest(
":test_models",
"//geometry:test_obj_files",
"//geometry:test_stl_files",
] + _DM_CONTROL_MUJOCO_FILES,
] + _DM_CONTROL_MUJOCO_FILES + _MUJOCO_MENAGERIE_FILES,
deps = [
":detail_mujoco_parser",
"//common:find_resource",
Expand Down
37 changes: 31 additions & 6 deletions multibody/parsing/test/detail_mujoco_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -117,10 +117,10 @@ class MujocoParserTest : public test::DiagnosticPolicyTestBase {
"drake/multibody/parsing/test/box_package/urdfs/box.urdf"))};
};

class GymModelTest : public MujocoParserTest,
public testing::WithParamInterface<const char*> {};
class DeepMindControlTest : public MujocoParserTest,
public testing::WithParamInterface<const char*> {};

TEST_P(GymModelTest, GymModel) {
TEST_P(DeepMindControlTest, DeepMindControl) {
// Confirm successful parsing of the MuJoCo models in the DeepMind control
// suite.
std::string model{GetParam()};
Expand All @@ -134,13 +134,38 @@ TEST_P(GymModelTest, GymModel) {
warning_records_.clear();
}

const char* gym_models[] = {
const char* dm_control_models[] = {
"acrobot", "cartpole", "cheetah", "finger", "fish",
"hopper", "humanoid", "humanoid_CMU", "lqr", "manipulator",
"pendulum", "point_mass", "quadruped", "reacher", "stacker",
"swimmer", "walker"};
INSTANTIATE_TEST_SUITE_P(GymModels, GymModelTest,
testing::ValuesIn(gym_models));
INSTANTIATE_TEST_SUITE_P(DeepMindControl, DeepMindControlTest,
testing::ValuesIn(dm_control_models));

class MujocoMenagerieTest : public MujocoParserTest,
public testing::WithParamInterface<const char*> {};

TEST_P(MujocoMenagerieTest, MujocoMenagerie) {
// Confirm successful parsing of the MuJoCo models in the DeepMind control
// suite.
std::string model{GetParam()};
const std::string filename = FindResourceOrThrow(
fmt::format("drake/multibody/parsing/mujoco_menagerie/{}.xml", model));
AddModelFromFile(filename, model);

EXPECT_TRUE(plant_.HasModelInstanceNamed(model));

// For this test, ignore all warnings.
warning_records_.clear();
}

const char* mujoco_menagerie_models[] = {"google_robot/robot",
"kuka_iiwa_14/iiwa14"};
// TODO(russt): Add the remaining models, once they can be parsed correctly, as
// tracked in #20444.

INSTANTIATE_TEST_SUITE_P(MujocoMenagerie, MujocoMenagerieTest,
testing::ValuesIn(mujoco_menagerie_models));

// In addition to confirming that the parser can successfully parse the model,
// this test can be used to manually inspect the resulting visualization.
Expand Down
6 changes: 5 additions & 1 deletion multibody/tree/geometry_spatial_inertia.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ namespace multibody {
/** Computes the SpatialInertia of a body made up of a homogeneous material
(of given `density` in kg/m³) uniformly distributed in the volume of the given
`shape`.
The `shape` is defined in its canonical frame S and the body in frame B. The
two frames are coincident and aligned (i.e., X_SB = I).
Expand All @@ -31,6 +31,10 @@ namespace multibody {
given `shape`.
@throws std::exception if `shape` is an instance of geometry::HalfSpace or
geometry::MeshcatCone.
@throws std::exception if the resulting spatial inertia computation does not
result in a physically meaningful value. See
SpatialInertia::IsPhysicallyValid() for more
information.
@pydrake_mkdoc_identifier{shape} */
SpatialInertia<double> CalcSpatialInertia(const geometry::Shape& shape,
double density);
Expand Down
3 changes: 3 additions & 0 deletions tools/workspace/default.bzl
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ load("//tools/workspace/meshcat:repository.bzl", "meshcat_repository")
load("//tools/workspace/mosek:repository.bzl", "mosek_repository")
load("//tools/workspace/mpmath_py_internal:repository.bzl", "mpmath_py_internal_repository") # noqa
load("//tools/workspace/msgpack_internal:repository.bzl", "msgpack_internal_repository") # noqa
load("//tools/workspace/mujoco_menagerie_internal:repository.bzl", "mujoco_menagerie_internal_repository") # noqa
load("//tools/workspace/mumps_internal:repository.bzl", "mumps_internal_repository") # noqa
load("//tools/workspace/mypy_extensions_internal:repository.bzl", "mypy_extensions_internal_repository") # noqa
load("//tools/workspace/mypy_internal:repository.bzl", "mypy_internal_repository") # noqa
Expand Down Expand Up @@ -219,6 +220,8 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS):
mpmath_py_internal_repository(name = "mpmath_py_internal", mirrors = mirrors) # noqa
if "msgpack_internal" not in excludes:
msgpack_internal_repository(name = "msgpack_internal", mirrors = mirrors) # noqa
if "mujoco_menagerie_internal" not in excludes:
mujoco_menagerie_internal_repository(name = "mujoco_menagerie_internal", mirrors = mirrors) # noqa
if "mumps_internal" not in excludes:
mumps_internal_repository(name = "mumps_internal")
if "mypy_extensions_internal" not in excludes:
Expand Down
3 changes: 3 additions & 0 deletions tools/workspace/mujoco_menagerie_internal/BUILD.bazel
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
load("//tools/lint:lint.bzl", "add_lint_tests")

add_lint_tests()
74 changes: 74 additions & 0 deletions tools/workspace/mujoco_menagerie_internal/files.bzl
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
# Keep the macros sorted alphabetically by macro name.
# Keep the lists of files sorted alphabetically by filename.

def mujoco_menagerie_files():
return [
"google_robot/robot.xml",
"google_robot/LICENSE",
"google_robot/assets/finger_base_texture.png",
"google_robot/assets/finger_tip_texture.png",
"google_robot/assets/link_base_0_00.stl",
"google_robot/assets/link_base_0_01.stl",
"google_robot/assets/link_base_1_00.stl",
"google_robot/assets/link_base_1_01.stl",
"google_robot/assets/link_base_1_02.stl",
"google_robot/assets/link_base_1_03.stl",
"google_robot/assets/link_base_1_04.stl",
"google_robot/assets/link_base_1_05.stl",
"google_robot/assets/link_base_1_06.stl",
"google_robot/assets/link_base_1_07.stl",
"google_robot/assets/link_base_1_08.stl",
"google_robot/assets/link_base_1_09.stl",
"google_robot/assets/link_base_1_10.stl",
"google_robot/assets/link_base_1_11.stl",
"google_robot/assets/link_base_1_12.stl",
"google_robot/assets/link_base_1_13.stl",
"google_robot/assets/link_base_1_14.stl",
"google_robot/assets/link_base_1_15.stl",
"google_robot/assets/link_base_1_16.stl",
"google_robot/assets/link_base_1_17.stl",
"google_robot/assets/link_base_1_18.stl",
"google_robot/assets/link_base_1_19.stl",
"google_robot/assets/link_base_v.obj",
"google_robot/assets/link_bicep.stl",
"google_robot/assets/link_bicep_v.obj",
"google_robot/assets/link_elbow.stl",
"google_robot/assets/link_elbow_v.obj",
"google_robot/assets/link_finger_base.stl",
"google_robot/assets/link_finger_base_v.obj",
"google_robot/assets/link_finger_tip.stl",
"google_robot/assets/link_finger_tip_v.obj",
"google_robot/assets/link_forearm.stl",
"google_robot/assets/link_forearm_v.obj",
"google_robot/assets/link_gripper.stl",
"google_robot/assets/link_gripper_v.obj",
"google_robot/assets/link_head_pan.stl",
"google_robot/assets/link_head_pan_v.obj",
"google_robot/assets/link_head_tilt.stl",
"google_robot/assets/link_head_tilt_v.obj",
"google_robot/assets/link_shoulder.stl",
"google_robot/assets/link_shoulder_v.obj",
"google_robot/assets/link_torso_00.stl",
"google_robot/assets/link_torso_01.stl",
"google_robot/assets/link_torso_v.obj",
"google_robot/assets/link_wheel_v.obj",
"google_robot/assets/link_wrist.stl",
"google_robot/assets/link_wrist_v.obj",
"google_robot/assets/robot_texture.png",
] + [
"kuka_iiwa_14/iiwa14.xml",
"kuka_iiwa_14/LICENSE",
"kuka_iiwa_14/assets/band.obj",
"kuka_iiwa_14/assets/kuka.obj",
"kuka_iiwa_14/assets/link_0.obj",
"kuka_iiwa_14/assets/link_1.obj",
"kuka_iiwa_14/assets/link_2_grey.obj",
"kuka_iiwa_14/assets/link_2_orange.obj",
"kuka_iiwa_14/assets/link_3.obj",
"kuka_iiwa_14/assets/link_4_grey.obj",
"kuka_iiwa_14/assets/link_4_orange.obj",
"kuka_iiwa_14/assets/link_5.obj",
"kuka_iiwa_14/assets/link_6_grey.obj",
"kuka_iiwa_14/assets/link_6_orange.obj",
"kuka_iiwa_14/assets/link_7.obj",
]
27 changes: 27 additions & 0 deletions tools/workspace/mujoco_menagerie_internal/package.BUILD.bazel
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
# -*- bazel -*-

package(default_visibility = ["//visibility:public"])

# We maintain a different export_files for each subdirectory of the repository,
# and only once we have vetted the license (in the main LICENSE file).

# google_robot
exports_files(
srcs = [
"google_robot/robot.xml",
"google_robot/LICENSE",
] + glob([
"google_robot/assets/*.obj",
"google_robot/assets/*.png",
"google_robot/assets/*.stl",
]),
)

# kuka_iiwa_14
exports_files(
srcs = [
"kuka_iiwa_14/iiwa14.xml",
"kuka_iiwa_14/LICENSE",
] + glob(["kuka_iiwa_14/assets/*.obj"]),
)

13 changes: 13 additions & 0 deletions tools/workspace/mujoco_menagerie_internal/repository.bzl
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
load("//tools/workspace:github.bzl", "github_archive")

def mujoco_menagerie_internal_repository(
name,
mirrors = None):
github_archive(
name = name,
repository = "google-deepmind/mujoco_menagerie",
commit = "af493511dbdfce2e046858a4d2f2955e063e17fd",
sha256 = "2975d9a03728bf1b8a2b7d2b14e62bc8d411031b73264e797dcce649bbf4811c", # noqa
build_file = ":package.BUILD.bazel",
mirrors = mirrors,
)

0 comments on commit 36492c2

Please sign in to comment.