Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[mujoco parser] Fix debug throw from RotationalInertia #22401

Merged
merged 1 commit into from
Jan 7, 2025
Merged

[mujoco parser] Fix debug throw from RotationalInertia #22401

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 10 additions & 2 deletions multibody/parsing/detail_mujoco_parser.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -565,7 +565,7 @@ class MujocoParser {
if (mesh_inertia_->contains(name_)) {
M_GGo_G_unitDensity = mesh_inertia_->at(name_);
} else {
const CalcSpatialInertiaResult result =
CalcSpatialInertiaResult result =
CalcSpatialInertiaImpl(mesh, 1.0 /* density */);
if (std::holds_alternative<std::string>(result)) {
policy_.Warning(std::get<std::string>(result));
Expand All @@ -575,9 +575,17 @@ class MujocoParser {
DRAKE_DEMAND(mesh.source().is_path());
// As with mujoco, failure leads to using the convex hull.
// https://github.com/google-deepmind/mujoco/blob/df7ea3ed3350164d0f111c12870e46bc59439a96/src/user/user_mesh.cc#L1379-L1382
M_GGo_G_unitDensity = CalcSpatialInertia(
result = CalcSpatialInertiaImpl(
geometry::Convex(mesh.source().path(), mesh.scale()),
1.0 /* density */);
if (std::holds_alternative<std::string>(result)) {
policy_.Error(fmt::format(
"Failed to compute spatial inertia even for the convex hull of "
"{}.\n{}",
mesh.source().path().string(), std::get<std::string>(result)));
} else {
M_GGo_G_unitDensity = std::get<SpatialInertia<double>>(result);
}
used_convex_hull_fallback_ = true;
} else {
M_GGo_G_unitDensity = std::get<SpatialInertia<double>>(result);
Expand Down
35 changes: 13 additions & 22 deletions multibody/parsing/test/detail_mujoco_parser_examples_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ INSTANTIATE_TEST_SUITE_P(DeepMindControl, DeepMindControlTest,
}));

constexpr std::string_view kItWorks{""};
constexpr std::string_view kSkipMe{"skip me"};
constexpr std::string_view kTooSlow{"skip me"}; // #22412
namespace KnownErrors {
constexpr std::string_view kNonUniformScale{".*non-uniform scale.*"}; // #22046
constexpr std::string_view kSizeFromMesh =
Expand All @@ -107,7 +107,7 @@ TEST_P(MujocoMenagerieTest, MujocoMenagerie) {
// Confirm successful parsing of the MuJoCo models in the DeepMind control
// suite.
auto [model, error_regex] = GetParam();
if (error_regex == kSkipMe) {
if (error_regex == kTooSlow) {
GTEST_SKIP_("Skipping this test case.");
}
const RlocationOrError rlocation = FindRunfile(
Expand Down Expand Up @@ -150,21 +150,17 @@ const std::pair<const char*, std::string_view> mujoco_menagerie_models[] = {
{"boston_dynamics_spot/scene_arm", kItWorks},
{"boston_dynamics_spot/spot", kItWorks},
{"boston_dynamics_spot/spot_arm", kItWorks},
{"flybody/fruitfly",
kSkipMe}, // kSizeFromMesh, but too slow in debug mode.
{"flybody/scene", kSkipMe}, // kSizeFromMesh, but too slow in debug mode.
{"flybody/fruitfly", kTooSlow}, // kSizeFromMesh
{"flybody/scene", kTooSlow}, // kSizeFromMesh
{"franka_emika_panda/hand", kItWorks},
{"franka_emika_panda/mjx_panda", kItWorks},
{"franka_emika_panda/mjx_scene", kItWorks},
{"franka_emika_panda/mjx_single_cube", kItWorks},
{"franka_emika_panda/panda",
kSkipMe}, // works, but too slow in debug mode.
{"franka_emika_panda/panda_nohand",
kSkipMe}, // works, but too slow in debug mode.
{"franka_emika_panda/scene",
kSkipMe}, // works, but too slow in debug mode.
{"franka_fr3/fr3", kSkipMe}, // works, but too slow in debug mode.
{"franka_fr3/scene", kSkipMe}, // works, but too slow in debug mode.
{"franka_emika_panda/panda", kTooSlow}, // kItWorks
{"franka_emika_panda/panda_nohand", kTooSlow}, // kItWorks
{"franka_emika_panda/scene", kTooSlow}, // kItWorks
{"franka_fr3/fr3", kTooSlow}, // kItWorks
{"franka_fr3/scene", kTooSlow}, // kItWorks
{"google_barkour_v0/barkour_v0", kItWorks},
{"google_barkour_v0/barkour_v0_mjx", kItWorks},
{"google_barkour_v0/scene", kItWorks},
Expand All @@ -177,15 +173,10 @@ const std::pair<const char*, std::string_view> mujoco_menagerie_models[] = {
{"google_barkour_vb/scene_mjx", kItWorks},
{"google_robot/robot", kItWorks},
{"google_robot/scene", kItWorks},
/* The hello_robot_stretch and hello_robot_stretch_3 models currently throw
in RotationalInertia<T>::ThrowNotPhysicallyValid(), but only in Debug
mode. This is possibly due to the fact that the stl geometries are not
being parsed, so the proper inertias are not being computed. They _also_
fail with KnownErrors::kNonUniformScale in release mode. */
{"hello_robot_stretch/scene", kSkipMe},
{"hello_robot_stretch/stretch", kSkipMe},
{"hello_robot_stretch_3/scene", kSkipMe},
{"hello_robot_stretch_3/stretch", kSkipMe},
{"hello_robot_stretch/scene", kTooSlow}, // kNonUniformScale,
{"hello_robot_stretch/stretch", kTooSlow}, // kNonUniformScale,
{"hello_robot_stretch_3/scene", kTooSlow}, // kNonUniformScale,
{"hello_robot_stretch_3/stretch", kTooSlow}, // kNonUniformScale,
{"kinova_gen3/gen3", kItWorks},
{"kinova_gen3/scene", kItWorks},
{"kuka_iiwa_14/iiwa14", kItWorks},
Expand Down
2 changes: 1 addition & 1 deletion multibody/parsing/test/detail_mujoco_parser_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -744,7 +744,7 @@ TEST_F(MujocoParserTest, BadMeshSpatialInertiaFallback) {
// This obj is known to produce a non-physical spatial inertia in
// CalcSpatialInertia().
const RlocationOrError rlocation = FindRunfile(
"mujoco_menagerie_internal/hello_robot_stretch/assets/base_link_0.obj");
"mujoco_menagerie_internal/hello_robot_stretch/assets/link_head_1.obj");
ASSERT_EQ(rlocation.error, "");
const geometry::Mesh bad_mesh(rlocation.abspath, 1.0);

Expand Down
16 changes: 8 additions & 8 deletions multibody/tree/geometry_spatial_inertia.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,17 +186,17 @@ CalcSpatialInertiaResult CalcSpatialInertiaImpl(
// and go get the six terms we actually care about.
C /= volume;
const double trace_C = C.trace();
const UnitInertia G_GGo_G(trace_C - C(0, 0), trace_C - C(1, 1),
trace_C - C(2, 2), -C(1, 0), -C(2, 0), -C(2, 1));
const UnitInertia G_GGo_G(
RotationalInertia<double>::MakeFromMomentsAndProductsOfInertia(
trace_C - C(0, 0), trace_C - C(1, 1), trace_C - C(2, 2), -C(1, 0),
-C(2, 0), -C(2, 1),
/* skip_validity_check = */ true));

auto result = SpatialInertia<double>{mass, p_GoGcm, G_GGo_G,
/* skip_validity_check = */ true};
if (!result.IsPhysicallyValid()) {
std::string message = result.CriticizeNotPhysicallyValid();
if (!message.empty()) {
return message;
}
return result;
std::string message = result.CriticizeNotPhysicallyValid();
if (!message.empty()) {
return message;
}
return result;
}
Expand Down