RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.17k stars 1.24k forks source link

Mujoco parser fails during CalcSpatialInertia, even when that inertia calculation is not needed. #21648

Open RussTedrake opened 4 days ago

RussTedrake commented 4 days ago

What happened?

Looking more into the IsPhysicallyValid() errors reported in #20444, I think I've found the culprit.

PR #19342 (by @rpoyner-tri and @SeanCurtis-TRI) introduced a change in detail_mujoco_parser.cc (now on this line), which I believe is too brittle.

Here is a minimal reproduction:

from pydrake.all import ModelVisualizer, PackageMap

visualizer = ModelVisualizer()
package_map = visualizer.parser().package_map()
package_map.AddRemote(
    package_name="mujoco_menagerie",
    params=PackageMap.RemoteParams(
        # This repository doesn't have up-to-date tags/releases; the scary
        # hash in the url is the most recent commit sha at the time of my
        # writing.
        urls=[
            f"https://github.com/google-deepmind/mujoco_menagerie/archive/8a5f659ac3607dc5adb988e0187f683fe0f4edf4.tar.gz"
        ],
        sha256=("64f63891098287c7dde8a063f5381512c0a4bb7a0f1de9786ff6d3ad7f411237"),
        strip_prefix="mujoco_menagerie-8a5f659ac3607dc5adb988e0187f683fe0f4edf4/",
    ),
)
visualizer.AddModels(url="package://mujoco_menagerie/universal_robots_ur5e/ur5e.xml")
visualizer.Run(loop_once=True)

This fails by throwing

RuntimeError: Spatial inertia fails SpatialInertia::IsPhysicallyValid().
 mass = 0.0007129490100185651
 Center of mass = [-5.975685083965463e-06  -0.00027408526995055256  0.06724730417771076]
 Inertia about point P, I_BP =
...

(notably, it does not actually say which body was at fault, which makes it frustrating to debug).

It turns out that the ur5e.xml specifies inertias for all of its bodies, so no inertia needs to be calculated from the geometry. But we still calculate the inertia from the mesh, and fail when doing so, and throw an undebuggable error message.

I believe this is also the culprit for at least some number of the other models listed as failing with invalid inertias in #20444.

Ideally, we would defer calculation of the inertia until if/when it is needed. And ideally our CalcSpatialInertia method would not (ever?) fail the IsPhysicallyValid() test.

cc @agarwal-abhinav

Version

No response

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

compiled from source code using Bazel

Relevant log output

No response

RussTedrake commented 3 days ago

Deferring the CalcSpatialInertia call in the parser is a relatively straightforward fix. I've opened #21649 to pull in the failing models and provide test coverage for that fix. I can push that fix through. Then I'll open a follow-up (lower priority) issue on the failing call to CalcSpatialInertia.