drake icon indicating copy to clipboard operation
drake copied to clipboard

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

Open RussTedrake opened this issue 1 year ago • 1 comments

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 avatar Jun 29 '24 21:06 RussTedrake