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

Compute moment of inertia for concave and boundary meshes. Resolve #338.

Browse Source 
- Mesh inertias can now be computed exactly for well-formed (no holes) non-convex meshes.
- To activate this feature, set `<compiler exactmeshinertia="true">` (defaults to `false`). This default may change in the future.
- Added `<geom shellinertia="true/false">` (defaults to `false`). When true, geom inertia is computed assuming all the mass is concentrated on the surface, and `density` is interpreted as surface density (mass/area). Currently only mesh geoms are supported.

PiperOrigin-RevId: 464368395
Change-Id: I17afd99b9b221c9d24ae951f6e63d5c61ff89820
This commit is contained in:
Alessio Quaglino
2022-07-31 04:31:46 -07:00
committed by Copybara-Service
parent 52d78f8d24
commit 5c5449bf82
19 changed files with 37807 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+14 -13
View File
@@ -12,10 +12,9 @@ This chapter is the reference manual for the MJCF modeling language used in MuJo
XML schema
~~~~~~~~~~
The table below summarizes the XML elements and their attributes in MJCF. It is generated automatically with the
function :ref:`mj_printSchema` which prints out the custom schema used by the parser to validate the model file.
Note that all information in MJCF is entered through elements and attributes. Text content in elements is not used;
if present, the parser ignores it. The symbols in the second column of the table have the following meaning:
The table below summarizes the XML elements and their attributes in MJCF. Note that all information in MJCF is entered
through elements and attributes. Text content in elements is not used; if present, the parser ignores it. The symbols
in the second column of the table have the following meaning:
====== ===================================================
**!** required element, can appear only once
@@ -46,7 +45,7 @@ if present, the parser ignores it. The symbols in the second column of the table
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`convexhull` | :at:`usethread` | :at:`fusestatic` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`inertiafromgeom` | :at:`inertiagrouprange` | | |
| | | | :at:`inertiafromgeom` | :at:`inertiagrouprange` | :at:`exactmeshinertia` | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`lengthrange` | ? | .. table:: |
@@ -274,6 +273,8 @@ if present, the parser ignores it. The symbols in the second column of the table
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`user` | :at:`fluidshape` | :at:`fluidcoef` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`shellinertia` | | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`site` | ? | .. table:: |
| | | :class: mjcf-attributes |
@@ -633,7 +634,7 @@ if present, the parser ignores it. The symbols in the second column of the table
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`fitscale` | :at:`rgba` | :at:`user` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`fluidshape` | :at:`fluidcoef` | | |
| | | | :at:`fluidshape` | :at:`fluidcoef` | :at:`shellinertia` | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`site` | \* | .. table:: |
@@ -1612,6 +1613,9 @@ any effect. The settings here are global and apply to the entire model.
particular, a number of publicly available URDF models have seemingly arbitrary inertias which are too large compared
to the mass. This results in equivalent inertia boxes which extend far beyond the geometric boundaries of the model.
Note that the built-in OpenGL visualizer can render equivalent inertia boxes.
:at:`exactmeshinertia`: :at-val:`[false, true], "false"`
If this attribute is set to false, computes mesh inertia with the legacy algorithm, which is exact only for convex
meshes. If set to true, it is exact for any closed mesh geometry.
:at:`inertiagrouprange`: :at-val:`int(2), "0 5"`
This attribute specifies the range of geom groups that are used to infer body masses and inertias (when such
inference is enabled). The group attribute of :ref:`geom <geom>` is an integer. If this integer falls in the range
@@ -2722,7 +2726,7 @@ MSH file format
Poorly designed meshes can display rendering artifacts. In particular, the shadow mapping mechanism relies on having
some distance between front and back-facing triangle faces. If the faces are repeated, with opposite normals as
determined by the vertex order in each triangle, this causes shadow aliasing. The solution is to remove the repeated
faces (which can be done in MeshLab) or use a better designed mesh.
faces (which can be done in MeshLab) or use a better designed mesh. Flipped faces are checked by MuJoCo for meshes specified as OBJ or XML and an error message is returned.
The size of the mesh is determined by the 3D coordinates of the vertex data in the mesh file, multiplied by the
components of the :at:`scale` attribute below. Scaling is applied separately for each coordinate axis. Note that
@@ -2767,12 +2771,7 @@ practice this is rarely needed.
The inertial computation mentioned above is part of an algorithm used not only to center and align the mesh, but also
to infer the mass and inertia of the body to which it is attached. This is done by computing the centroid of the
triangle faces, connecting each face with the centroid to form a triangular pyramid, computing the mass and inertia of
all pyramids and accumulating them. This algorithm comes from Astronomy where it is used to estimate inertial
properties of asteroids. It is exact for convex meshes but is not always exact for non-convex meshes; indeed no
algorithm can be exact when the notion of interior is ill-defined. Thus for non-convex models designed in CAD software
(which usually knows what the interior is) it is better to ask that software to compute the inertial properties of the
body and enter them in the MJCF file explicitly via the :ref:`inertial <inertial>` element.
triangle faces, connecting each face with the centroid to form a triangular pyramid, computing the mass and signed inertia of all pyramids (considered solid or hollow if :at:`shellinertia` is true) and accumulating them. The sign ensures that pyramids on the outside of the surfaces are subtracted, as it can occur with concave geometries. This algorithm can be found in section 1.3.8 of Computational Geometry in C (Second Edition) by Joseph O'Rourke.
The full list of processing steps applied by the compiler to each mesh is as follows:
@@ -3390,6 +3389,8 @@ helps clarify the role of bodies and geoms in MuJoCo.
Material density used to compute the geom mass and inertia. The computation is based on the geom shape and the
assumption of uniform density. The internal default of 1000 is the density of water in SI units. This attribute is
used only when the mass attribute above is unspecified.
: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.
:at:`solmix`: :at-val:`real, "1"`
This attribute specifies the weight used for averaging of contact parameters, and interacts with the priority
attribute. See :ref:`CContact`.
doc/changelog.rst
+12 -1
View File
@@ -5,7 +5,18 @@ Changelog
Upcoming version (not yet released)
-----------------------------------
- Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
General
^^^^^^^
- Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
- Added moment of inertia computation for concave meshes. This is a breaking change, to get back to the previous
behavior set the compiler flag :at:`exactmeshinertia` to false.
- Added parameter :at:`shellinertia` in :at:`geom` for treating a mesh as a boundary mesh (shell) for inertia
computations. This is currently supported only for meshes.
- Raise error if the orientation of mesh faces is not consistent, which causes the inertia computations to be
inaccurate. If this occurs, open the mesh in MeshLab or Blender and recalculate the faces.
Bug fixes
^^^^^^^^^
Version 2.2.1 (July 18, 2022)
-----------------------------
src/user/user_mesh.cc
+272 -233
View File
@@ -65,9 +65,12 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
useredge.clear();
// clear internal variables
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(quat, 1, 0, 0, 0);
mjuu_setvec(boxsz, 0, 0, 0);
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(boxsz_surface, 0, 0, 0);
mjuu_setvec(boxsz_volume, 0, 0, 0);
mjuu_setvec(aabb, 0, 0, 0);
nvert = 0;
nface = 0;
@@ -251,20 +254,23 @@ void mjCMesh::Compile(const mjVFS* vfs) {
// get position
void mjCMesh::GetPos(double* _pos) {
_pos[0] = pos[0];
_pos[1] = pos[1];
_pos[2] = pos[2];
double* mjCMesh::GetPosPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return pos_surface;
} else {
return pos_volume;
}
}
// get orientation
void mjCMesh::GetQuat(double* _quat) {
_quat[0] = quat[0];
_quat[1] = quat[1];
_quat[2] = quat[2];
_quat[3] = quat[3];
double* mjCMesh::GetQuatPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return quat_surface;
} else {
return quat_volume;
}
}
@@ -275,7 +281,8 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// use inertial box
if (!model->fitaabb) {
// compute depending on type
// get inertia box type (shell or volume)
double* boxsz = GetInertiaBoxPtr(geom->typeinertia);
switch (geom->type) {
case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3;
@@ -303,7 +310,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
}
// copy mesh pos into meshpos
mjuu_copyvec(meshpos, pos, 3);
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
}
// use AABB
@@ -860,252 +867,284 @@ static double _areaNrmCen(double* normal, double* center,
// apply transformations
void mjCMesh::Process(void) {
double CoM[3] = {0, 0, 0};
double facecen[3] = {0, 0, 0};
double area = 0;
double inert[6] = {0, 0, 0, 0, 0, 0};
double volume;
void mjCMesh::Process() {
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
double CoM[3] = {0, 0, 0};
double facecen[3] = {0, 0, 0};
double area = 0;
double inert[6] = {0, 0, 0, 0, 0, 0};
int i, j;
double nrm[3];
double cen[3];
int i, j;
double nrm[3];
double cen[3];
// translate
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
if (type==mjVOLUME_MESH) {
// translate
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
// prepare translation
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
// process vertices
for (i=0; i<nvert; i++) {
// positions
vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
// process vertices
for (i=0; i<nvert; i++) {
// positions
vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
// normals not affected by translation
}
}
// normals not affected by translation
}
}
// rotate
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
// rotate
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
// process vertices
for (i=0; i<nvert; i++) {
// positions
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
mju_rotVecMatT(p1, p0, mat);
vert[3*i] = (float) p1[0];
vert[3*i+1] = (float) p1[1];
vert[3*i+2] = (float) p1[2];
// process vertices
for (i=0; i<nvert; i++) {
// positions
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
mju_rotVecMatT(p1, p0, mat);
vert[3*i] = (float) p1[0];
vert[3*i+1] = (float) p1[1];
vert[3*i+2] = (float) p1[2];
// nromals
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mju_rotVecMatT(n1, n0, mat);
normal[3*i] = (float) n1[0];
normal[3*i+1] = (float) n1[1];
normal[3*i+2] = (float) n1[2];
}
}
// nromals
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mju_rotVecMatT(n1, n0, mat);
normal[3*i] = (float) n1[0];
normal[3*i+1] = (float) n1[1];
normal[3*i+2] = (float) n1[2];
}
}
// scale
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (i=0; i<nvert; i++) {
// positions
vert[3*i] *= scale[0];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
// scale
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (i=0; i<nvert; i++) {
// positions
vert[3*i] *= scale[0];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
// normals
normal[3*i] *= scale[0];
normal[3*i+1] *= scale[1];
normal[3*i+2] *= scale[2];
}
}
// normals
normal[3*i] *= scale[0];
normal[3*i+1] *= scale[1];
normal[3*i+2] *= scale[2];
}
}
// normalize normals
for (i=0; i<nvert; i++) {
// compute length
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
// normalize normals
for (i=0; i<nvert; i++) {
// compute length
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
// rescale
if (len>mjMINVAL) {
float scl = 1/sqrtf(len);
normal[3*i] *= scl;
normal[3*i+1] *= scl;
normal[3*i+2] *= scl;
} else {
normal[3*i] = 0;
normal[3*i+1] = 0;
normal[3*i+2] = 1;
}
}
// rescale
if (len>mjMINVAL) {
float scl = 1/sqrtf(len);
normal[3*i] *= scl;
normal[3*i+1] *= scl;
normal[3*i+2] *= scl;
} else {
normal[3*i] = 0;
normal[3*i+1] = 0;
normal[3*i+2] = 1;
}
}
// find centroid of faces
for (i=0; i<nface; i++) {
// check vertex indices
for (j=0; j<3; j++) {
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
// find centroid of faces
for (i=0; i<nface; i++) {
// check vertex indices
for (j=0; j<3; j++) {
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
}
}
// get area and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// require positive area
if (area < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
// finalize centroid of faces
for (j=0; j<3; j++) {
facecen[j] /= area;
}
}
// get area and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// compute CoM and volume from pyramid volumes
GetVolumeRef(type) = 0;
for (i=0; i<nface; i++) {
// get area, normal and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
GetVolumeRef(type) += vol;
// require positive area
if (area < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
// finalize centroid of faces
for (j=0; j<3; j++) {
facecen[j] /= area;
}
// compute CoM and volume from pyramid volumes
volume = 0;
for (i=0; i<nface; i++) {
// get area, normal and center
double a = _areaNrmCen(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = fabs(mjuu_dot3(vec, nrm)) * a / 3;
volume += vol;
// add pyramid com
for (j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
}
}
// require positive volume
if (volume < mjMINVAL) {
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
}
// finalize CoM, save as mesh center
for (j=0; j<3; j++) {
CoM[j] /= volume;
}
mjuu_copyvec(pos, CoM, 3);
// re-center mesh at CoM
for (i=0; i<nvert; i++) {
for (j=0; j<3; j++) {
vert[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};
volume = 0;
for (i=0; i<nface; i++) {
float* D = vert+3*face[3*i];
float* E = vert+3*face[3*i+1];
float* F = vert+3*face[3*i+2];
// get area, normal and center; update volume
double a = _areaNrmCen(nrm, cen, D, E, F);
double vol = fabs(mjuu_dot3(cen, nrm)) * a / 3;
volume += vol;
// apply formula, accumulate
for (j=0; j<6; j++) {
P[j] += def->geom.density*vol/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]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
// get quaternion and diagonal inertia
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
inert[0], inert[3], inert[4],
inert[3], inert[1], inert[5],
inert[4], inert[5], inert[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
}
if (eigval[0] + eigval[1] < eigval[2] ||
eigval[0] + eigval[2] < eigval[1] ||
eigval[1] + eigval[2] < eigval[0]) {
throw mjCError(this,
"eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str());
}
// compute sizes of equivalent inertia box
double mass = volume * def->geom.density;
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;
// copy quat
for (j=0; j<4; j++) {
quat[j] = quattmp[j];
}
// rotate vertices and normals into axis-aligned frame
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (i=0; i<nvert; i++) {
// vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (j=0; j<3; j++) {
vert[3*i+j] = (float) res[j];
// add pyramid com
for (j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
}
}
// normals
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjuu_mulvecmat(res, nrm, mat);
for (j=0; j<3; j++) {
normal[3*i+j] = (float) res[j];
// require positive volume
if (GetVolumeRef(type) < mjMINVAL) {
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
}
}
// compute axis-aligned bounding box
for (i=0; i<nvert; i++) {
float* v = vert+3*i;
// finalize CoM, save as mesh center
for (j=0; j<3; j++) {
aabb[j] = mjMAX(aabb[j], fabs(v[j]));
CoM[j] /= GetVolumeRef(type);
}
mjuu_copyvec(GetPosPtr(type), CoM, 3);
// re-center mesh at CoM
if (type==mjVOLUME_MESH) {
for (i=0; i<nvert; i++) {
for (j=0; j<3; j++) {
vert[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;
for (i=0; i<nface; i++) {
float* D = vert+3*face[3*i];
float* E = vert+3*face[3*i+1];
float* F = vert+3*face[3*i+2];
// get area, normal and center; update volume
double a = _areaNrmCen(nrm, cen, D, E, F);
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!model->exactmeshinertia) {
vol = fabs(vol);
}
// apply formula, accumulate
GetVolumeRef(type) += vol;
for (j=0; j<6; j++) {
P[j] += def->geom.density*vol /
(type==mjSHELL_MESH ? 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]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
// get quaternion and diagonal inertia
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
inert[0], inert[3], inert[4],
inert[3], inert[1], inert[5],
inert[4], inert[5], inert[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
}
if (eigval[0] + eigval[1] < eigval[2] ||
eigval[0] + eigval[2] < eigval[1] ||
eigval[1] + eigval[2] < eigval[0]) {
throw mjCError(this,
"eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str());
}
// compute sizes of equivalent inertia box
double mass = GetVolumeRef(type) * def->geom.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;
// copy quat
for (j=0; j<4; j++) {
GetQuatPtr(type)[j] = quattmp[j];
}
// rotate vertices and normals into axis-aligned frame
if (type==mjVOLUME_MESH) {
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (i=0; i<nvert; i++) {
// vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (j=0; j<3; j++) {
vert[3*i+j] = (float) res[j];
}
// normals
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjuu_mulvecmat(res, nrm, mat);
for (j=0; j<3; j++) {
normal[3*i+j] = (float) res[j];
}
}
// compute axis-aligned bounding box
for (i=0; i<nvert; i++) {
float* v = vert+3*i;
for (j=0; j<3; j++) {
aabb[j] = mjMAX(aabb[j], fabs(v[j]));
}
}
}
}
}
// compute inertia
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
return boxsz_surface;
} else {
return boxsz_volume;
}
}
double& mjCMesh::GetVolumeRef(mjtMeshType type) {
if (type) {
return surface;
} else {
return volume;
}
}
// make graph describing convex hull
void mjCMesh::MakeGraph(void) {
src/user/user_model.cc
+1
View File
@@ -99,6 +99,7 @@ mjCModel::mjCModel() {
inertiafromgeom = mjINERTIAFROMGEOM_AUTO;
inertiagrouprange[0] = 0;
inertiagrouprange[1] = mjNGROUP-1;
exactmeshinertia = false;
mj_defaultLROpt(&LRopt);
//------------------------ statistics override
src/user/user_model.h
+1
View File
@@ -124,6 +124,7 @@ class mjCModel {
bool fusestatic; // fuse static bodies with parent
int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom)
int inertiagrouprange[2]; // range of geom groups used to compute inertia
bool exactmeshinertia; // if false, use old formula
mjLROpt LRopt; // options for lengthrange computation
//------------------------ statistics override (if defined)
src/user/user_objects.cc
+16 -7
View File
@@ -967,6 +967,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
userdata.clear();
typeinertia = mjVOLUME_MESH;
// clear internal variables
mjuu_setvec(quat, 1, 0, 0, 0);
@@ -1003,7 +1004,11 @@ double mjCGeom::GetVolume(void) {
}
mjCMesh* pmesh = model->meshes[meshid];
return pmesh->boxsz[0]*pmesh->boxsz[1]*pmesh->boxsz[2]*8;
if (model->exactmeshinertia) {
return pmesh->GetVolumeRef(typeinertia);
} else {
return pmesh->boxsz_volume[0]*pmesh->boxsz_volume[1]*pmesh->boxsz_volume[2]*8;
}
}
// compute from geom shape
@@ -1045,13 +1050,17 @@ void mjCGeom::SetInertia(void) {
}
mjCMesh* pmesh = model->meshes[meshid];
inertia[0] = mass*(pmesh->boxsz[1]*pmesh->boxsz[1] + pmesh->boxsz[2]*pmesh->boxsz[2]) / 3;
inertia[1] = mass*(pmesh->boxsz[0]*pmesh->boxsz[0] + pmesh->boxsz[2]*pmesh->boxsz[2]) / 3;
inertia[2] = mass*(pmesh->boxsz[0]*pmesh->boxsz[0] + pmesh->boxsz[1]*pmesh->boxsz[1]) / 3;
double* boxsz = pmesh->GetInertiaBoxPtr(typeinertia);
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;
}
// compute from geom shape
else {
if (typeinertia)
throw mjCError(this, "typeinertia currently only available for meshes'%s' (id = %d)",
name.c_str(), id);
switch (type) {
case mjGEOM_SPHERE:
inertia[0] = inertia[1] = inertia[2] = 2*mass*size[0]*size[0]/5;
@@ -1068,7 +1077,7 @@ void mjCGeom::SetInertia(void) {
// add two hemispheres, displace along third axis
double sphere_inertia = 2*sphere_mass*radius*radius/5;
inertia[0] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;
inertia[1] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;;
inertia[1] += sphere_inertia + sphere_mass*height*(3*radius + 2*height)/8;
inertia[2] += sphere_inertia;
return;
}
@@ -1354,11 +1363,11 @@ void mjCGeom::Compile(void) {
mesh.clear();
meshid = -1;
} else {
mjuu_copyvec(meshpos, pmesh->pos, 3);
mjuu_copyvec(meshpos, pmesh->GetPosPtr(typeinertia), 3);
}
// apply geom pos/quat as offset
mjuu_frameaccum(pos, quat, meshpos, pmesh->quat);
mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
}
// check size parameters
src/user/user_objects.h
+23 -7
View File
@@ -76,6 +76,13 @@ typedef enum _mjtMark {
} mjtMark;
// type of mesh
typedef enum _mjtMeshType {
mjVOLUME_MESH,
mjSHELL_MESH,
} mjtMeshType;
// error information
class mjCError {
public:
@@ -298,6 +305,7 @@ class mjCGeom : public mjCBase {
std::string material; // name of material used for rendering
std::vector<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
mjtMeshType typeinertia; // selects between surface and volume inertia
// variables set by user and used during compilation
double _mass; // used to compute density
@@ -443,9 +451,11 @@ class mjCMesh: public mjCBase {
friend class mjXWriter;
public:
void GetPos(double* pos); // get position
void GetQuat(double* quat); // get orientation
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
double* GetPosPtr(mjtMeshType type); // get position
double* GetQuatPtr(mjtMeshType type); // get orientation
double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box
double& GetVolumeRef(mjtMeshType type); // get volume
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
std::string file; // mesh file
double refpos[3]; // reference position (translate)
@@ -469,14 +479,20 @@ class mjCMesh: public mjCBase {
void MakeGraph(void); // make graph of convex hull
void CopyGraph(void); // copy graph into face data
void MakeNormal(void); // compute vertex normals
void Process(void); // apply transformations
void Process(); // apply transformations
void RemoveRepeated(void); // remove repeated vertices
void ComputeInertia(mjtMeshType type); // compute inertia
// mesh properties computed by Compile
double pos[3]; // CoM position
double quat[4]; // inertia orientation
double boxsz[3]; // half-sizes of equivalent inertia box
double pos_volume[3]; // CoM position
double pos_surface[3]; // CoM position
double quat_volume[4]; // inertia orientation
double quat_surface[4]; // inertia orientation
double boxsz_volume[3]; // half-sizes of equivalent inertia box (volume)
double boxsz_surface[3]; // half-sizes of equivalent inertia box (surface)
double aabb[3]; // half-sizes of axis-aligned bounding box
double volume; // volume of the mesh
double surface; // surface of the mesh
// mesh data to be copied into mjModel
int nvert; // number of vertices
src/xml/xml_base.h
+1
View File
@@ -68,6 +68,7 @@ extern const mjMap gain_map[];
extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap meshtype_map[];
//---------------------------------- Base XML class ------------------------------------------------
src/xml/xml_native_reader.cc
+22 -7
View File
@@ -46,10 +46,10 @@ static const int nMJCF = 163;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
{"compiler", "*", "17", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
{"compiler", "*", "18", "boundmass", "boundinertia", "settotalmass", "balanceinertia",
"strippath", "coordinate", "angle", "fitaabb", "eulerseq",
"meshdir", "texturedir", "discardvisual", "convexhull", "usethread",
"fusestatic", "inertiafromgeom", "inertiagrouprange"},
"fusestatic", "inertiafromgeom", "inertiagrouprange", "exactmeshinertia"},
{"<"},
{"lengthrange", "?", "10", "mode", "useexisting", "uselimit",
"accel", "maxforce", "timeconst", "timestep",
@@ -103,9 +103,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"limited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "margin",
"ref", "springref", "armature", "damping", "frictionloss", "user"},
{"geom", "?", "30", "type", "pos", "quat", "contype", "conaffinity", "condim",
{"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"solmix", "solref", "solimp",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "?", "13", "type", "group", "pos", "quat", "material",
@@ -160,7 +160,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"asset", "*", "0"},
{"<"},
{"texture", "*", "21", "name", "type", "file", "gridsize", "gridlayout",
"fileright", "fileleft", "fileup", "filedown","filefront", "fileback",
"fileright", "fileleft", "fileup", "filedown", "filefront", "fileback",
"builtin", "rgb1", "rgb2", "mark", "markrgb", "random", "width", "height",
"hflip", "vflip"},
{"hfield", "*", "5", "name", "file", "nrow", "ncol", "size"},
@@ -186,9 +186,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"stiffness", "range", "margin", "ref", "springref", "armature", "damping",
"frictionloss", "user"},
{"freejoint", "*", "2", "name", "group"},
{"geom", "*", "32", "name", "class", "type", "contype", "conaffinity", "condim",
{"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"solmix", "solref", "solimp",
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
@@ -608,6 +608,13 @@ const mjMap tkind_map[2] = {
};
// mesh type
const mjMap meshtype_map[2] = {
{"false", mjVOLUME_MESH},
{"true", mjSHELL_MESH},
};
//---------------------------------- class mjXReader implementation --------------------------------
@@ -793,6 +800,9 @@ void mjXReader::Compiler(XMLElement* section, mjCModel* mod) {
}
MapValue(section, "inertiafromgeom", &mod->inertiafromgeom, TFAuto_map, 3);
ReadAttr(section, "inertiagrouprange", 2, mod->inertiagrouprange, text);
if (MapValue(section, "exactmeshinertia", &n, bool_map, 2)){
mod->exactmeshinertia = (n==1);
}
// lengthrange subelement
XMLElement* elem = FindSubElem(section, "lengthrange");
@@ -1149,6 +1159,11 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
ReadAttr(elem, "quat", 4, pgeom->quat, text);
ReadAlternative(elem, pgeom->alt);
// compute inertia using either solid or shell geometry
if (MapValue(elem, "shellinertia", &n, meshtype_map, 2)) {
pgeom->typeinertia = (mjtMeshType)n;
}
GetXMLPos(elem, pgeom);
}
src/xml/xml_native_writer.cc
+6 -3
View File
@@ -253,13 +253,14 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
mjCMesh* pmesh = model->meshes[pgeom->meshid];
// write pos/quat if there is a difference
if (!SameVector(pgeom->locpos, pmesh->pos, 3) ||
!SameVector(pgeom->locquat, pmesh->quat, 4)) {
if (!SameVector(pgeom->locpos, pmesh->GetPosPtr(pgeom->typeinertia), 3) ||
!SameVector(pgeom->locquat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) {
// recover geom pos/quat before mesh frame transformation
double p[3], q[4];
mjuu_copyvec(p, pgeom->locpos, 3);
mjuu_copyvec(q, pgeom->locquat, 4);
mjuu_frameaccuminv(p, q, pmesh->pos, pmesh->quat);
mjuu_frameaccuminv(p, q, pmesh->GetPosPtr(pgeom->typeinertia),
pmesh->GetQuatPtr(pgeom->typeinertia));
// write
WriteAttr(elem, "pos", 3, p, unitq+1);
@@ -292,6 +293,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_switch, def->geom.fluid_switch);
WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
if (mjuu_defined(pgeom->_mass)) {
double mass = pgeom->GetVolume() * def->geom.density;
WriteAttr(elem, "mass", 1, &pgeom->mass, &mass);
@@ -657,6 +659,7 @@ void mjXWriter::Compiler(XMLElement* root) {
if (!model->usethread) {
WriteAttrTxt(section, "usethread", "false");
}
WriteAttrTxt(section, "exactmeshinertia", FindValue(bool_map, 2, model->exactmeshinertia));
}
src/xml/xml_util.h
+1 -1
View File
@@ -46,7 +46,7 @@ class mjXError {
// max number of attribute fields in schema (plus 3)
#define mjXATTRNUM 35
#define mjXATTRNUM 36
// Custom XML file validation
test/user/testdata/cube_cup.obj
Vendored
+83
View File
@@ -0,0 +1,83 @@
v -0.050000 0.050000 0.100000
v -0.050000 0.050000 0.000000
v -0.050000 -0.050000 0.000000
v -0.050000 -0.050000 0.100000
v -0.050000 -0.050000 0.100000
v -0.050000 -0.050000 0.000000
v 0.050000 -0.050000 0.000000
v 0.050000 -0.050000 0.100000
v 0.050000 -0.050000 0.100000
v 0.050000 -0.050000 0.000000
v 0.050000 0.050000 0.000000
v 0.050000 0.050000 0.100000
v 0.050000 0.050000 0.100000
v 0.050000 0.050000 0.000000
v -0.050000 0.050000 0.000000
v -0.050000 0.050000 0.100000
v 0.050000 0.050000 0.000000
v 0.050000 -0.050000 0.000000
v -0.050000 -0.050000 0.000000
v -0.050000 0.050000 0.000000
v -0.050000 0.050000 0.100000
v -0.050000 -0.050000 0.100000
v -0.040000 -0.040000 0.100000
v 0.050000 -0.050000 0.100000
v 0.040000 -0.040000 0.100000
v 0.050000 0.050000 0.100000
v 0.040000 0.040000 0.100000
v -0.040000 0.040000 0.100000
v 0.040000 -0.040000 0.100000
v 0.040000 -0.040000 0.010000
v -0.040000 -0.040000 0.010000
v -0.040000 -0.040000 0.100000
v 0.040000 0.040000 0.100000
v 0.040000 0.040000 0.010000
v 0.040000 -0.040000 0.010000
v 0.040000 -0.040000 0.100000
v -0.040000 0.040000 0.100000
v -0.040000 0.040000 0.010000
v 0.040000 0.040000 0.010000
v 0.040000 0.040000 0.100000
v -0.040000 -0.040000 0.100000
v -0.040000 -0.040000 0.010000
v -0.040000 0.040000 0.010000
v -0.040000 0.040000 0.100000
v -0.040000 0.040000 0.010000
v -0.040000 -0.040000 0.010000
v 0.040000 -0.040000 0.010000
v 0.040000 0.040000 0.010000
vn -1.0000 0.0000 0.0000
vn 0.0000 -1.0000 0.0000
vn 1.0000 0.0000 0.0000
vn 0.0000 1.0000 0.0000
vn 0.0000 0.0000 -1.0000
vn 0.0000 0.0000 1.0000
s 1
f 1//1 2//1 3//1
f 3//1 4//1 1//1
f 5//2 6//2 7//2
f 7//2 8//2 5//2
f 9//3 10//3 11//3
f 11//3 12//3 9//3
f 13//4 14//4 15//4
f 15//4 16//4 13//4
f 17//5 18//5 19//5
f 19//5 20//5 17//5
f 21//6 22//6 23//6
f 22//6 24//6 25//6
f 24//6 26//6 27//6
f 26//6 21//6 28//6
f 25//6 23//6 22//6
f 28//6 27//6 26//6
f 27//6 25//6 24//6
f 23//6 28//6 21//6
f 29//4 30//4 31//4
f 31//4 32//4 29//4
f 33//1 34//1 35//1
f 35//1 36//1 33//1
f 37//2 38//2 39//2
f 39//2 40//2 37//2
f 41//3 42//3 43//3
f 43//3 44//3 41//3
f 45//6 46//6 47//6
f 47//6 48//6 45//6
test/user/testdata/cube_cup_hi.obj
Vendored
+22323
View File
File diff suppressed because it is too large Load Diff
test/user/testdata/cube_cup_quad.obj
Vendored
+14885
View File
File diff suppressed because it is too large Load Diff
test/user/testdata/inertia_concave.xml
Vendored
+27
View File
@@ -0,0 +1,27 @@
<mujoco>
<compiler exactmeshinertia="true"/>
<asset>
<mesh file="cube_cup.obj" scale="10 10 10"/>
<mesh file="cube_cup_quad.obj" scale="10 10 10"/>
<mesh file="cube_cup_hi.obj" scale="10 10 10"/>
</asset>
<worldbody>
<light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<body pos="-1.5 0 1">
<geom type="mesh" mesh="cube_cup" density="1"/>
</body>
<body pos="0 0 1">
<geom type="mesh" mesh="cube_cup_quad" density="1"/>
</body>
<body pos="1.5 0 1">
<geom type="mesh" mesh="cube_cup_hi" density="1"/>
</body>
<body pos="3 0 1">
<geom type="box" pos="0 0 0.05" size=".5 .5 .05" density="1"/>
<geom type="box" pos="-.45 0 .55" size=".05 .5 .45" density="1"/>
<geom type="box" pos=" .45 0 .55" size=".05 .5 .45" density="1"/>
<geom type="box" pos="0 -.45 .55" size=".4 .05 .45" density="1"/>
<geom type="box" pos="0 .45 .55" size=".4 .05 .45" density="1"/>
</body>
</worldbody>
</mujoco>
test/user/testdata/inertia_convex.xml
Vendored
+23
View File
@@ -0,0 +1,23 @@
<mujoco>
<asset>
<mesh file="cube.obj"/>
<mesh name="inline_cube"
vertex="
-.5 -.5 -.5
.5 -.5 -.5
.5 .5 -.5
-.5 .5 -.5
-.5 -.5 .5
.5 -.5 .5
.5 .5 .5
-.5 .5 .5"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="cube" density="1"/>
</body>
<body>
<geom type="mesh" mesh="inline_cube" density="1"/>
</body>
</worldbody>
</mujoco>
test/user/testdata/inertia_shell.xml
Vendored
+11
View File
@@ -0,0 +1,11 @@
<mujoco>
<compiler exactmeshinertia="true"/>
<asset>
<mesh file="cube.obj" name="hollow_cube"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="hollow_cube" density="1" shellinertia="true"/>
</body>
</worldbody>
</mujoco>
test/user/user_mesh_test.cc
+67
View File
@@ -37,6 +37,12 @@ static const char* const kCubePath =
"user/testdata/cube.xml";
static const char* const kTorusPath =
"user/testdata/torus.xml";
static const char* const kConvexInertiaPath =
"user/testdata/inertia_convex.xml";
static const char* const kConcaveInertiaPath =
"user/testdata/inertia_concave.xml";
static const char* const kShellInertiaPath =
"user/testdata/inertia_shell.xml";
static const char* const kTorusQuadsPath =
"user/testdata/torus_quads.xml";
static const char* const kTexturedTorusPath =
@@ -134,6 +140,8 @@ TEST_F(MujocoTest, TinyMeshLoads) {
mj_deleteModel(model);
}
// ------------- test inertia -------------------------------------------------
TEST_F(MujocoTest, SmallInertiaLoads) {
static constexpr char xml[] = R"(
<mujoco>
@@ -202,5 +210,64 @@ TEST_F(MujocoTest, FlippedFaceFails) {
EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
}
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
TEST_F(MujocoTest, ExactConcaveInertia) {
const std::string xml_path = GetTestDataFilePath(kConcaveInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// analytic computation of 1x1x1 cube with a .8x.8x.9 hole
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_hole = .9 * .8 * .8;
mjtNum m_cube = 1.;
mjtNum m_concave_cube = m_cube - m_hole;
mjtNum I_cube = m_cube/6.;
// due to the asymmetric hole, the com position has changed
// so we need to use https://en.wikipedia.org/wiki/Parallel_axis_theorem
mjtNum d_cube = .5 - model->body_ipos[5];
mjtNum d_hole = .55 - model->body_ipos[5];
mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12;
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole;
EXPECT_LE(fabs(model->body_mass[1] - m_concave_cube), max_abs_err);
EXPECT_LE(fabs(model->body_mass[2] - m_concave_cube), max_abs_err);
EXPECT_LE(fabs(model->body_mass[3] - m_concave_cube), max_abs_err);
EXPECT_LE(fabs(model->body_mass[4] - m_concave_cube), max_abs_err);
for (int i=3; i<15; i+=3) {
EXPECT_LE(fabs(model->body_inertia[i] - I1), max_abs_err);
EXPECT_LE(fabs(model->body_inertia[i+1] - I2), max_abs_err);
EXPECT_LE(fabs(model->body_inertia[i+2] - I2), max_abs_err);
}
mj_deleteModel(model);
}
TEST_F(MujocoTest, ExactConvexInertia) {
const std::string xml_path = GetTestDataFilePath(kConvexInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_solid_cube = 1.;
mjtNum I_solid_cube = 1./6. * m_solid_cube;
EXPECT_LE(fabs(model->body_mass[1] - m_solid_cube), max_abs_err);
EXPECT_LE(fabs(model->body_mass[2] - m_solid_cube), max_abs_err);
for (int i=3; i<9; i++) {
EXPECT_LE(fabs(model->body_inertia[i] - I_solid_cube), max_abs_err);
}
mj_deleteModel(model);
}
TEST_F(MujocoTest, ExactShellInertia) {
const std::string xml_path = GetTestDataFilePath(kShellInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_hollow_cube = 6.;
mjtNum I_hollow_cube = 5./18. * m_hollow_cube;
EXPECT_LE(fabs(model->body_mass[1] - m_hollow_cube), max_abs_err);
EXPECT_LE(fabs(model->body_inertia[3] - I_hollow_cube), max_abs_err);
EXPECT_LE(fabs(model->body_inertia[4] - I_hollow_cube), max_abs_err);
EXPECT_LE(fabs(model->body_inertia[5] - I_hollow_cube), max_abs_err);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/xml/xml_native_reader_test.cc
+19
View File
@@ -294,6 +294,25 @@ TEST_F(UserDataTest, InvalidInertialOrientation) {
EXPECT_THAT(error.data(), HasSubstr("multiple orientation specifiers for the same field"));
}
TEST_F(UserDataTest, ReadShellParameter) {
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 2 0 3" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
mj_deleteModel(model);
}
TEST_F(UserDataTest, ReadsDamper) {
static constexpr char xml[] = R"(
<mujoco>