Save normalising transform applied to mesh assets in mjModel.mesh_{pos,quat}

Fixes #409

PiperOrigin-RevId: 564217402
Change-Id: I53ddeef4e343905021cd7852d548f6769c876474

doc/XMLreference.rst

@@ -1439,16 +1439,16 @@ Positioning and orienting is complicated by the fact that vertex data are often
 whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to coincide with the
 geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, so that it is
 centered around (0,0,0) and its principal axes of inertia are the coordinate axes. 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
+rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos<mjModel>` and
-orientation; see also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot
+:ref:`mjModel.mesh_quat<mjModel>`. These offsets are then applied to the referencing geom's position and orientation; see
-models are designed in a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we
+also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in
-set the body frame at the joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh.
+a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the
-Below is an MJCF model fragment of a forearm, containing all the information needed to put the mesh where one would
+joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model
-expect it to be. The body position is specified relative to the parent body, namely the upper arm (not shown). It is
+fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body
-offset by 35 cm which is the typical length of the human upper arm. If the mesh vertex data were not designed in the
+position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the
-above convention, we would have to use the geom position and orientation (or the new refpos, refquat mechanism) to
+typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have
-compensate, but in practice this is rarely needed.
+to use the geom position and orientation (or the new refpos, refquat mechanism) to compensate, but in practice this is
-
+rarely needed.
 .. code-block:: xml
 
    <asset>

doc/changelog.rst

@@ -64,17 +64,17 @@ General
     :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
 12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
     linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning.
-
+13. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store normalizing transformation.
 Python bindings
 ^^^^^^^^^^^^^^^
 
-13. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+14. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
     camera name used the default camera.
 
 Bug fixes
 ^^^^^^^^^
 
-14. Fixed a bug that was causing the geom margins to be ignored during the midphase.
+15. Fixed a bug that was causing the geom margins to be ignored during the midphase.
 
 
 Version 2.3.7 (July 20, 2023)

doc/includes/references.h

@@ -1045,6 +1045,8 @@ struct mjModel_ {
   int*      mesh_texcoordadr;     // texcoord data address; -1: no texcoord   (nmesh x 1)
   int*      mesh_texcoordnum;     // number of texcoord                       (nmesh x 1)
   int*      mesh_graphadr;        // graph data address; -1: no graph         (nmesh x 1)
+  mjtNum*   mesh_pos;             // translation applied to asset vertices    (nmesh x 3)
+  mjtNum*   mesh_quat;            // rotation applied to asset vertices       (nmesh x 4)
   float*    mesh_vert;            // vertex positions for all meshes          (nmeshvert x 3)
   float*    mesh_normal;          // normals for all meshes                   (nmeshnormal x 3)
   float*    mesh_texcoord;        // vertex texcoords for all meshes          (nmeshtexcoord x 2)

include/mujoco/mjmodel.h

@@ -780,6 +780,8 @@ struct mjModel_ {
   int*      mesh_texcoordadr;     // texcoord data address; -1: no texcoord   (nmesh x 1)
   int*      mesh_texcoordnum;     // number of texcoord                       (nmesh x 1)
   int*      mesh_graphadr;        // graph data address; -1: no graph         (nmesh x 1)
+  mjtNum*   mesh_pos;             // translation applied to asset vertices    (nmesh x 3)
+  mjtNum*   mesh_quat;            // rotation applied to asset vertices       (nmesh x 4)
   float*    mesh_vert;            // vertex positions for all meshes          (nmeshvert x 3)
   float*    mesh_normal;          // normals for all meshes                   (nmeshnormal x 3)
   float*    mesh_texcoord;        // vertex texcoords for all meshes          (nmeshtexcoord x 2)

include/mujoco/mjxmacro.h

@@ -294,6 +294,8 @@
     XMJV( int,     mesh_bvhadr,           nmesh,         1                    ) \
     XMJV( int,     mesh_bvhnum,           nmesh,         1                    ) \
     XMJV( int,     mesh_graphadr,         nmesh,         1                    ) \
+    X   ( mjtNum,  mesh_pos,              nmesh,         3                    ) \
+    X   ( mjtNum,  mesh_quat,             nmesh,         4                    ) \
     X   ( float,   mesh_vert,             nmeshvert,     3                    ) \
     X   ( float,   mesh_normal,           nmeshnormal,   3                    ) \
     X   ( float,   mesh_texcoord,         nmeshtexcoord, 2                    ) \

introspect/structs.py

@@ -2102,6 +2102,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='graph data address; -1: no graph         (nmesh x 1)',
              ),
+             StructFieldDecl(
+                 name='mesh_pos',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='translation applied to asset vertices    (nmesh x 3)',
+             ),
+             StructFieldDecl(
+                 name='mesh_quat',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='rotation applied to asset vertices       (nmesh x 4)',
+             ),
              StructFieldDecl(
                  name='mesh_vert',
                  type=PointerType(

src/user/user_mesh.cc

@@ -134,6 +134,9 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   mjuu_setvec(pos_volume_, 0, 0, 0);
   mjuu_setvec(quat_surface_, 1, 0, 0, 0);
   mjuu_setvec(quat_volume_, 1, 0, 0, 0);
+  mjuu_setvec(pos_, 0, 0, 0);
+  mjuu_setvec(quat_, 1, 0, 0, 0);
+
   mjuu_setvec(boxsz_surface_, 0, 0, 0);
   mjuu_setvec(boxsz_volume_, 0, 0, 0);
   mjuu_setvec(aabb_, 1e10, 1e10, 1e10);
@@ -630,6 +633,18 @@ double* mjCMesh::GetQuatPtr(mjtMeshType type) {
 
 
 
+double* mjCMesh::GetOffsetPosPtr() {
+  return pos_;
+}
+
+
+
+double* mjCMesh::GetOffsetQuatPtr() {
+  return quat_;
+}
+
+
+
 bool mjCMesh::HasTexcoord() const {
   return texcoord_ != nullptr;
 }

src/user/user_model.cc

@@ -1737,6 +1737,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
     m->mesh_bvhadr[i] = bvh_adr;
     m->mesh_bvhnum[i] = pme->tree().nbvh;
+    copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
+    copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
 
     // copy vertices, normals, faces, texcoords, aux data
     pme->CopyVert(m->mesh_vert + 3*vert_adr);
@@ -3015,6 +3017,15 @@ bool mjCModel::CopyBack(const mjModel* m) {
     }
   }
 
+  // mesh
+  mjCMesh* pm;
+  for (int i=0; i<nmesh; i++) {
+    pm = meshes[i];
+
+    copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
+    copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
+  }
+
   // sites
   for (int i=0; i<nsite; i++) {
     copyvec(sites[i]->size, m->site_size + 3 * i, 3);

src/user/user_objects.cc

@@ -1747,6 +1747,8 @@ void mjCGeom::Compile(void) {
 
     // apply geom pos/quat as offset
     mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
+    mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
+    mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
   }
 
   // check size parameters

src/user/user_objects.h

@@ -576,6 +576,8 @@ class mjCMesh: public mjCBase {
   void Compile(const mjVFS* vfs);                   // compiler
   double* GetPosPtr(mjtMeshType type);              // get position
   double* GetQuatPtr(mjtMeshType type);             // get orientation
+  double* GetOffsetPosPtr();                        // get position offset for geom
+  double* GetOffsetQuatPtr();                       // get orientation offset for geom
   double* GetInertiaBoxPtr(mjtMeshType type);       // get inertia box
   double& GetVolumeRef(mjtMeshType type);           // get volume
   void FitGeom(mjCGeom* geom, double* meshpos);     // approximate mesh with simple geom
@@ -639,6 +641,8 @@ class mjCMesh: public mjCBase {
   double pos_surface_[3];             // CoM position
   double quat_volume_[4];             // inertia orientation
   double quat_surface_[4];            // inertia orientation
+  double pos_[3];                     // translation applied to asset vertices
+  double quat_[4];                    // rotation applied to asset vertices
   double boxsz_volume_[3];            // half-sizes of equivalent inertia box (volume)
   double boxsz_surface_[3];           // half-sizes of equivalent inertia box (surface)
   double aabb_[6];                    // axis-aligned bounding box

test/user/user_mesh_test.cc

@@ -802,5 +802,62 @@ TEST_F(MjCMeshTest, ExactShellInertia) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjCMeshTest, MeshPosQuat) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="pyramid" vertex="0 0 0  1 0 0  0 1 0  0 0 1"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" name="geom1" mesh="pyramid"/>
+      <geom type="mesh" name="geom2" pos="1 2 3" quat="0.5 0.5 0.5 0.5" mesh="pyramid"/>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, testing::NotNull());
+  // Loading the mesh results in an offset of the geom's pos and quat due to the
+  // fact that the geom's center is not the volumetric center of the mesh. To
+  // recover the geom's originally specified pose, the offset used is stored in
+  // mesh_pos and mesh_quat. In order to recover the originally specified pose
+  // and orientation, first invert the specified mesh_pos and mesh_quat.
+  mjtNum inverse_mesh_pos[3];
+  mjtNum inverse_mesh_quat[4];
+  mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
+              &model->mesh_pos[0], &model->mesh_quat[0]);
+
+  // Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
+  // quat. It should match the originally specified values.
+  double recovered_pos[3];
+  double recovered_quat[4];
+  mju_mulPose(recovered_pos, recovered_quat,
+              &model->geom_pos[0], &model->geom_quat[0],
+              inverse_mesh_pos, inverse_mesh_quat);
+  EXPECT_NEAR(recovered_pos[0], 0, 1e-12);
+  EXPECT_NEAR(recovered_pos[1], 0, 1e-12);
+  EXPECT_NEAR(recovered_pos[2], 0, 1e-12);
+
+  EXPECT_NEAR(recovered_quat[0], 1, 1e-12);
+  EXPECT_NEAR(recovered_quat[1], 0, 1e-12);
+  EXPECT_NEAR(recovered_quat[2], 0, 1e-12);
+  EXPECT_NEAR(recovered_quat[3], 0, 1e-12);
+
+  // Same test on the other geom.
+  mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
+              &model->mesh_pos[0], &model->mesh_quat[0]);
+  mju_mulPose(recovered_pos, recovered_quat,
+              &model->geom_pos[3], &model->geom_quat[4],
+              inverse_mesh_pos, inverse_mesh_quat);
+  EXPECT_NEAR(recovered_pos[0], 1, 1e-12);
+  EXPECT_NEAR(recovered_pos[1], 2, 1e-12);
+  EXPECT_NEAR(recovered_pos[2], 3, 1e-12);
+
+  EXPECT_NEAR(recovered_quat[0], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[1], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[2], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[3], 0.5, 1e-12);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco

unity/Runtime/Bindings/MjBindings.cs

@@ -2115,6 +2115,8 @@ public unsafe struct mjModel_ {
   public int* mesh_texcoordadr;
   public int* mesh_texcoordnum;
   public int* mesh_graphadr;
+  public double* mesh_pos;
+  public double* mesh_quat;
   public float* mesh_vert;
   public float* mesh_normal;
   public float* mesh_texcoord;