diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 3d878eff..7c6c8b56 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1246,18 +1246,21 @@ The full list of processing steps applied by the compiler to each mesh is as fol .. _asset-mesh-inertia: -:at:`inertia`: :at-val:`[convex, exact, legacy], "legacy"` +:at:`inertia`: :at-val:`[convex, exact, legacy, shell], "legacy"` This attribute controls how the mesh is used when mass and inertia are :ref:`inferred from geometry`. The current default value :at-val:`legacy` will be changed to :at-val:`convex` in a future release. - :at-val:`convex`: Use the mesh's convex hull to compute volume and inertia. + :at-val:`convex`: Use the mesh's convex hull to compute volume and inertia, assuming uniform density. - :at-val:`exact`: Use an exact algorithm to compute volume and inertia. This algorithm requires a well-oriented, - watertight mesh and will error otherwise. + :at-val:`exact`: Compute volume and inertia exactly, even for non-convex meshes. This algorithm requires a + well-oriented, watertight mesh and will error otherwise. - :at-val:`legacy`: Use the legacy algorithm, which is similar to :at-val:`convex`, but leads to volume overcounting - for non-convex meshes. + :at-val:`legacy`: Use the legacy algorithm, leads to volume overcounting for non-convex meshes. Though currently the + default to avoid breakages, it is not recommended. + + :at-val:`shell`: Assume mass is concentrated on the surface of the mesh. Use the mesh's surface to compute + the inertia, assuming uniform surface density. .. _asset-mesh-smoothnormal: @@ -2457,8 +2460,10 @@ helps clarify the role of bodies and geoms in MuJoCo. .. _body-geom-shellinertia: :at:`shellinertia` :at-val:`[false, true], "false"` - If true, the geom's inertia is computed assuming that all the mass is concentrated on the boundary. In this case - :at:`density` is interpreted as surface density rather than volumetric density. + If true, the geom's inertia is computed assuming that all the mass is concentrated on the surface. In this case + :at:`density` is interpreted as surface rather than volumetric density. This attribute only applies to primitive + geoms and is ignored for meshes. Surface inertia for meshes can be specified by setting the + :ref:`asset/mesh/inertia` attribute to :at-val:`"shell"`. .. _body-geom-solmix: diff --git a/doc/changelog.rst b/doc/changelog.rst index 43d1a79f..9b88b28a 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -34,6 +34,18 @@ General - Added :ref:`potential` and :ref:`kinetic` energy sensors. - Improved shadow rendering in the native renderer. +.. admonition:: Breaking API changes + :class: attention + + - Changes to inertia inference from meshes: + + Previously, in order to specify that the mass lies on the surface, :ref:`geom/shellinertia` + could be used for any geom type. Now this attribute is ignored if the geom is a mesh; instead, inertia inference + for meshes is specified in the asset, using the :ref:`asset/mesh/inertia` attribute. + + Previously, if the volumetric inertia computation failed (for example due to a very flat mesh), the compiler + would silently fall back to surface inertia computation. Now, the compiler will throw an informative error. + MJX ^^^ - Added support for spatial tendons with internal sphere and cylinder wrapping. diff --git a/doc/includes/references.h b/doc/includes/references.h index dcb3f22d..534fd989 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1652,10 +1652,11 @@ typedef enum mjtGeomInertia_ { // type of inertia inference mjINERTIA_VOLUME = 0, // mass distributed in the volume mjINERTIA_SHELL, // mass distributed on the surface } mjtGeomInertia; -typedef enum mjtMeshInertia_ { // type of mesh inertia - mjINERTIA_CONVEX = 0, // convex mesh inertia - mjINERTIA_EXACT, // exact mesh inertia - mjINERTIA_LEGACY, // legacy mesh inertia +typedef enum mjtMeshInertia_ { // type of mesh inertia + mjMESH_INERTIA_CONVEX = 0, // convex mesh inertia + mjMESH_INERTIA_EXACT, // exact mesh inertia + mjMESH_INERTIA_LEGACY, // legacy mesh inertia + mjMESH_INERTIA_SHELL // shell mesh inertia } mjtMeshInertia; typedef enum mjtBuiltin_ { // type of built-in procedural texture mjBUILTIN_NONE = 0, // no built-in texture @@ -2008,7 +2009,7 @@ typedef struct mjsMesh_ { // mesh specification double refpos[3]; // reference position double refquat[4]; // reference orientation double scale[3]; // rescale mesh - mjtMeshInertia inertia; // inertia type (convex, legacy, exact) + mjtMeshInertia inertia; // inertia type (convex, legacy, exact, shell) mjtByte smoothnormal; // do not exclude large-angle faces from normals int maxhullvert; // maximum vertex count for the convex hull mjFloatVec* uservert; // user vertex data diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 3e3d475c..9fd1046f 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -62,10 +62,11 @@ typedef enum mjtGeomInertia_ { // type of inertia inference } mjtGeomInertia; -typedef enum mjtMeshInertia_ { // type of mesh inertia - mjINERTIA_CONVEX = 0, // convex mesh inertia - mjINERTIA_EXACT, // exact mesh inertia - mjINERTIA_LEGACY, // legacy mesh inertia +typedef enum mjtMeshInertia_ { // type of mesh inertia + mjMESH_INERTIA_CONVEX = 0, // convex mesh inertia + mjMESH_INERTIA_EXACT, // exact mesh inertia + mjMESH_INERTIA_LEGACY, // legacy mesh inertia + mjMESH_INERTIA_SHELL // shell mesh inertia } mjtMeshInertia; @@ -459,7 +460,7 @@ typedef struct mjsMesh_ { // mesh specification double refpos[3]; // reference position double refquat[4]; // reference orientation double scale[3]; // rescale mesh - mjtMeshInertia inertia; // inertia type (convex, legacy, exact) + mjtMeshInertia inertia; // inertia type (convex, legacy, exact, shell) mjtByte smoothnormal; // do not exclude large-angle faces from normals int maxhullvert; // maximum vertex count for the convex hull mjFloatVec* uservert; // user vertex data diff --git a/introspect/enums.py b/introspect/enums.py index 770716a1..32caae09 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -734,9 +734,10 @@ ENUMS: Mapping[str, EnumDecl] = dict([ name='mjtMeshInertia', declname='enum mjtMeshInertia_', values=dict([ - ('mjINERTIA_CONVEX', 0), - ('mjINERTIA_EXACT', 1), - ('mjINERTIA_LEGACY', 2), + ('mjMESH_INERTIA_CONVEX', 0), + ('mjMESH_INERTIA_EXACT', 1), + ('mjMESH_INERTIA_LEGACY', 2), + ('mjMESH_INERTIA_SHELL', 3), ]), )), ('mjtBuiltin', diff --git a/introspect/structs.py b/introspect/structs.py index abe63628..27938984 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -10415,7 +10415,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='inertia', type=ValueType(name='mjtMeshInertia'), - doc='inertia type (convex, legacy, exact)', + doc='inertia type (convex, legacy, exact, shell)', ), StructFieldDecl( name='smoothnormal', diff --git a/src/user/user_init.c b/src/user/user_init.c index 08fceb59..60f74c64 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -245,7 +245,7 @@ void mjs_defaultMesh(mjsMesh* mesh) { mesh->refquat[0] = 1; mesh->scale[0] = mesh->scale[1] = mesh->scale[2] = 1; mesh->maxhullvert = -1; - mesh->inertia = mjINERTIA_LEGACY; + mesh->inertia = mjMESH_INERTIA_LEGACY; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a1c30cbe..a46df9e0 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -119,15 +119,10 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { elemtype = mjOBJ_MESH; // clear internal variables - mjuu_setvec(pos_surface_, 0, 0, 0); - 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(boxsz_, 0, 0, 0); mjuu_setvec(aamm_, 1e10, 1e10, 1e10); mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10); szgraph_ = 0; @@ -616,6 +611,9 @@ void mjCMesh::Compile(const mjVFS* vfs) { } tree_.CreateBVH(); } + + // check that processed mesh is valid + CheckMesh(); } @@ -649,35 +647,13 @@ void mjCMesh::SetBoundingVolume(int faceid) { -// get position -double* mjCMesh::GetPosPtr(mjtGeomInertia type) { - if (type==mjINERTIA_SHELL) { - return pos_surface_; - } else { - return pos_volume_; - } -} - - - -// get orientation -double* mjCMesh::GetQuatPtr(mjtGeomInertia type) { - if (type==mjINERTIA_SHELL) { - return quat_surface_; - } else { - return quat_volume_; - } -} - - - -double* mjCMesh::GetOffsetPosPtr() { +double* mjCMesh::GetPosPtr() { return pos_; } -double* mjCMesh::GetOffsetQuatPtr() { +double* mjCMesh::GetQuatPtr() { return quat_; } @@ -740,12 +716,12 @@ void mjCMesh::DelTexcoord() { // set geom size to match mesh void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { // copy mesh pos into meshpos - mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3); + mjuu_copyvec(meshpos, GetPosPtr(), 3); // use inertial box if (!model->compiler.fitaabb) { // get inertia box type (shell or volume) - double* boxsz = GetInertiaBoxPtr(geom->typeinertia); + double* boxsz = GetInertiaBoxPtr(); switch (geom->type) { case mjGEOM_SPHERE: geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3; @@ -1253,14 +1229,13 @@ void mjCMesh::LoadMSH(mjResource* resource) { } -void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type, - const double facecen[3]) { +void mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) { double nrm[3]; double cen[3]; - GetVolumeRef(type) = 0; + GetVolumeRef() = 0; mjuu_zerovec(CoM, 3); - int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface(); - int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); + int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface(); + int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); float* vv = vert_.data(); for (int i=0; i < nf; i++) { // get area, normal and center @@ -1268,19 +1243,57 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type, // compute and add volume const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]}; - double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(vec, nrm) * a / 3; + double vol = mjuu_dot3(vec, nrm) * a / 3; // if legacy computation requested, then always positive - if (inertia == mjINERTIA_LEGACY) { + if (inertia == mjMESH_INERTIA_LEGACY) { vol = abs(vol); } // add pyramid com - GetVolumeRef(type) += vol; + GetVolumeRef() += vol; for (int j=0; j<3; j++) { CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0); } } + + // if volume is valid normalize CoM + if (GetVolumeRef() < mjMINVAL) { + validvolume_ = GetVolumeRef() < 0 ? MeshNegativeVolume : MeshZeroVolume; + } else { + for (int j=0; j<3; j++) { + CoM[j] /= GetVolumeRef(); + } + } +} + + +void mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) { + double nrm[3]; + double cen[3]; + GetVolumeRef() = 0; + mjuu_zerovec(CoM, 3); + float* vv = vert_.data(); + for (int i=0; i < nface(); i++) { + // get area, normal and center + double a = _triangle(nrm, cen, vv+3*face_.data()[3*i], + vv+3*face_.data()[3*i+1], vv+3*face_.data()[3*i+2]); + + // add pyramid com + GetVolumeRef() += a; + for (int j=0; j<3; j++) { + CoM[j] += a*(cen[j]*3.0/4.0 + facecen[j]/4.0); + } + } + + // if area is valid normalize CoM + if (GetVolumeRef() < mjMINVAL) { + validarea_ = false; + } else { + for (int j=0; j<3; j++) { + CoM[j] /= GetVolumeRef(); + } + } } @@ -1400,7 +1413,7 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) { void mjCMesh::Process() { - double facecen[3] = {0, 0, 0};; + double facecen[3] = {0, 0, 0}; // user offset, rotation, scaling ApplyTransformations(); @@ -1409,124 +1422,102 @@ void mjCMesh::Process() { double density = model->def_map[classname]->Geom().density; - bool centered = false; // true if the mesh is centered at the CoM - bool aligned_with_inertial_frame = false; // true if mesh is aligned with inertial frame + // compute inertia and transform mesh. The mesh is transformed such that it is + // centered at the CoM and the axes are the principle axes of inertia + double CoM[3] = {0, 0, 0}; + double inert[6] = {0, 0, 0, 0, 0, 0}; - // compute inertial properties for both inertia types - // the mesh is transformed such that it is centered at the CoM and the axes - // are the principle axes of inertia. If the volume is valid, we use the volume - // inertia for this transformation, otherwise we use the shell inertia. - for ( const auto type : { mjtGeomInertia::mjINERTIA_VOLUME, mjtGeomInertia::mjINERTIA_SHELL } ) { - double CoM[3] = {0, 0, 0}; - double inert[6] = {0, 0, 0, 0, 0, 0}; - - // compute CoM and volume from pyramid volumes - ComputeVolume(CoM, type, facecen); - - // if volume is invalid, skip the rest of the computations - if (GetVolumeRef(type) < mjMINVAL) { - if (type == mjINERTIA_SHELL) { - validarea_ = 0; - } else { - validvolume_ = GetVolumeRef(type) < 0 ? MeshNegativeVolume : MeshZeroVolume; - } - continue; - } - - // finalize CoM, save as mesh center - for (int j=0; j<3; j++) { - CoM[j] /= GetVolumeRef(type); - } - mjuu_copyvec(GetPosPtr(type), CoM, 3); - - // re-center mesh at CoM - // we only want to do this if the mesh is not already centered at the CoM - if (!centered) { - for (int i=0; i < nvert(); i++) { - for (int j=0; j<3; j++) { - vert_[3*i+j] -= CoM[j]; - } - } - centered = true; - } - - // compute inertia - ComputeInertia(type, inert); - - // get quaternion and diagonal inertia - double eigval[3], eigvec[9], quattmp[4]; - double full[9] = { - inert[0], inert[3], inert[4], - inert[3], inert[1], inert[5], - inert[4], inert[5], inert[2] - }; - mjuu_eig3(eigval, eigvec, quattmp, full); - - // check eigval - SHOULD NOT OCCUR - if (eigval[2]<=0) { - valideigenvalue_ = false; + // compute CoM and volume/area + if (inertia == mjMESH_INERTIA_SHELL) { + ComputeSurfaceArea(CoM, facecen); + if (!validarea_) { return; } - if (eigval[0] + eigval[1] < eigval[2] || - eigval[0] + eigval[2] < eigval[1] || - eigval[1] + eigval[2] < eigval[0]) { - validinequality_ = false; + } else { + ComputeVolume(CoM, facecen); + if (validvolume_ != MeshVolumeOK) { return; } - - // compute sizes of equivalent inertia box - double mass = GetVolumeRef(type) * density; - double* boxsz = GetInertiaBoxPtr(type); - boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2; - boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2; - boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2; - - // if mesh is aligned with inertial frame, we already successfully - // computed the volume inertia, so we can copy volume quat to shell, - // otherwise use shell quat for coordinate transformations - if (aligned_with_inertial_frame) { - mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4); - } - // rotate vertices and normals to axes of inertia - // we only want to do this if the mesh is not already rotated - else { - mjuu_copyvec(GetQuatPtr(type), quattmp, 4); - Rotate(quattmp); - aligned_with_inertial_frame = true; - } } + + // compute inertia + ComputeInertia(inert, CoM); + + // get quaternion and diagonal inertia + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { + inert[0], inert[3], inert[4], + inert[3], inert[1], inert[5], + inert[4], inert[5], inert[2] + }; + mjuu_eig3(eigval, eigvec, quattmp, full); + + constexpr double inequality_atol = 1e-9; + constexpr double inequality_rtol = 1e-6; + + // check eigval - SHOULD NOT OCCUR + if (eigval[2]<=0) { + valideigenvalue_= false; + return; + } + if (eigval[0] + eigval[1] < eigval[2] * (1.0 - inequality_rtol) - inequality_atol || + eigval[0] + eigval[2] < eigval[1] * (1.0 - inequality_rtol) - inequality_atol || + eigval[1] + eigval[2] < eigval[0] * (1.0 - inequality_rtol) - inequality_atol) { + validinequality_ = false; + return; + } + + // compute sizes of equivalent inertia box + double mass = GetVolumeRef() * density; + double* boxsz = GetInertiaBoxPtr(); + boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2; + boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2; + boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2; + + // transform CoM to origin + Transform(CoM, quattmp); } -void mjCMesh::ComputeInertia(mjtGeomInertia type, double inert[6]) { +void mjCMesh::ComputeInertia(double inert[6], double CoM[3]) { double nrm[3]; double cen[3]; double density = model->def_map[classname]->Geom().density; + // copy vertices to avoid modifying the original mesh + std::vector vert_centered(vert_); + + // translate vertices to origin in order to compute inertia + for (int i=0; i < nvert(); i++) { + for (int j=0; j<3; j++) { + vert_centered[3*i+j] -= CoM[j]; + } + } + // accumulate products of inertia, recompute volume const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}}; double P[6] = {0, 0, 0, 0, 0, 0}; - GetVolumeRef(type) = 0; - int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface(); - int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); + GetVolumeRef() = 0; + int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface(); + int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); for (int i=0; i < nf; i++) { - float* D = vert_.data()+3*f[3*i]; - float* E = vert_.data()+3*f[3*i+1]; - float* F = vert_.data()+3*f[3*i+2]; + float* D = vert_centered.data()+3*f[3*i]; + float* E = vert_centered.data()+3*f[3*i+1]; + float* F = vert_centered.data()+3*f[3*i+2]; // get area, normal and center; update volume double a = _triangle(nrm, cen, D, E, F); - double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(cen, nrm) * a / 3; + double vol = inertia==mjMESH_INERTIA_SHELL ? a : mjuu_dot3(cen, nrm) * a / 3; // if legacy computation requested, then always positive - if (inertia == mjINERTIA_LEGACY) { + if (inertia == mjMESH_INERTIA_LEGACY) { vol = abs(vol); } // apply formula, accumulate - GetVolumeRef(type) += vol; + GetVolumeRef() += vol; for (int j=0; j<6; j++) { P[j] += density*vol / - (type==mjINERTIA_SHELL ? 12 : 20) * ( + (inertia==mjMESH_INERTIA_SHELL ? 12 : 20) * ( 2*(D[k[j][0]] * D[k[j][1]] + E[k[j][0]] * E[k[j][1]] + F[k[j][0]] * F[k[j][1]]) + @@ -1547,7 +1538,7 @@ void mjCMesh::ComputeInertia(mjtGeomInertia type, double inert[6]) { void mjCMesh::Rotate(double quat[4]) { - // Rotates vertices and normals of mesh by quaternion. + // rotate vertices and normals of mesh by quaternion double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]}; double mat[9]; mjuu_quat2mat(mat, neg); @@ -1575,22 +1566,37 @@ void mjCMesh::Rotate(double quat[4]) { } } + +void mjCMesh::Transform(double pos[3], double quat[4]) { + // subtract CoM position from vertices + for (int i=0; i < nvert(); i++) { + for (int j=0; j<3; j++) { + vert_[3*i+j] -= pos[j]; + } + } + Rotate(quat); + + // save the pos and quat that was used to transform the mesh + mjuu_copyvec(GetPosPtr(), pos, 3); + mjuu_copyvec(GetQuatPtr(), quat, 4); +} // check that the mesh is valid -void mjCMesh::CheckMesh(mjtGeomInertia type) { +void mjCMesh::CheckMesh() { if (!processed_) { return; } - if ((invalidorientation_.first>=0 || invalidorientation_.second>=0) && inertia == mjINERTIA_EXACT) + if ((invalidorientation_.first>=0 || invalidorientation_.second>=0) && inertia == mjMESH_INERTIA_EXACT) throw mjCError(this, "faces of mesh '%s' have inconsistent orientation. Please check the " "faces containing the vertices %d and %d.", name.c_str(), invalidorientation_.first, invalidorientation_.second); - if (!validarea_ && type==mjINERTIA_SHELL) + if (!validarea_ && inertia==mjMESH_INERTIA_SHELL) throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); - if (validvolume_==MeshNegativeVolume && type==mjINERTIA_VOLUME) + if (validvolume_==MeshNegativeVolume && inertia!=mjMESH_INERTIA_SHELL) throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str()); - if (validvolume_==MeshZeroVolume && type==mjINERTIA_VOLUME) - throw mjCError(this, "mesh volume is too small: %s", name.c_str()); + if (validvolume_==MeshZeroVolume && inertia!=mjMESH_INERTIA_SHELL) + throw mjCError(this, "mesh volume is too small: %s . Try setting inertia to shell", + name.c_str()); if (!valideigenvalue_) throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str()); if (!validinequality_) @@ -1599,15 +1605,14 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) { // get inertia pointer -double* mjCMesh::GetInertiaBoxPtr(mjtGeomInertia type) { - CheckMesh(type); - return type==mjINERTIA_SHELL ? boxsz_surface_ : boxsz_volume_; +double* mjCMesh::GetInertiaBoxPtr() { + return boxsz_; } -double& mjCMesh::GetVolumeRef(mjtGeomInertia type) { - CheckMesh(type); - return type==mjINERTIA_SHELL ? surface_ : volume_; +double& mjCMesh::GetVolumeRef() { + CheckMesh(); + return inertia==mjMESH_INERTIA_SHELL ? surface_ : volume_; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index fc4f4e67..fc21d29f 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2793,8 +2793,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->mesh_bvhnum[i] = pme->tree().Nbvh(); m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1; mjuu_copyvec(&m->mesh_scale[3 * i], pme->Scale(), 3); - mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); - mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); + mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetPosPtr(), 3); + mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetQuatPtr(), 4); // copy vertices, normals, faces, texcoords, aux data pme->CopyVert(m->mesh_vert + 3*vert_adr); @@ -4249,7 +4249,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (geoms_[i]->mesh && (geoms_[i]->spec.type == mjGEOM_MESH || geoms_[i]->spec.type == mjGEOM_SDF) && (geoms_[i]->spec.contype || geoms_[i]->spec.conaffinity || - geoms_[i]->mesh->spec.inertia == mjINERTIA_CONVEX)) { + geoms_[i]->mesh->spec.inertia == mjMESH_INERTIA_CONVEX)) { geoms_[i]->mesh->SetNeedHull(true); } } @@ -4633,8 +4633,8 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCMesh* pm; for (int i=0; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); - mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); + mjuu_copyvec(pm->GetPosPtr(), m->mesh_pos+3*i, 3); + mjuu_copyvec(pm->GetQuatPtr(), m->mesh_quat+4*i, 4); } // heightfield diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index a24ae31f..74062dff 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2386,7 +2386,7 @@ double mjCGeom::GetVolume() const { throw mjCError(this, "invalid mesh id in mesh geom"); } - return mesh->GetVolumeRef(typeinertia); + return mesh->GetVolumeRef(); } // compute from geom shape (type) and inertia type (typeinertia) @@ -2476,7 +2476,7 @@ void mjCGeom::SetInertia(void) { throw mjCError(this, "invalid mesh id in mesh geom"); } - double* boxsz = mesh->GetInertiaBoxPtr(typeinertia); + double* boxsz = mesh->GetInertiaBoxPtr(); inertia[0] = mass_ * (boxsz[1] * boxsz[1] + boxsz[2] * boxsz[2]) / 3; inertia[1] = mass_ * (boxsz[0] * boxsz[0] + boxsz[2] * boxsz[2]) / 3; inertia[2] = mass_ * (boxsz[0] * boxsz[0] + boxsz[1] * boxsz[1]) / 3; @@ -2551,6 +2551,7 @@ void mjCGeom::SetInertia(void) { double radius = size[0]; switch (typeinertia) { case mjINERTIA_VOLUME: + inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12; inertia[2] = mass_ * radius * radius / 2; return; @@ -3006,23 +3007,20 @@ void mjCGeom::Compile(void) { mjCMesh* pmesh = mesh; // fit geom if type is not mjGEOM_MESH - double meshpos[3]; if (type != mjGEOM_MESH && type != mjGEOM_SDF) { + double meshpos[3]; mesh->FitGeom(this, meshpos); // remove reference to mesh meshname_.clear(); mesh = nullptr; - } else { - // Retrieve the mesh position for the relevant inertia type. - mjuu_copyvec(meshpos, mesh->GetPosPtr(typeinertia), 3); + mjuu_copyvec(pmesh->GetPosPtr(), meshpos, 3); + } else if (typeinertia == mjINERTIA_SHELL) { + throw mjCError(this, "for mesh geoms, inertia should be specified in the mesh asset"); } // apply geom pos/quat as offset - mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia)); - mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3); - // Retrieve the mesh quaternion for the relevant inertia type. - mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4); + mjuu_frameaccum(pos, quat, pmesh->GetPosPtr(), pmesh->GetQuatPtr()); } // check size parameters diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 073182b3..f69b0e6d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -871,20 +871,16 @@ class mjCMesh_ : public mjCBase { MeshNegativeVolume = -1, MeshZeroVolume = 0, MeshVolumeOK = 1 - } validvolume_; // indicates if volume is valid - bool valideigenvalue_; // false if inertia eigenvalue is too small - bool validinequality_; // false if inertia inequality is not satisfied - bool processed_; // false if the mesh has not been processed yet + } validvolume_; // indicates if volume is valid + bool valideigenvalue_; // are inertia eigenvalues positive + bool validinequality_; // is inertia eigenvalue inequality satisfied + bool processed_; // has the mesh been processed yet + bool transformed_; // has the mesh been transformed to CoM and inertial frame // mesh properties computed by Compile - double pos_volume_[3]; // CoM position (volume) - double pos_surface_[3]; // CoM position (surface) - double quat_volume_[4]; // inertia orientation (volume) - double quat_surface_[4]; // inertia orientation (surface) - 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 pos_[3]; // CoM position + double quat_[4]; // inertia orientation + double boxsz_[3]; // half-sizes of equivalent inertia box double aamm_[6]; // axis-aligned bounding box in (min, max) format double volume_; // volume of the mesh double surface_; // surface of the mesh @@ -935,7 +931,6 @@ class mjCMesh: public mjCMesh_, private mjsMesh { const std::vector& Face() const { return face_; } const std::vector& UserFace() const { return spec_face_; } mjtMeshInertia Inertia() const { return spec.inertia; } - // setters void SetNeedHull(bool needhull) { needhull_ = needhull; } @@ -955,12 +950,10 @@ class mjCMesh: public mjCMesh_, private mjsMesh { const mjCBoundingVolumeHierarchy& tree() { return tree_; } void Compile(const mjVFS* vfs); // compiler - double* GetPosPtr(mjtGeomInertia type); // get position - double* GetQuatPtr(mjtGeomInertia type); // get orientation - double* GetOffsetPosPtr(); // get position offset for geom - double* GetOffsetQuatPtr(); // get orientation offset for geom - double* GetInertiaBoxPtr(mjtGeomInertia type); // get inertia box - double& GetVolumeRef(mjtGeomInertia type); // get volume + double* GetPosPtr(); // get position + double* GetQuatPtr(); // get orientation + double* GetInertiaBoxPtr(); // get inertia box + double& GetVolumeRef(); // get volume void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom bool HasTexcoord() const; // texcoord not null void DelTexcoord(); // delete texcoord @@ -991,20 +984,21 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void CacheMesh(mjCCache *cache, const mjResource* resource, std::string_view asset_type); - void LoadSDF(); // generate mesh using marching cubes - void MakeGraph(); // make graph of convex hull - void CopyGraph(); // copy graph into face data - void MakeNormal(); // compute vertex normals - void MakeCenter(); // compute face circumcircle data - void Process(); // compute inertial properties - void ApplyTransformations(); // apply user transformations - void ComputeFaceCentroid(double[3]); // compute centroid of all faces - void CheckMesh(mjtGeomInertia type); // check if the mesh is valid + void LoadSDF(); // generate mesh using marching cubes + void MakeGraph(); // make graph of convex hull + void CopyGraph(); // copy graph into face data + void MakeNormal(); // compute vertex normals + void MakeCenter(); // compute face circumcircle data + void Process(); // compute inertial properties + void ApplyTransformations(); // apply user transformations + void ComputeFaceCentroid(double[3]); // compute centroid of all faces + void CheckMesh(); // check if the mesh is valid void CopyPlugin(); - void Rotate(double quat[4]); // rotate mesh by quaternion + void Rotate(double quat[4]); // rotate mesh by quaternion + void Transform(double pos[3], double quat[4]); // transform mesh by position and quaternion // computes the inertia matrix of the mesh given the type of inertia - void ComputeInertia(mjtGeomInertia type, double inert[6]); + void ComputeInertia(double inert[6], double CoM[3]); // mesh data to be copied into mjModel double* center_; // face circumcenter data (3*nface) @@ -1017,7 +1011,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh { std::vector num_face_vertices_; // compute the volume and center-of-mass of the mesh given the face center - void ComputeVolume(double CoM[3], mjtGeomInertia gtype, const double facecen[3]); + void ComputeVolume(double CoM[3], const double facecen[3]); + // compute the surface area and center-of-mass of the mesh given the face center + void ComputeSurfaceArea(double CoM[3], const double facecen[3]); }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index deb01e2b..ec586a92 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -781,10 +781,11 @@ const mjMap meshtype_map[2] = { // mesh inertia type -const mjMap meshinertia_map[3] = { - {"convex", mjINERTIA_CONVEX}, - {"legacy", mjINERTIA_LEGACY}, - {"exact", mjINERTIA_EXACT} +const mjMap meshinertia_map[4] = { + {"convex", mjMESH_INERTIA_CONVEX}, + {"legacy", mjMESH_INERTIA_LEGACY}, + {"exact", mjMESH_INERTIA_EXACT}, + {"shell", mjMESH_INERTIA_SHELL} }; @@ -1432,7 +1433,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* mesh, const mjVFS* vfs) { ReadAttr(elem, "refpos", 3, mesh->refpos, text); ReadAttr(elem, "refquat", 4, mesh->refquat, text); ReadAttr(elem, "scale", 3, mesh->scale, text); - if (MapValue(elem, "inertia", &n, meshinertia_map, 3)) { + if (MapValue(elem, "inertia", &n, meshinertia_map, 4)) { mesh->inertia = (mjtMeshInertia)n; } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 4277bf07..8cb0ae1c 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -217,7 +217,7 @@ void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) { WriteAttrTxt(elem, "content_type", mesh->ContentType()); WriteAttrTxt(elem, "file", mesh->File()); if (mesh->Inertia() != def->Mesh().Inertia()) { - WriteAttrTxt(elem, "inertia", FindValue(meshinertia_map, 3, mesh->Inertia())); + WriteAttrTxt(elem, "inertia", FindValue(meshinertia_map, 4, mesh->Inertia())); } // write vertex data @@ -422,14 +422,14 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri mjCMesh* mesh = geom->mesh; // write pos/quat if there is a difference - if (!SameVector(geom->pos, mesh->GetPosPtr(geom->typeinertia), 3) || - !SameVector(geom->quat, mesh->GetQuatPtr(geom->typeinertia), 4)) { + if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) || + !SameVector(geom->quat, mesh->GetQuatPtr(), 4)) { // recover geom pos/quat before mesh frame transformation double p[3], q[4]; mjuu_copyvec(p, geom->pos, 3); mjuu_copyvec(q, geom->quat, 4); - mjuu_frameaccuminv(p, q, mesh->GetPosPtr(geom->typeinertia), - mesh->GetQuatPtr(geom->typeinertia)); + mjuu_frameaccuminv(p, q, mesh->GetPosPtr(), + mesh->GetQuatPtr()); // write WriteAttr(elem, "pos", 3, p, unitq+1); @@ -462,7 +462,10 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap); WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid); WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs); - WriteAttrKey(elem, "shellinertia", meshtype_map, 2, geom->typeinertia, def->Geom().typeinertia); + if (geom->type != mjGEOM_MESH) { + WriteAttrKey(elem, "shellinertia", meshtype_map, 2, geom->typeinertia, + def->Geom().typeinertia); + } if (mjuu_defined(geom->mass)) { WriteAttr(elem, "mass", 1, &geom->mass_, &mass); } else { diff --git a/test/user/testdata/inertia_shell.xml b/test/user/testdata/inertia_shell.xml index f64141aa..8e27d4b9 100644 --- a/test/user/testdata/inertia_shell.xml +++ b/test/user/testdata/inertia_shell.xml @@ -1,11 +1,11 @@ - + - + diff --git a/test/user/testdata/torus_shell.xml b/test/user/testdata/torus_shell.xml index b31a6039..9a7ee4a8 100644 --- a/test/user/testdata/torus_shell.xml +++ b/test/user/testdata/torus_shell.xml @@ -1,8 +1,8 @@ - + - + diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 39850f14..b0e5e6b4 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -444,6 +444,36 @@ TEST_F(MujocoTest, RecompileFails) { mj_deleteSpec(spec); } +TEST_F(PluginTest, ModifyShellInertiaFails) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array err; + mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size()); + ASSERT_THAT(spec, NotNull()) << err.data(); + + // add a geom to spec + mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), nullptr); + geom->type = mjGEOM_MESH; + mjs_setString(geom->meshname, "example_mesh"); + geom->typeinertia = mjINERTIA_SHELL; + + mjModel* model = mj_compile(spec, nullptr); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(mjs_getError(spec), + HasSubstr("inertia should be specified in the mesh asset")); + mj_deleteSpec(spec); + mj_deleteModel(model); +} + // ------------------- test recompilation multiple files ----------------------- TEST_F(PluginTest, RecompileCompare) { mjtNum tol = 0; diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index cac1b7ed..284c6edd 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -653,32 +653,17 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { mj_deleteModel(model); } -TEST_F(MjCMeshTest, ShellUsesVolumeFrame) { - const std::string xml_path_v = GetTestDataFilePath(kTorusPath); - const std::string xml_path_s = GetTestDataFilePath(kTorusShellPath); - std::array error; - mjModel* mv = mj_loadXML(xml_path_v.c_str(), 0, error.data(), error.size()); - mjModel* ms = mj_loadXML(xml_path_s.c_str(), 0, error.data(), error.size()); - mjtNum tolerance = std::numeric_limits::epsilon(); - EXPECT_NEAR(mv->geom_quat[0], ms->geom_quat[0], tolerance); - EXPECT_NEAR(mv->geom_quat[1], ms->geom_quat[1], tolerance); - EXPECT_NEAR(mv->geom_quat[2], ms->geom_quat[2], tolerance); - EXPECT_NEAR(mv->geom_quat[3], ms->geom_quat[3], tolerance); - mj_deleteModel(mv); - mj_deleteModel(ms); -} - TEST_F(MjCMeshTest, AreaTooSmall) { static constexpr char xml[] = R"( + face="2 0 3 0 1 3 1 2 3 0 2 1" inertia="shell"/> - + @@ -689,25 +674,6 @@ TEST_F(MjCMeshTest, AreaTooSmall) { EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); } -TEST_F(MjCMeshTest, AreaTooSmallAllowedWorld) { - static constexpr char xml[] = R"( - - - - - - - - - )"; - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); -} - TEST_F(MjCMeshTest, VolumeTooSmall) { static constexpr char xml[] = R"( @@ -727,6 +693,65 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { mjModel* model = LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); + mj_deleteModel(model); + +} + +TEST_F(MjCMeshTest, VisualVolumeTooSmall) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); + mj_deleteModel(model); + +} + +TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + EXPECT_LE(mju_abs(model->geom_size[0]), 1); + EXPECT_LE(mju_abs(model->geom_size[1]), 1); + EXPECT_LE(mju_abs(model->geom_size[2]), 1); + mj_deleteModel(model); } TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { @@ -735,11 +760,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { + face="0 1 2 2 1 3" inertia="shell"/> - + @@ -797,25 +822,6 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) { } } -TEST_F(MjCMeshTest, VolumeTooSmallAllowedWorld) { - static constexpr char xml[] = R"( - - - - - - - - - )"; - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); -} - // ------------- test concave and shell inertia -------------------------------- const mjtNum max_abs_err = std::numeric_limits::epsilon(); @@ -893,18 +899,18 @@ TEST_F(MjCMeshTest, MeshPosQuat) { char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; - // Loading the mesh results in an offset of the geom's pos and quat due to the + // 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. + // 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. + // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and + // quat. It should match the originally specified values mjtNum recovered_pos[3]; mjtNum recovered_quat[4]; mju_mulPose(recovered_pos, recovered_quat, @@ -919,7 +925,66 @@ TEST_F(MjCMeshTest, MeshPosQuat) { EXPECT_NEAR(recovered_quat[2], 0, 1e-12); EXPECT_NEAR(recovered_quat[3], 0, 1e-12); - // Same test on the other geom. + // 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); +} + +TEST_F(MjCMeshTest, MeshPosQuatShellInertia) { + static constexpr char xml[] = R"( + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + // 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 + mjtNum recovered_pos[3]; + mjtNum 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, @@ -958,6 +1023,28 @@ TEST_F(MjCMeshTest, MeshScale) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, ShellInertiaTest) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1)); + EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1)); + mj_deleteModel(model); +} + // ----------------------------- texcoord ------------------------------------- TEST_F(MjCMeshTest, CreateFaceTexCoord) { diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index b6fbaf5f..efd51a99 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1141,7 +1141,7 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) { + face="0 2 1" inertia="shell"/> diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index c7509183..873d9294 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1728,10 +1728,10 @@ TEST_F(XMLReaderTest, ReadShellParameter) { + face="0 2 1 2 0 3" inertia="shell"/> - + )"; @@ -1741,7 +1741,6 @@ TEST_F(XMLReaderTest, ReadShellParameter) { mj_deleteModel(model); } - TEST_F(XMLReaderTest, ReadsSkinGroups) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 1b8e2eb2..36dca0de 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1323,7 +1323,6 @@ TEST_F(XMLWriterTest, WriteReadCompare) { absl::StrContains(p.path().string(), "spheremesh")) { continue; } - // load model std::array error; mjModel* m = mj_loadXML( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index cb60989d..80729811 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -458,9 +458,10 @@ public enum mjtGeomInertia : int{ mjINERTIA_SHELL = 1, } public enum mjtMeshInertia : int{ - mjINERTIA_CONVEX = 0, - mjINERTIA_EXACT = 1, - mjINERTIA_LEGACY = 2, + mjMESH_INERTIA_CONVEX = 0, + mjMESH_INERTIA_EXACT = 1, + mjMESH_INERTIA_LEGACY = 2, + mjMESH_INERTIA_SHELL = 3, } public enum mjtBuiltin : int{ mjBUILTIN_NONE = 0,