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

Changes to inertia inference from meshes.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+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:
: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<compiler-inertiafromgeom>`. 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<asset-mesh-inertia>` attribute to :at-val:`"shell"`.
.. _body-geom-solmix:
doc/changelog.rst
+12
View File
@@ -34,6 +34,18 @@ General
- Added :ref:`potential<sensor-e_potential>` and :ref:`kinetic<sensor-e_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<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
^^^
- Added support for spatial tendons with internal sphere and cylinder wrapping.
doc/includes/references.h
+6 -5
View File
@@ -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
include/mujoco/mjspec.h
+6 -5
View File
@@ -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
introspect/enums.py
+4 -3
View File
@@ -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',
introspect/structs.py
+1 -1
View File
@@ -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',
src/user/user_init.c
+1 -1
View File
@@ -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;
}
src/user/user_mesh.cc
+150 -145
View File
@@ -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<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
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_;
}
src/user/user_model.cc
+5 -5
View File
@@ -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; i<nmesh; i++) {
pm = meshes_[i];
mjuu_copyvec(pm->GetOffsetPosPtr(), 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
src/user/user_objects.cc
+8 -10
View File
@@ -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
src/user/user_objects.h
+27 -31
View File
@@ -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<int>& Face() const { return face_; }
const std::vector<int>& 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<face_vertices_type> 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]);
};
src/xml/xml_native_reader.cc
+6 -5
View File
@@ -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;
}
src/xml/xml_native_writer.cc
+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, "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 {
test/user/testdata/inertia_shell.xml
Vendored
+2 -2
View File
@@ -1,11 +1,11 @@
<mujoco>
<compiler/>
<asset>
<mesh file="cube.obj" name="hollow_cube"/>
<mesh file="cube.obj" name="hollow_cube" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="hollow_cube" density="1" shellinertia="true"/>
<geom type="mesh" mesh="hollow_cube" density="1"/>
</body>
</worldbody>
</mujoco>
test/user/testdata/torus_shell.xml
Vendored
+2 -2
View File
@@ -1,8 +1,8 @@
<mujoco>
<asset>
<mesh file="torus.obj"/>
<mesh file="torus.obj" inertia="shell"/>
</asset>
<worldbody>
<geom type="mesh" mesh="torus" shellinertia="true"/>
<geom type="mesh" mesh="torus"/>
</worldbody>
</mujoco>
test/user/user_api_test.cc
+30
View File
@@ -444,6 +444,36 @@ TEST_F(MujocoTest, RecompileFails) {
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_F(PluginTest, RecompileCompare) {
mjtNum tol = 0;
test/user/user_mesh_test.cc
+149 -62
View File
@@ -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<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) {
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" />
face="2 0 3 0 1 3 1 2 3 0 2 1" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
@@ -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"(
<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) {
static constexpr char xml[] = R"(
<mujoco>
@@ -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"(
<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) {
@@ -735,11 +760,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
<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" />
face="0 1 2 2 1 3" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</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 --------------------------------
const mjtNum max_abs_err = std::numeric_limits<float>::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"(
<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,
&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"(
<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 -------------------------------------
TEST_F(MjCMeshTest, CreateFaceTexCoord) {
test/user/user_objects_test.cc
+1 -1
View File
@@ -1141,7 +1141,7 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) {
<asset>
<mesh name="bad_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1" />
face="0 2 1" inertia="shell"/>
</asset>
<worldbody>
<body>
test/xml/xml_native_reader_test.cc
+2 -3
View File
@@ -1728,10 +1728,10 @@ TEST_F(XMLReaderTest, ReadShellParameter) {
<asset>
<mesh name="example_mesh"
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>
<worldbody>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
@@ -1741,7 +1741,6 @@ TEST_F(XMLReaderTest, ReadShellParameter) {
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, ReadsSkinGroups) {
static constexpr char xml[] = R"(
<mujoco>
test/xml/xml_native_writer_test.cc
-1
View File
@@ -1323,7 +1323,6 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
absl::StrContains(p.path().string(), "spheremesh")) {
continue;
}
// load model
std::array<char, 1000> error;
mjModel* m = mj_loadXML(
unity/Runtime/Bindings/MjBindings.cs
+4 -3
View File
@@ -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,