chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
Fixes #409

PiperOrigin-RevId: 564217402
Change-Id: I53ddeef4e343905021cd7852d548f6769c876474
This commit is contained in:
Matthew Bennice
2023-09-10 15:25:17 -07:00
committed by Copybara-Service
parent a07e172770
commit 0ba10cc4f0
12 changed files with 124 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+10 -10
View File
@@ -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 <body-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<mjModel>` and
:ref:`mjModel.mesh_quat<mjModel>`. These offsets are then applied to the referencing geom's position and orientation; see
also :at:`mesh` attribute of :ref:`geom <body-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
<asset>
doc/changelog.rst
+3 -3
View File
@@ -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
+2
View File
@@ -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
+2
View File
@@ -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
+2
View File
@@ -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
+14
View File
@@ -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
+15
View File
@@ -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
+11
View File
@@ -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
+2
View File
@@ -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
+4
View File
@@ -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
+57
View File
@@ -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
+2
View File
@@ -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;