How to get the auto-adjusted mesh pos/quat for visualization in RViz?

[PeterMitrano]

Hi,

I'm a phd student and I'm trying to use MuJoCo for robotic manipulation.

I want to visualize the mujoco scene in rviz using Markers. I'm doing this by creaiting a MESH_RESOURCE marker, which loads from the STLs that mujoco loads. However, when I use the geom xpos/xmat to set the Marker pose in rviz, it is wrong. I think this is because mujoco automatically transforms meshes to be centered and have axis aligned inertia. However, I can't find anywhere in the python or C API where these transforms are exposed. It looks like user_mesh.cc is where these values are calculated, but they don't seem to be exposed anywhere. However, the docs suggest they are:

We also save the translation and rotation offsets needed to achieve such alignment. These offsets are then applied to the referencing geom’s position and orientation

Here is an example STL we can use as reference:

https://github.com/UM-ARM-Lab/link_bot/blob/master/dm_envs/meshes/Drive56_mujoco.stl

If there's no way to access this info, a workaround would be to make a new version of the mesh that is already centered/rotated, but I'm not sure how to do that exactly the same way mujoco does. I tried it in meshlab, and sometimes it worked, but sometimes meshlab gave different results than mujoco for transform.

code for visualizer I'm working on: https://github.com/UM-ARM-Lab/link_bot/blob/master/dm_envs/src/dm_envs/mujoco_visualizer.py

[yuvaltassa]

As it says in the docs, I believe this transformation (or rather the inverse of the transformation) lives in mjModel.geom_pos and mjModel.geom_quat, of the geom referencing the mesh. I have not tested this. The prediction is that if you apply this transformation to the vertices in the MuJoCo model, you will recover the vertices in your STL. mju_quat2mat would be a useful function in this regard.

Let us know if this doesn't work for you.

[PeterMitrano]

That geom_pos/quat tells me the transform from the body to the mesh frame, which isn't what I need. That's a combination of the transform from body to geom, and geom to mesh. I want that separately, specifically the transform from the geom frame to the mesh frame.

Consider this simple 2m box, where the origin is at the corner, not the center.

https://github.com/UM-ARM-Lab/link_bot/blob/master/dm_envs/meshes/box.stl

If you put this in xml as <geom pos="1 0 0" mesh="box" name="box"/>, for instance, and you look at model.geom_pos you'll get "2 1 1" which is the combination of "1 0 0" from body to geom and the "1 1 1" from geom to mesh. This "1 1 1" is what I'm trying to get.

[yuvaltassa]

Yep. But since you know the transform in the XML, you should be able to recover the vertex transform, right?

We could add fields to the model (probably mesh_pos and mesh_quat) to expose the transform you want, but I'm not sure your use case justifies that, especially as there is a simple workaround. We'll discuss internally.

Is there anyone else here besides the OP who would like the raw vertex transform to be exposed?

[PeterMitrano]

That's a good point, I had expected to be able to access the transform I put in the XML via either model or data but I'm not sure if that's actually possible. As we've said model.geom_pos isn't it. So other than re-parsing the XML is there a good way to get that info?

[PeterMitrano]

For now, my workaround has been to ensure I never use pos or quat inside a geom tag for a mesh, instead wrapping them in a body and putting the pos/quat there instead. Then it's easy, but that's just a workaround.