Changes to inertia inference from meshes.

PiperOrigin-RevId: 726051033
Change-Id: I6edfc118280d103a2dd9d07f29f0094858769761
This commit is contained in:
Tom Power
2025-02-12 07:42:15 -08:00
committed by Copybara-Service
parent e39a5df06a
commit 89253d957d
21 changed files with 438 additions and 299 deletions
+13 -8
View File
@@ -1246,18 +1246,21 @@ The full list of processing steps applied by the compiler to each mesh is as fol
.. _asset-mesh-inertia: .. _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 This attribute controls how the mesh is used when mass and inertia are
:ref:`inferred from geometry<compiler-inertiafromgeom>`. The current default value :at-val:`legacy` will be changed :ref:`inferred from geometry<compiler-inertiafromgeom>`. The current default value :at-val:`legacy` will be changed
to :at-val:`convex` in a future release. 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, :at-val:`exact`: Compute volume and inertia exactly, even for non-convex meshes. This algorithm requires a
watertight mesh and will error otherwise. 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 :at-val:`legacy`: Use the legacy algorithm, leads to volume overcounting for non-convex meshes. Though currently the
for non-convex meshes. 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: .. _asset-mesh-smoothnormal:
@@ -2457,8 +2460,10 @@ helps clarify the role of bodies and geoms in MuJoCo.
.. _body-geom-shellinertia: .. _body-geom-shellinertia:
:at:`shellinertia` :at-val:`[false, true], "false"` :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 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 density rather than volumetric density. :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<asset-mesh-inertia>` attribute to :at-val:`"shell"`.
.. _body-geom-solmix: .. _body-geom-solmix:
+12
View File
@@ -34,6 +34,18 @@ General
- Added :ref:`potential<sensor-e_potential>` and :ref:`kinetic<sensor-e_kinetic>` energy sensors. - Added :ref:`potential<sensor-e_potential>` and :ref:`kinetic<sensor-e_kinetic>` energy sensors.
- Improved shadow rendering in the native renderer. - 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<body-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<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 MJX
^^^ ^^^
- Added support for spatial tendons with internal sphere and cylinder wrapping. - Added support for spatial tendons with internal sphere and cylinder wrapping.
+6 -5
View File
@@ -1652,10 +1652,11 @@ typedef enum mjtGeomInertia_ { // type of inertia inference
mjINERTIA_VOLUME = 0, // mass distributed in the volume mjINERTIA_VOLUME = 0, // mass distributed in the volume
mjINERTIA_SHELL, // mass distributed on the surface mjINERTIA_SHELL, // mass distributed on the surface
} mjtGeomInertia; } mjtGeomInertia;
typedef enum mjtMeshInertia_ { // type of mesh inertia typedef enum mjtMeshInertia_ { // type of mesh inertia
mjINERTIA_CONVEX = 0, // convex mesh inertia mjMESH_INERTIA_CONVEX = 0, // convex mesh inertia
mjINERTIA_EXACT, // exact mesh inertia mjMESH_INERTIA_EXACT, // exact mesh inertia
mjINERTIA_LEGACY, // legacy mesh inertia mjMESH_INERTIA_LEGACY, // legacy mesh inertia
mjMESH_INERTIA_SHELL // shell mesh inertia
} mjtMeshInertia; } mjtMeshInertia;
typedef enum mjtBuiltin_ { // type of built-in procedural texture typedef enum mjtBuiltin_ { // type of built-in procedural texture
mjBUILTIN_NONE = 0, // no built-in texture mjBUILTIN_NONE = 0, // no built-in texture
@@ -2008,7 +2009,7 @@ typedef struct mjsMesh_ { // mesh specification
double refpos[3]; // reference position double refpos[3]; // reference position
double refquat[4]; // reference orientation double refquat[4]; // reference orientation
double scale[3]; // rescale mesh 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 mjtByte smoothnormal; // do not exclude large-angle faces from normals
int maxhullvert; // maximum vertex count for the convex hull int maxhullvert; // maximum vertex count for the convex hull
mjFloatVec* uservert; // user vertex data mjFloatVec* uservert; // user vertex data
+6 -5
View File
@@ -62,10 +62,11 @@ typedef enum mjtGeomInertia_ { // type of inertia inference
} mjtGeomInertia; } mjtGeomInertia;
typedef enum mjtMeshInertia_ { // type of mesh inertia typedef enum mjtMeshInertia_ { // type of mesh inertia
mjINERTIA_CONVEX = 0, // convex mesh inertia mjMESH_INERTIA_CONVEX = 0, // convex mesh inertia
mjINERTIA_EXACT, // exact mesh inertia mjMESH_INERTIA_EXACT, // exact mesh inertia
mjINERTIA_LEGACY, // legacy mesh inertia mjMESH_INERTIA_LEGACY, // legacy mesh inertia
mjMESH_INERTIA_SHELL // shell mesh inertia
} mjtMeshInertia; } mjtMeshInertia;
@@ -459,7 +460,7 @@ typedef struct mjsMesh_ { // mesh specification
double refpos[3]; // reference position double refpos[3]; // reference position
double refquat[4]; // reference orientation double refquat[4]; // reference orientation
double scale[3]; // rescale mesh 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 mjtByte smoothnormal; // do not exclude large-angle faces from normals
int maxhullvert; // maximum vertex count for the convex hull int maxhullvert; // maximum vertex count for the convex hull
mjFloatVec* uservert; // user vertex data mjFloatVec* uservert; // user vertex data
+4 -3
View File
@@ -734,9 +734,10 @@ ENUMS: Mapping[str, EnumDecl] = dict([
name='mjtMeshInertia', name='mjtMeshInertia',
declname='enum mjtMeshInertia_', declname='enum mjtMeshInertia_',
values=dict([ values=dict([
('mjINERTIA_CONVEX', 0), ('mjMESH_INERTIA_CONVEX', 0),
('mjINERTIA_EXACT', 1), ('mjMESH_INERTIA_EXACT', 1),
('mjINERTIA_LEGACY', 2), ('mjMESH_INERTIA_LEGACY', 2),
('mjMESH_INERTIA_SHELL', 3),
]), ]),
)), )),
('mjtBuiltin', ('mjtBuiltin',
+1 -1
View File
@@ -10415,7 +10415,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl( StructFieldDecl(
name='inertia', name='inertia',
type=ValueType(name='mjtMeshInertia'), type=ValueType(name='mjtMeshInertia'),
doc='inertia type (convex, legacy, exact)', doc='inertia type (convex, legacy, exact, shell)',
), ),
StructFieldDecl( StructFieldDecl(
name='smoothnormal', name='smoothnormal',
+1 -1
View File
@@ -245,7 +245,7 @@ void mjs_defaultMesh(mjsMesh* mesh) {
mesh->refquat[0] = 1; mesh->refquat[0] = 1;
mesh->scale[0] = mesh->scale[1] = mesh->scale[2] = 1; mesh->scale[0] = mesh->scale[1] = mesh->scale[2] = 1;
mesh->maxhullvert = -1; mesh->maxhullvert = -1;
mesh->inertia = mjINERTIA_LEGACY; mesh->inertia = mjMESH_INERTIA_LEGACY;
} }
+150 -145
View File
@@ -119,15 +119,10 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
elemtype = mjOBJ_MESH; elemtype = mjOBJ_MESH;
// clear internal variables // 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(pos_, 0, 0, 0);
mjuu_setvec(quat_, 1, 0, 0, 0); mjuu_setvec(quat_, 1, 0, 0, 0);
mjuu_setvec(boxsz_surface_, 0, 0, 0); mjuu_setvec(boxsz_, 0, 0, 0);
mjuu_setvec(boxsz_volume_, 0, 0, 0);
mjuu_setvec(aamm_, 1e10, 1e10, 1e10); mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10); mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10);
szgraph_ = 0; szgraph_ = 0;
@@ -616,6 +611,9 @@ void mjCMesh::Compile(const mjVFS* vfs) {
} }
tree_.CreateBVH(); tree_.CreateBVH();
} }
// check that processed mesh is valid
CheckMesh();
} }
@@ -649,35 +647,13 @@ void mjCMesh::SetBoundingVolume(int faceid) {
// get position double* mjCMesh::GetPosPtr() {
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() {
return pos_; return pos_;
} }
double* mjCMesh::GetOffsetQuatPtr() { double* mjCMesh::GetQuatPtr() {
return quat_; return quat_;
} }
@@ -740,12 +716,12 @@ void mjCMesh::DelTexcoord() {
// set geom size to match mesh // set geom size to match mesh
void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// copy mesh pos into meshpos // copy mesh pos into meshpos
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3); mjuu_copyvec(meshpos, GetPosPtr(), 3);
// use inertial box // use inertial box
if (!model->compiler.fitaabb) { if (!model->compiler.fitaabb) {
// get inertia box type (shell or volume) // get inertia box type (shell or volume)
double* boxsz = GetInertiaBoxPtr(geom->typeinertia); double* boxsz = GetInertiaBoxPtr();
switch (geom->type) { switch (geom->type) {
case mjGEOM_SPHERE: case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3; 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, void mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) {
const double facecen[3]) {
double nrm[3]; double nrm[3];
double cen[3]; double cen[3];
GetVolumeRef(type) = 0; GetVolumeRef() = 0;
mjuu_zerovec(CoM, 3); mjuu_zerovec(CoM, 3);
int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface(); int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface();
int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data();
float* vv = vert_.data(); float* vv = vert_.data();
for (int i=0; i < nf; i++) { for (int i=0; i < nf; i++) {
// get area, normal and center // get area, normal and center
@@ -1268,19 +1243,57 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type,
// compute and add volume // compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]}; 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 legacy computation requested, then always positive
if (inertia == mjINERTIA_LEGACY) { if (inertia == mjMESH_INERTIA_LEGACY) {
vol = abs(vol); vol = abs(vol);
} }
// add pyramid com // add pyramid com
GetVolumeRef(type) += vol; GetVolumeRef() += vol;
for (int j=0; j<3; j++) { for (int j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0); 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() { void mjCMesh::Process() {
double facecen[3] = {0, 0, 0};; double facecen[3] = {0, 0, 0};
// user offset, rotation, scaling // user offset, rotation, scaling
ApplyTransformations(); ApplyTransformations();
@@ -1409,124 +1422,102 @@ void mjCMesh::Process() {
double density = model->def_map[classname]->Geom().density; double density = model->def_map[classname]->Geom().density;
bool centered = false; // true if the mesh is centered at the CoM // compute inertia and transform mesh. The mesh is transformed such that it is
bool aligned_with_inertial_frame = false; // true if mesh is aligned with inertial frame // 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 // compute CoM and volume/area
// the mesh is transformed such that it is centered at the CoM and the axes if (inertia == mjMESH_INERTIA_SHELL) {
// are the principle axes of inertia. If the volume is valid, we use the volume ComputeSurfaceArea(CoM, facecen);
// inertia for this transformation, otherwise we use the shell inertia. if (!validarea_) {
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;
return; return;
} }
if (eigval[0] + eigval[1] < eigval[2] || } else {
eigval[0] + eigval[2] < eigval[1] || ComputeVolume(CoM, facecen);
eigval[1] + eigval[2] < eigval[0]) { if (validvolume_ != MeshVolumeOK) {
validinequality_ = false;
return; 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 nrm[3];
double cen[3]; double cen[3];
double density = model->def_map[classname]->Geom().density; double density = model->def_map[classname]->Geom().density;
// copy vertices to avoid modifying the original mesh
std::vector<float> 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 // accumulate products of inertia, recompute volume
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}}; 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}; double P[6] = {0, 0, 0, 0, 0, 0};
GetVolumeRef(type) = 0; GetVolumeRef() = 0;
int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface(); int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface();
int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data(); int* f = (inertia == mjMESH_INERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data();
for (int i=0; i < nf; i++) { for (int i=0; i < nf; i++) {
float* D = vert_.data()+3*f[3*i]; float* D = vert_centered.data()+3*f[3*i];
float* E = vert_.data()+3*f[3*i+1]; float* E = vert_centered.data()+3*f[3*i+1];
float* F = vert_.data()+3*f[3*i+2]; float* F = vert_centered.data()+3*f[3*i+2];
// get area, normal and center; update volume // get area, normal and center; update volume
double a = _triangle(nrm, cen, D, E, F); 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 legacy computation requested, then always positive
if (inertia == mjINERTIA_LEGACY) { if (inertia == mjMESH_INERTIA_LEGACY) {
vol = abs(vol); vol = abs(vol);
} }
// apply formula, accumulate // apply formula, accumulate
GetVolumeRef(type) += vol; GetVolumeRef() += vol;
for (int j=0; j<6; j++) { for (int j=0; j<6; j++) {
P[j] += density*vol / P[j] += density*vol /
(type==mjINERTIA_SHELL ? 12 : 20) * ( (inertia==mjMESH_INERTIA_SHELL ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] + 2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] + E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[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]) { 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 neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
double mat[9]; double mat[9];
mjuu_quat2mat(mat, neg); 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 // check that the mesh is valid
void mjCMesh::CheckMesh(mjtGeomInertia type) { void mjCMesh::CheckMesh() {
if (!processed_) { if (!processed_) {
return; 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, throw mjCError(this,
"faces of mesh '%s' have inconsistent orientation. Please check the " "faces of mesh '%s' have inconsistent orientation. Please check the "
"faces containing the vertices %d and %d.", "faces containing the vertices %d and %d.",
name.c_str(), invalidorientation_.first, invalidorientation_.second); 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()); 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()); throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
if (validvolume_==MeshZeroVolume && type==mjINERTIA_VOLUME) if (validvolume_==MeshZeroVolume && inertia!=mjMESH_INERTIA_SHELL)
throw mjCError(this, "mesh volume is too small: %s", name.c_str()); throw mjCError(this, "mesh volume is too small: %s . Try setting inertia to shell",
name.c_str());
if (!valideigenvalue_) if (!valideigenvalue_)
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str()); throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
if (!validinequality_) if (!validinequality_)
@@ -1599,15 +1605,14 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) {
// get inertia pointer // get inertia pointer
double* mjCMesh::GetInertiaBoxPtr(mjtGeomInertia type) { double* mjCMesh::GetInertiaBoxPtr() {
CheckMesh(type); return boxsz_;
return type==mjINERTIA_SHELL ? boxsz_surface_ : boxsz_volume_;
} }
double& mjCMesh::GetVolumeRef(mjtGeomInertia type) { double& mjCMesh::GetVolumeRef() {
CheckMesh(type); CheckMesh();
return type==mjINERTIA_SHELL ? surface_ : volume_; return inertia==mjMESH_INERTIA_SHELL ? surface_ : volume_;
} }
+5 -5
View File
@@ -2793,8 +2793,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_bvhnum[i] = pme->tree().Nbvh(); m->mesh_bvhnum[i] = pme->tree().Nbvh();
m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1; m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1;
mjuu_copyvec(&m->mesh_scale[3 * i], pme->Scale(), 3); mjuu_copyvec(&m->mesh_scale[3 * i], pme->Scale(), 3);
mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetPosPtr(), 3);
mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetQuatPtr(), 4);
// copy vertices, normals, faces, texcoords, aux data // copy vertices, normals, faces, texcoords, aux data
pme->CopyVert(m->mesh_vert + 3*vert_adr); 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 && if (geoms_[i]->mesh &&
(geoms_[i]->spec.type == mjGEOM_MESH || geoms_[i]->spec.type == mjGEOM_SDF) && (geoms_[i]->spec.type == mjGEOM_MESH || geoms_[i]->spec.type == mjGEOM_SDF) &&
(geoms_[i]->spec.contype || geoms_[i]->spec.conaffinity || (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); geoms_[i]->mesh->SetNeedHull(true);
} }
} }
@@ -4633,8 +4633,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCMesh* pm; mjCMesh* pm;
for (int i=0; i<nmesh; i++) { for (int i=0; i<nmesh; i++) {
pm = meshes_[i]; pm = meshes_[i];
mjuu_copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3); mjuu_copyvec(pm->GetPosPtr(), m->mesh_pos+3*i, 3);
mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); mjuu_copyvec(pm->GetQuatPtr(), m->mesh_quat+4*i, 4);
} }
// heightfield // heightfield
+8 -10
View File
@@ -2386,7 +2386,7 @@ double mjCGeom::GetVolume() const {
throw mjCError(this, "invalid mesh id in mesh geom"); 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) // 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"); 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[0] = mass_ * (boxsz[1] * boxsz[1] + boxsz[2] * boxsz[2]) / 3;
inertia[1] = mass_ * (boxsz[0] * boxsz[0] + 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; inertia[2] = mass_ * (boxsz[0] * boxsz[0] + boxsz[1] * boxsz[1]) / 3;
@@ -2551,6 +2551,7 @@ void mjCGeom::SetInertia(void) {
double radius = size[0]; double radius = size[0];
switch (typeinertia) { switch (typeinertia) {
case mjINERTIA_VOLUME: case mjINERTIA_VOLUME:
inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12; inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12;
inertia[2] = mass_ * radius * radius / 2; inertia[2] = mass_ * radius * radius / 2;
return; return;
@@ -3006,23 +3007,20 @@ void mjCGeom::Compile(void) {
mjCMesh* pmesh = mesh; mjCMesh* pmesh = mesh;
// fit geom if type is not mjGEOM_MESH // fit geom if type is not mjGEOM_MESH
double meshpos[3];
if (type != mjGEOM_MESH && type != mjGEOM_SDF) { if (type != mjGEOM_MESH && type != mjGEOM_SDF) {
double meshpos[3];
mesh->FitGeom(this, meshpos); mesh->FitGeom(this, meshpos);
// remove reference to mesh // remove reference to mesh
meshname_.clear(); meshname_.clear();
mesh = nullptr; mesh = nullptr;
} else { mjuu_copyvec(pmesh->GetPosPtr(), meshpos, 3);
// Retrieve the mesh position for the relevant inertia type. } else if (typeinertia == mjINERTIA_SHELL) {
mjuu_copyvec(meshpos, mesh->GetPosPtr(typeinertia), 3); throw mjCError(this, "for mesh geoms, inertia should be specified in the mesh asset");
} }
// apply geom pos/quat as offset // apply geom pos/quat as offset
mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia)); mjuu_frameaccum(pos, quat, pmesh->GetPosPtr(), pmesh->GetQuatPtr());
mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
// Retrieve the mesh quaternion for the relevant inertia type.
mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
} }
// check size parameters // check size parameters
+27 -31
View File
@@ -871,20 +871,16 @@ class mjCMesh_ : public mjCBase {
MeshNegativeVolume = -1, MeshNegativeVolume = -1,
MeshZeroVolume = 0, MeshZeroVolume = 0,
MeshVolumeOK = 1 MeshVolumeOK = 1
} validvolume_; // indicates if volume is valid } validvolume_; // indicates if volume is valid
bool valideigenvalue_; // false if inertia eigenvalue is too small bool valideigenvalue_; // are inertia eigenvalues positive
bool validinequality_; // false if inertia inequality is not satisfied bool validinequality_; // is inertia eigenvalue inequality satisfied
bool processed_; // false if the mesh has not been processed yet 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 // mesh properties computed by Compile
double pos_volume_[3]; // CoM position (volume) double pos_[3]; // CoM position
double pos_surface_[3]; // CoM position (surface) double quat_[4]; // inertia orientation
double quat_volume_[4]; // inertia orientation (volume) double boxsz_[3]; // half-sizes of equivalent inertia box
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 aamm_[6]; // axis-aligned bounding box in (min, max) format double aamm_[6]; // axis-aligned bounding box in (min, max) format
double volume_; // volume of the mesh double volume_; // volume of the mesh
double surface_; // surface of the mesh double surface_; // surface of the mesh
@@ -935,7 +931,6 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
const std::vector<int>& Face() const { return face_; } const std::vector<int>& Face() const { return face_; }
const std::vector<int>& UserFace() const { return spec_face_; } const std::vector<int>& UserFace() const { return spec_face_; }
mjtMeshInertia Inertia() const { return spec.inertia; } mjtMeshInertia Inertia() const { return spec.inertia; }
// setters // setters
void SetNeedHull(bool needhull) { needhull_ = needhull; } void SetNeedHull(bool needhull) { needhull_ = needhull; }
@@ -955,12 +950,10 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
const mjCBoundingVolumeHierarchy& tree() { return tree_; } const mjCBoundingVolumeHierarchy& tree() { return tree_; }
void Compile(const mjVFS* vfs); // compiler void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(mjtGeomInertia type); // get position double* GetPosPtr(); // get position
double* GetQuatPtr(mjtGeomInertia type); // get orientation double* GetQuatPtr(); // get orientation
double* GetOffsetPosPtr(); // get position offset for geom double* GetInertiaBoxPtr(); // get inertia box
double* GetOffsetQuatPtr(); // get orientation offset for geom double& GetVolumeRef(); // get volume
double* GetInertiaBoxPtr(mjtGeomInertia type); // get inertia box
double& GetVolumeRef(mjtGeomInertia type); // get volume
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
bool HasTexcoord() const; // texcoord not null bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord void DelTexcoord(); // delete texcoord
@@ -991,20 +984,21 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
void CacheMesh(mjCCache *cache, const mjResource* resource, void CacheMesh(mjCCache *cache, const mjResource* resource,
std::string_view asset_type); std::string_view asset_type);
void LoadSDF(); // generate mesh using marching cubes void LoadSDF(); // generate mesh using marching cubes
void MakeGraph(); // make graph of convex hull void MakeGraph(); // make graph of convex hull
void CopyGraph(); // copy graph into face data void CopyGraph(); // copy graph into face data
void MakeNormal(); // compute vertex normals void MakeNormal(); // compute vertex normals
void MakeCenter(); // compute face circumcircle data void MakeCenter(); // compute face circumcircle data
void Process(); // compute inertial properties void Process(); // compute inertial properties
void ApplyTransformations(); // apply user transformations void ApplyTransformations(); // apply user transformations
void ComputeFaceCentroid(double[3]); // compute centroid of all faces void ComputeFaceCentroid(double[3]); // compute centroid of all faces
void CheckMesh(mjtGeomInertia type); // check if the mesh is valid void CheckMesh(); // check if the mesh is valid
void CopyPlugin(); 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 // 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 // mesh data to be copied into mjModel
double* center_; // face circumcenter data (3*nface) double* center_; // face circumcenter data (3*nface)
@@ -1017,7 +1011,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
std::vector<face_vertices_type> num_face_vertices_; std::vector<face_vertices_type> num_face_vertices_;
// compute the volume and center-of-mass of the mesh given the face center // 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]);
}; };
+6 -5
View File
@@ -781,10 +781,11 @@ const mjMap meshtype_map[2] = {
// mesh inertia type // mesh inertia type
const mjMap meshinertia_map[3] = { const mjMap meshinertia_map[4] = {
{"convex", mjINERTIA_CONVEX}, {"convex", mjMESH_INERTIA_CONVEX},
{"legacy", mjINERTIA_LEGACY}, {"legacy", mjMESH_INERTIA_LEGACY},
{"exact", mjINERTIA_EXACT} {"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, "refpos", 3, mesh->refpos, text);
ReadAttr(elem, "refquat", 4, mesh->refquat, text); ReadAttr(elem, "refquat", 4, mesh->refquat, text);
ReadAttr(elem, "scale", 3, mesh->scale, 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; mesh->inertia = (mjtMeshInertia)n;
} }
+9 -6
View File
@@ -217,7 +217,7 @@ void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) {
WriteAttrTxt(elem, "content_type", mesh->ContentType()); WriteAttrTxt(elem, "content_type", mesh->ContentType());
WriteAttrTxt(elem, "file", mesh->File()); WriteAttrTxt(elem, "file", mesh->File());
if (mesh->Inertia() != def->Mesh().Inertia()) { 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 // write vertex data
@@ -422,14 +422,14 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, stri
mjCMesh* mesh = geom->mesh; mjCMesh* mesh = geom->mesh;
// write pos/quat if there is a difference // write pos/quat if there is a difference
if (!SameVector(geom->pos, mesh->GetPosPtr(geom->typeinertia), 3) || if (!SameVector(geom->pos, mesh->GetPosPtr(), 3) ||
!SameVector(geom->quat, mesh->GetQuatPtr(geom->typeinertia), 4)) { !SameVector(geom->quat, mesh->GetQuatPtr(), 4)) {
// recover geom pos/quat before mesh frame transformation // recover geom pos/quat before mesh frame transformation
double p[3], q[4]; double p[3], q[4];
mjuu_copyvec(p, geom->pos, 3); mjuu_copyvec(p, geom->pos, 3);
mjuu_copyvec(q, geom->quat, 4); mjuu_copyvec(q, geom->quat, 4);
mjuu_frameaccuminv(p, q, mesh->GetPosPtr(geom->typeinertia), mjuu_frameaccuminv(p, q, mesh->GetPosPtr(),
mesh->GetQuatPtr(geom->typeinertia)); mesh->GetQuatPtr());
// write // write
WriteAttr(elem, "pos", 3, p, unitq+1); 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); WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid); WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs); 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)) { if (mjuu_defined(geom->mass)) {
WriteAttr(elem, "mass", 1, &geom->mass_, &mass); WriteAttr(elem, "mass", 1, &geom->mass_, &mass);
} else { } else {
+2 -2
View File
@@ -1,11 +1,11 @@
<mujoco> <mujoco>
<compiler/> <compiler/>
<asset> <asset>
<mesh file="cube.obj" name="hollow_cube"/> <mesh file="cube.obj" name="hollow_cube" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<body> <body>
<geom type="mesh" mesh="hollow_cube" density="1" shellinertia="true"/> <geom type="mesh" mesh="hollow_cube" density="1"/>
</body> </body>
</worldbody> </worldbody>
</mujoco> </mujoco>
+2 -2
View File
@@ -1,8 +1,8 @@
<mujoco> <mujoco>
<asset> <asset>
<mesh file="torus.obj"/> <mesh file="torus.obj" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<geom type="mesh" mesh="torus" shellinertia="true"/> <geom type="mesh" mesh="torus"/>
</worldbody> </worldbody>
</mujoco> </mujoco>
+30
View File
@@ -444,6 +444,36 @@ TEST_F(MujocoTest, RecompileFails) {
mj_deleteSpec(spec); mj_deleteSpec(spec);
} }
TEST_F(PluginTest, ModifyShellInertiaFails) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6"
face="0 1 2 2 1 3" inertia="shell"/>
</asset>
<worldbody>
</worldbody>
</mujoco>
)";
std::array<char, 1000> 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 recompilation multiple files -----------------------
TEST_F(PluginTest, RecompileCompare) { TEST_F(PluginTest, RecompileCompare) {
mjtNum tol = 0; mjtNum tol = 0;
+149 -62
View File
@@ -653,32 +653,17 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
mj_deleteModel(model); 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<char, 1024> 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<float>::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) { TEST_F(MjCMeshTest, AreaTooSmall) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<asset> <asset>
<mesh name="example_mesh" <mesh name="example_mesh"
vertex="0 0 0 1e-8 0 0 0 1e-8 0 0 0 1e-8" vertex="0 0 0 1e-8 0 0 0 1e-8 0 0 0 1e-8"
face="2 0 3 0 1 3 1 2 3 0 2 1" /> face="2 0 3 0 1 3 1 2 3 0 2 1" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<body> <body>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/> <geom type="mesh" mesh="example_mesh"/>
</body> </body>
</worldbody> </worldbody>
</mujoco> </mujoco>
@@ -689,25 +674,6 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
} }
TEST_F(MjCMeshTest, AreaTooSmallAllowedWorld) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1e-8 0 0 0 1e-8 0 0 0 1e-8"
face="2 0 3 0 1 3 1 2 3 0 2 1" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, VolumeTooSmall) { TEST_F(MjCMeshTest, VolumeTooSmall) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -727,6 +693,65 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" mass="0"/>
</default>
</default>
<asset>
<mesh name="example_mesh"
vertex="0 -4e-16 0 1 0 4e-16 0 1 0 0 0 1"
face="0 2 1" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" class="visual"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" mass="0"/>
</default>
</default>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6"
face="0 1 2 2 1 3" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" class="visual"/>
</body>
</worldbody>
</mujoco>
)";
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) { TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
@@ -735,11 +760,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
<asset> <asset>
<mesh name="example_mesh" <mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6" vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6"
face="0 1 2 2 1 3" /> face="0 1 2 2 1 3" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<body> <body>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/> <geom type="mesh" mesh="example_mesh"/>
</body> </body>
</worldbody> </worldbody>
</mujoco> </mujoco>
@@ -797,25 +822,6 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
} }
} }
TEST_F(MjCMeshTest, VolumeTooSmallAllowedWorld) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
// ------------- test concave and shell inertia -------------------------------- // ------------- test concave and shell inertia --------------------------------
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon(); const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
@@ -893,18 +899,18 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << 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 // 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 // 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 // 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_pos[3];
mjtNum inverse_mesh_quat[4]; mjtNum inverse_mesh_quat[4];
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_pos[0], &model->mesh_quat[0]);
// Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values. // quat. It should match the originally specified values
mjtNum recovered_pos[3]; mjtNum recovered_pos[3];
mjtNum recovered_quat[4]; mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat, 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[2], 0, 1e-12);
EXPECT_NEAR(recovered_quat[3], 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"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1" inertia="shell"/>
</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>
)";
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, mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_pos[0], &model->mesh_quat[0]);
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat,
@@ -958,6 +1023,28 @@ TEST_F(MjCMeshTest, MeshScale) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, ShellInertiaTest) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1" inertia="shell"/>
<mesh name="pyramid_scaled" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="0.9 1 -1"/>
</asset>
<worldbody>
<geom type="mesh" name="geom1" mesh="pyramid"/>
<geom type="mesh" name="geom2" mesh="pyramid_scaled"/>
</worldbody>
</mujoco>
)";
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 ------------------------------------- // ----------------------------- texcoord -------------------------------------
TEST_F(MjCMeshTest, CreateFaceTexCoord) { TEST_F(MjCMeshTest, CreateFaceTexCoord) {
+1 -1
View File
@@ -1141,7 +1141,7 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) {
<asset> <asset>
<mesh name="bad_mesh" <mesh name="bad_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1" /> face="0 2 1" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<body> <body>
+2 -3
View File
@@ -1728,10 +1728,10 @@ TEST_F(XMLReaderTest, ReadShellParameter) {
<asset> <asset>
<mesh name="example_mesh" <mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 2 0 3" /> face="0 2 1 2 0 3" inertia="shell"/>
</asset> </asset>
<worldbody> <worldbody>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/> <geom type="mesh" mesh="example_mesh"/>
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
@@ -1741,7 +1741,6 @@ TEST_F(XMLReaderTest, ReadShellParameter) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(XMLReaderTest, ReadsSkinGroups) { TEST_F(XMLReaderTest, ReadsSkinGroups) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
-1
View File
@@ -1323,7 +1323,6 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
absl::StrContains(p.path().string(), "spheremesh")) { absl::StrContains(p.path().string(), "spheremesh")) {
continue; continue;
} }
// load model // load model
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* m = mj_loadXML( mjModel* m = mj_loadXML(
+4 -3
View File
@@ -458,9 +458,10 @@ public enum mjtGeomInertia : int{
mjINERTIA_SHELL = 1, mjINERTIA_SHELL = 1,
} }
public enum mjtMeshInertia : int{ public enum mjtMeshInertia : int{
mjINERTIA_CONVEX = 0, mjMESH_INERTIA_CONVEX = 0,
mjINERTIA_EXACT = 1, mjMESH_INERTIA_EXACT = 1,
mjINERTIA_LEGACY = 2, mjMESH_INERTIA_LEGACY = 2,
mjMESH_INERTIA_SHELL = 3,
} }
public enum mjtBuiltin : int{ public enum mjtBuiltin : int{
mjBUILTIN_NONE = 0, mjBUILTIN_NONE = 0,