Get correct world position and orientation for each mesh in MuJoCo to export to external viewer

[zcczhang]

Hi,

I'm a researcher and I'm trying to use MuJoCo for research.

I'd like to know how to get the correct world position and orientation for each mesh in MuJoCo to export to an external viewer eg open3d

For example, for a robot XML in mujoco_menagerie, at every step, I'd like to keep tracking each mesh's pos and rotations and render them into the external viewer, e.g. open3d. I first get the body to mesh map by sth like

def generate_mesh_name_to_body_map(root):
    mesh_to_body_map = {}

    def recurse_bodies(parent_body, parent_name=""):
        body_name = parent_body.get("name") or parent_name
        if body_name:
            for geom in parent_body.findall("./geom"):
                mesh_name = geom.get("mesh")
                if mesh_name:
                    if mesh_name in mesh_to_body_map:
                        assert body_name == mesh_to_body_map[mesh_name]
                    mesh_to_body_map[mesh_name] = body_name
            for sub_body in parent_body.findall("./body"):
                recurse_bodies(sub_body, body_name)

    for body in root.findall("./worldbody/body"):
        recurse_bodies(body)
    return mesh_to_body_map

And then by iteration and mapping, getting the transform

        data_pos = data.xpos[body_id]
        data_xmat = data.xmat[body_id].reshape(3, 3)
        transform = np.eye(4)
        transform[:3, :3] = data_xmat
        transform[:3, 3] = data_pos
        render_object: o3d.geometry.TriangleMesh
        render_object.transform(global_transform)

However, most parts work properly, but it's weird that one of the grippers has the wrong orientation, and is missing one mesh part, which may be due to the wrong rotation or sth else. Here is a screenshot that compares the meshes in MuJoCo (left) and open3d (right):

Thanks for the help in advance!

[yuvaltassa]

MuJoCo applies an automatic transform to all meshes (centering and reorienting), this is explained here. This transformation is recorded in mjModel.mesh_{scale, pos, quat}.

Note to self update the docs.

[yuvaltassa]

Actually, this is already documented!

We save the translation and rotation offsets applied to the source asset in mjModel.mesh_pos and mjModel.mesh_quat; these are required if one reads vertex data from the source and needs to re-apply the transform.

[zcczhang]

thanks @yuvaltassa do you mean to use the mesh_transform_mat @ np.ling.inv(geom_transform_mat) where mesh_transform_mat is matrix from mjModel.mesh_pos and mjModel.mesh_quat and geom_transform_mat is from geom pos and quat? Seems it still does not work properly. Could you please share some simple explanatory scripts for this?

[yuvaltassa]

1757, #409