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

Use shell inertia to compute mesh aabb and frame if volume is invalid.

Browse Source 
PiperOrigin-RevId: 530925161
Change-Id: I94b97efc7198e2b750480df8687e91bc047a6fc9
This commit is contained in:
Alessio Quaglino
2023-05-10 09:11:09 -07:00
committed by Copybara-Service
parent 1aadef5927
commit 41a7049920
4 changed files with 192 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+8
View File
@@ -9,6 +9,14 @@ Upcoming version (not yet released)
:align: right :align: right
:width: 240px :width: 240px
Bug fixes
^^^^^^^
- Fixed a bug that was causing an incorrect computation of the mesh bounding box and coordinate frame if the volume was
invalid. In such case, now MuJoCo only accepts a non-watertight geometry if :ref:`shellinertia<body-geom-shellinertia>`
is equal to ``true`.
Plugins Plugins
^^^^^^^ ^^^^^^^
src/user/user_mesh.cc
+154 -140
View File
@@ -912,137 +912,150 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
// apply transformations // apply transformations
void mjCMesh::Process() { void mjCMesh::ApplyTransformations() {
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) { // translate
double CoM[3] = {0, 0, 0}; if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) {
double facecen[3] = {0, 0, 0}; // prepare translation
double area = 0; float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
double inert[6] = {0, 0, 0, 0, 0, 0};
double nrm[3]; // process vertices
double cen[3]; for (int i=0; i<nvert; i++) {
bool exactmeshinertia = model->exactmeshinertia; vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
}
}
if (type==mjVOLUME_MESH) { // rotate
// translate if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) { // prepare rotation
// prepare translation mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]}; mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
// process vertices // process vertices
for (int i=0; i<nvert; i++) { for (int i=0; i<nvert; i++) {
vert[3*i] -= rp[0]; mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
vert[3*i+1] -= rp[1]; mju_rotVecMatT(p1, p0, mat);
vert[3*i+2] -= rp[2]; vert[3*i] = (float) p1[0];
} vert[3*i+1] = (float) p1[1];
} vert[3*i+2] = (float) p1[2];
}
// rotate // process normals
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) { for (int i=0; i<nnormal; i++) {
// prepare rotation mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; mju_rotVecMatT(n1, n0, mat);
mjtNum mat[9]; normal[3*i] = (float) n1[0];
mju_normalize4(quat); normal[3*i+1] = (float) n1[1];
mju_quat2Mat(mat, quat); normal[3*i+2] = (float) n1[2];
}
}
// process vertices // scale
for (int i=0; i<nvert; i++) { if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]}; for (int i=0; i<nvert; i++) {
mju_rotVecMatT(p1, p0, mat); vert[3*i] *= scale[0];
vert[3*i] = (float) p1[0]; vert[3*i+1] *= scale[1];
vert[3*i+1] = (float) p1[1]; vert[3*i+2] *= scale[2];
vert[3*i+2] = (float) p1[2]; }
}
// process normals for (int i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) { normal[3*i] *= scale[0];
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]}; normal[3*i+1] *= scale[1];
mju_rotVecMatT(n1, n0, mat); normal[3*i+2] *= scale[2];
normal[3*i] = (float) n1[0]; }
normal[3*i+1] = (float) n1[1]; }
normal[3*i+2] = (float) n1[2];
}
}
// scale // normalize normals
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) { for (int i=0; i<nnormal; i++) {
for (int i=0; i<nvert; i++) { // compute length
vert[3*i] *= scale[0]; float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
}
for (int i=0; i<nnormal; i++) { // rescale
normal[3*i] *= scale[0]; if (len>mjMINVAL) {
normal[3*i+1] *= scale[1]; float scl = 1/sqrtf(len);
normal[3*i+2] *= scale[2]; 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;
}
}
}
// normalize normals
for (int i=0; i<nnormal; 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 // find centroid of faces
if (len>mjMINVAL) { void mjCMesh::ComputeFaceCentroid(double facecen[3]) {
float scl = 1/sqrtf(len); double area = 0;
normal[3*i] *= scl; double nrm[3];
normal[3*i+1] *= scl; double cen[3];
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 (int i=0; i<nface; i++) {
for (int i=0; i<nface; i++) { // check vertex indices
// check vertex indices for (int j=0; j<3; j++) {
for (int j=0; j<3; j++) { if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
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);
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
}
}
// get area and center
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (int j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// require positive area
if (area < mjMINVAL) {
validarea = false;
return;
}
// finalize centroid of faces
for (int j=0; j<3; j++) {
facecen[j] /= area;
} }
} }
// get area and center
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (int j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
}
// require positive area
if (area < mjMINVAL) {
validarea = false;
return;
}
// finalize centroid of faces
for (int j=0; j<3; j++) {
facecen[j] /= area;
}
}
void mjCMesh::Process() {
double facecen[3] = {0, 0, 0};
double nrm[3];
double cen[3];
// user offset, rotation, scaling
ApplyTransformations();
// find centroid of faces
ComputeFaceCentroid(facecen);
// compute inertial properties for both inertia types
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
double CoM[3] = {0, 0, 0};
double inert[6] = {0, 0, 0, 0, 0, 0};
bool exactmeshinertia = model->exactmeshinertia;
// compute CoM and volume from pyramid volumes // compute CoM and volume from pyramid volumes
ComputeVolume(CoM, type, facecen, model->exactmeshinertia); ComputeVolume(CoM, type, facecen, model->exactmeshinertia);
// perform computation again if volume is negative // perform computation with convex mesh if volume is negative
if (GetVolumeRef(type) < mjMINVAL && exactmeshinertia) { if (GetVolumeRef(type) <= 0 && exactmeshinertia) {
mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str()); mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str());
exactmeshinertia = false; exactmeshinertia = false;
ComputeVolume(CoM, type, facecen, exactmeshinertia); ComputeVolume(CoM, type, facecen, exactmeshinertia);
} }
// if volume is still invalid, skip the rest of the computations
// require positive volume
if (GetVolumeRef(type) < mjMINVAL) { if (GetVolumeRef(type) < mjMINVAL) {
validvolume = GetVolumeRef(type) < 0 ? -1 : 0; validvolume = GetVolumeRef(type) < 0 ? -1 : 0;
return; continue;
} }
// finalize CoM, save as mesh center // finalize CoM, save as mesh center
@@ -1052,7 +1065,7 @@ void mjCMesh::Process() {
mjuu_copyvec(GetPosPtr(type), CoM, 3); mjuu_copyvec(GetPosPtr(type), CoM, 3);
// re-center mesh at CoM // re-center mesh at CoM
if (type==mjVOLUME_MESH) { if (type==mjVOLUME_MESH || validvolume<=0) {
for (int i=0; i<nvert; i++) { for (int i=0; i<nvert; i++) {
for (int j=0; j<3; j++) { for (int j=0; j<3; j++) {
vert[3*i+j] -= CoM[j]; vert[3*i+j] -= CoM[j];
@@ -1128,37 +1141,38 @@ void mjCMesh::Process() {
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/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; boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
// copy quat // if volume was valid, copy volume quat to shell and stop,
for (int j=0; j<4; j++) { // otherwise use shell quat for coordinate transformations
GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j]; if (type==mjSHELL_MESH && validvolume>0) {
mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH));
continue;
} }
// rotate vertices and normals into axis-aligned frame // rotate vertices and normals into axis-aligned frame
if (type==mjVOLUME_MESH) { mju_copy4(GetQuatPtr(type), quattmp);
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9]; double mat[9];
mjuu_quat2mat(mat, neg); mjuu_quat2mat(mat, neg);
for (int i=0; i<nvert; i++) { for (int i=0; i<nvert; i++) {
// vertices // vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]}; const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3]; double res[3];
mjuu_mulvecmat(res, vec, mat); mjuu_mulvecmat(res, vec, mat);
for (int j=0; j<3; j++) { for (int j=0; j<3; j++) {
vert[3*i+j] = (float) res[j]; vert[3*i+j] = (float) res[j];
// axis-aligned bounding box // axis-aligned bounding box
aabb[j+0] = mjMIN(aabb[j+0], res[j]); aabb[j+0] = mjMIN(aabb[j+0], res[j]);
aabb[j+3] = mjMAX(aabb[j+3], res[j]); aabb[j+3] = mjMAX(aabb[j+3], res[j]);
}
} }
for (int i=0; i<nnormal; i++) { }
// normals for (int i=0; i<nnormal; i++) {
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]}; // normals
double res[3]; const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mjuu_mulvecmat(res, nrm, mat); double res[3];
for (int j=0; j<3; j++) { mjuu_mulvecmat(res, nrm, mat);
normal[3*i+j] = (float) res[j]; for (int j=0; j<3; j++) {
} normal[3*i+j] = (float) res[j];
} }
} }
} }
@@ -1166,7 +1180,7 @@ void mjCMesh::Process() {
// check that the mesh is valid // check that the mesh is valid
void mjCMesh::CheckMesh() { void mjCMesh::CheckMesh(mjtMeshType type) {
if (!processed) { if (!processed) {
return; return;
} }
@@ -1175,11 +1189,11 @@ void mjCMesh::CheckMesh() {
"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) if (!validarea && type==mjSHELL_MESH)
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<0) if (validvolume<0 && type==mjVOLUME_MESH)
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) if (!validvolume && type==mjVOLUME_MESH)
throw mjCError(this, "mesh volume is too small: %s", name.c_str()); throw mjCError(this, "mesh volume is too small: %s", 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());
@@ -1190,13 +1204,13 @@ void mjCMesh::CheckMesh() {
// get inertia pointer // get inertia pointer
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) { double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
CheckMesh(); CheckMesh(type);
return type==mjSHELL_MESH ? boxsz_surface : boxsz_volume; return type==mjSHELL_MESH ? boxsz_surface : boxsz_volume;
} }
double& mjCMesh::GetVolumeRef(mjtMeshType type) { double& mjCMesh::GetVolumeRef(mjtMeshType type) {
CheckMesh(); CheckMesh(type);
return type==mjSHELL_MESH ? surface : volume; return type==mjSHELL_MESH ? surface : volume;
} }
src/user/user_objects.h
+4 -2
View File
@@ -554,9 +554,11 @@ class mjCMesh: public mjCBase {
void CopyGraph(void); // copy graph into face data void CopyGraph(void); // copy graph into face data
void MakeNormal(void); // compute vertex normals void MakeNormal(void); // compute vertex normals
void MakeCenter(void); // compute face circumcircle data void MakeCenter(void); // compute face circumcircle data
void Process(); // apply transformations void Process(); // compute inertial properties
void ApplyTransformations(); // apply user transformations
void ComputeFaceCentroid(double[3]); // compute centroid of all faces
void RemoveRepeated(void); // remove repeated vertices void RemoveRepeated(void); // remove repeated vertices
void CheckMesh(void); // check if the mesh is valid void CheckMesh(mjtMeshType type); // check if the mesh is valid
// 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], mjtMeshType type, const double facecen[3], void ComputeVolume(double CoM[3], mjtMeshType type, const double facecen[3],
test/user/user_mesh_test.cc
+26 -1
View File
@@ -493,7 +493,7 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
</asset> </asset>
<worldbody> <worldbody>
<body> <body>
<geom type="mesh" mesh="example_mesh"/> <geom type="mesh" mesh="example_mesh" shellinertia="true"/>
</body> </body>
</worldbody> </worldbody>
</mujoco> </mujoco>
@@ -544,6 +544,31 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
} }
TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
static constexpr char xml[] = R"(
<mujoco>
<compiler convexhull="false"/>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 0"
face="0 1 2 2 1 3" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" shellinertia="true"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull());
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, VolumeNegativeDefaultsLegacy) { TEST_F(MjCMeshTest, VolumeNegativeDefaultsLegacy) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>