diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index c189757a..b8518652 100644 --- a/doc/XMLreference.rst +++ b/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 -orientation; see also :at:`mesh` attribute of :ref:`geom ` below. Fortunately most meshes used in robot -models are designed in a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we -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. -Below is an MJCF model fragment of a forearm, containing all the information needed to put the mesh where one would -expect it to be. The body position is specified relative to the parent body, namely the upper arm (not shown). It is -offset by 35 cm which is the typical length of the human upper arm. If the mesh vertex data were not designed in the -above convention, we would have to use the geom position and orientation (or the new refpos, refquat mechanism) to -compensate, but in practice this is rarely needed. - +rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos` and +:ref:`mjModel.mesh_quat`. These offsets are then applied to the referencing geom's position and orientation; see +also :at:`mesh` attribute of :ref:`geom ` below. Fortunately most meshes used in robot models are designed in +a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we 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. Below is an MJCF model +fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body +position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the +typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have +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 diff --git a/doc/changelog.rst b/doc/changelog.rst index 835ab828..5c690f10 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -64,17 +64,17 @@ General :ref:`integrators` other than ``RK4``. See the flag documentation for more details. 12. Added :ref:`ls_iterations` and :ref:`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 `__ where calling ``update_scene`` with an invalid +14. Fixed `#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) diff --git a/doc/includes/references.h b/doc/includes/references.h index eda352fe..7fa7dca4 100644 --- a/doc/includes/references.h +++ b/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) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 13b7fc5a..ec3bfeac 100644 --- a/include/mujoco/mjmodel.h +++ b/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) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 842d7914..9f64b389 100644 --- a/include/mujoco/mjxmacro.h +++ b/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 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index 2e5f08a0..b9877656 100644 --- a/introspect/structs.py +++ b/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( diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 606fc468..500e444a 100644 --- a/src/user/user_mesh.cc +++ b/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; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8b782f08..c69fffcf 100644 --- a/src/user/user_model.cc +++ b/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; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); + copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); + } + // sites for (int i=0; isize, m->site_size + 3 * i, 3); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index df06cafe..3f5f6376 100644 --- a/src/user/user_objects.cc +++ b/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 diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 148b3997..812606dc 100644 --- a/src/user/user_objects.h +++ b/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 diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index d3c65a4f..4013ed74 100644 --- a/test/user/user_mesh_test.cc +++ b/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"( + + + + + + + + + + )"; + 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 diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2500e8a6..685233a1 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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;