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

Allow malformed meshes for visualization purposes. Throw error only if the mesh is required for inertia computation.

Browse Source 
PiperOrigin-RevId: 466421062
Change-Id: I947dbc107e481bd8c328b1a6430ab2e88bc82fd6
This commit is contained in:
Alessio Quaglino
2022-08-09 11:40:49 -07:00
committed by Copybara-Service
parent aee73a9023
commit 95d5db3f81
8 changed files with 230 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+37 -9
View File
@@ -81,6 +81,11 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
face = NULL;
graph = NULL;
needhull = false;
validorientation = true;
validarea = true;
validvolume = true;
valideigenvalue = true;
validinequality = true;
// reset to default if given
if (_def) {
@@ -223,8 +228,9 @@ void mjCMesh::Compile(const mjVFS* vfs) {
if (!useredge.empty()) {
std::sort(useredge.begin(), useredge.end());
auto iterator = std::adjacent_find(useredge.begin(), useredge.end());
if (iterator != useredge.end())
throw mjCError(this, "faces have inconsistent orientation");
if (iterator != useredge.end()) {
validorientation = false;
}
}
// require vertices
@@ -248,7 +254,9 @@ void mjCMesh::Compile(const mjVFS* vfs) {
}
// scale, center, orient, compute mass and inertia
Process();
if (validorientation) {
Process();
}
}
@@ -920,7 +928,7 @@ void mjCMesh::Process() {
vert[3*i+1] = (float) p1[1];
vert[3*i+2] = (float) p1[2];
// nromals
// normals
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];
@@ -983,7 +991,8 @@ void mjCMesh::Process() {
// require positive area
if (area < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
validarea = false;
return;
}
// finalize centroid of faces
@@ -1016,7 +1025,8 @@ void mjCMesh::Process() {
// require positive volume
if (GetVolumeRef(type) < mjMINVAL) {
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
validvolume = false;
return;
}
// finalize CoM, save as mesh center
@@ -1085,13 +1095,14 @@ void mjCMesh::Process() {
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
valideigenvalue = false;
return;
}
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());
validinequality = false;
return;
}
// compute sizes of equivalent inertia box
@@ -1140,8 +1151,24 @@ void mjCMesh::Process() {
}
// check that the mesh is valid
void mjCMesh::CheckMesh() {
if (!validorientation)
throw mjCError(this, "faces have inconsistent orientation: %s", name.c_str());
if (!validarea)
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
if (!validvolume)
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
if (!valideigenvalue)
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
if (!validinequality)
throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str());
}
// compute inertia
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
CheckMesh();
if (type==mjSHELL_MESH) {
return boxsz_surface;
} else {
@@ -1151,6 +1178,7 @@ double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
double& mjCMesh::GetVolumeRef(mjtMeshType type) {
CheckMesh();
if (type) {
return surface;
} else {
src/user/user_objects.cc
+25 -14
View File
@@ -295,7 +295,7 @@ mjCBody::mjCBody(mjCModel* _model) {
pos[0] = ipos[0] = mjNAN;
// clear variables
explicit_inertial = false;
explicitinertial = false;
mocap = false;
mjuu_setvec(quat, 1, 0, 0, 0);
mjuu_setvec(iquat, 1, 0, 0, 0);
@@ -634,9 +634,9 @@ void mjCBody::MakeLocal(double* _locpos, double* _locquat,
}
}
// set explicit_inertial to true
// set explicitinertial to true
void mjCBody::MakeInertialExplicit() {
explicit_inertial = true;
explicitinertial = true;
}
@@ -680,6 +680,8 @@ void mjCBody::Compile(void) {
// compile all geoms, phase 1
for (i=0; i<geoms.size(); i++) {
geoms[i]->inferinertia = id>0 && (!explicitinertial ||
model->inertiafromgeom==mjINERTIAFROMGEOM_TRUE);
geoms[i]->Compile();
}
@@ -968,6 +970,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
rgba[3] = 1.0f;
userdata.clear();
typeinertia = mjVOLUME_MESH;
inferinertia = true;
// clear internal variables
mjuu_setvec(quat, 1, 0, 0, 0);
@@ -1385,18 +1388,26 @@ void mjCGeom::Compile(void) {
}
// compute geom mass and inertia
if (mjuu_defined(_mass) && GetVolume()>mjMINVAL) {
mass = _mass;
density = _mass / GetVolume();
} else {
mass = density * GetVolume();
}
SetInertia();
if (inferinertia) {
if (mjuu_defined(_mass)) {
if (_mass==0) {
mass = 0;
density = 0;
} else if (GetVolume()>mjMINVAL) {
mass = _mass;
density = _mass / GetVolume();
SetInertia();
}
} else {
mass = density * GetVolume();
SetInertia();
}
// check for negative values
if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)",
name.c_str(), id);
// check for negative values
if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)",
name.c_str(), id);
}
// fluid-interaction coefficients, requires computed inertia and mass
if (fluid_switch > 0) {
src/user/user_objects.h
+12 -3
View File
@@ -167,7 +167,7 @@ class mjCBody : public mjCBase {
// setup child local frame, take into account change
void MakeLocal(double* locpos, double* locquat, const double* pos, const double* quat);
// set explicit_inertial to true
// set explicitinertial to true
void MakeInertialExplicit();
// variables set by user or 'Compile'
@@ -198,7 +198,7 @@ class mjCBody : public mjCBase {
int weldid; // top index of body we are welded to
int dofnum; // number of motion dofs for body
int mocapid; // mocap id, -1: not mocap
bool explicit_inertial; // whether to save the body with an explicit inertial clause
bool explicitinertial; // whether to save the body with an explicit inertial clause
int lastdof; // id of last dof (used by compiler)
@@ -306,6 +306,7 @@ class mjCGeom : public mjCBase {
std::vector<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
mjtMeshType typeinertia; // selects between surface and volume inertia
bool inferinertia; // true if inertia has to be computed from geom
// variables set by user and used during compilation
double _mass; // used to compute density
@@ -481,7 +482,15 @@ class mjCMesh: public mjCBase {
void MakeNormal(void); // compute vertex normals
void Process(); // apply transformations
void RemoveRepeated(void); // remove repeated vertices
void ComputeInertia(mjtMeshType type); // compute inertia
void ComputeInertia(mjtMeshType type); // compute inertia
void CheckMesh(void); // check if the mesh is valid
// mesh properties that indicate a well-formed mesh
bool validorientation; // false if mesh have inconsistent faces
bool validarea; // false if the area is too small
bool validvolume; // false if the volume is too small
bool valideigenvalue; // false if inertia eigenvalue is too small
bool validinequality; // false if inertia inequality is not satisfied
// mesh properties computed by Compile
double pos_volume[3]; // CoM position
src/xml/xml_native_reader.cc
+1 -1
View File
@@ -2284,7 +2284,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
if (pbody->id==0) {
throw mjXError(elem, "World body cannot have inertia");
}
pbody->explicit_inertial = true;
pbody->explicitinertial = true;
ReadAttr(elem, "pos", 3, pbody->ipos, text, true);
ReadAttr(elem, "quat", 4, pbody->iquat, text);
ReadAttr(elem, "mass", 1, &pbody->mass, text, true);
src/xml/xml_native_writer.cc
+1 -1
View File
@@ -1178,7 +1178,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
WriteVector(elem, "user", body->userdata);
// write inertial
if (body->explicit_inertial &&
if (body->explicitinertial &&
model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) {
XMLElement* inertial = InsertEnd(elem, "inertial");
WriteAttr(inertial, "pos", 3, body->locipos);
src/xml/xml_urdf.cc
+1 -1
View File
@@ -207,7 +207,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
// inertial element: copy into alternative body frame
if ((elem = FindSubElem(body_elem, "inertial"))) {
pbody->explicit_inertial = true;
pbody->explicitinertial = true;
// origin- relative to joint frame for now
Origin(elem, pbody->ipos, pbody->iquat);
test/user/testdata/malformed_face.xml
Vendored
+3 -1
View File
@@ -3,6 +3,8 @@
<mesh file="malformed_face.obj"/>
</asset>
<worldbody>
<geom type="mesh" mesh="malformed_face"/>
<body>
<geom type="mesh" mesh="malformed_face"/>
</body>
</worldbody>
</mujoco>
test/user/user_mesh_test.cc
+150 -3
View File
@@ -188,11 +188,32 @@ TEST_F(MujocoTest, MalformedFaceFails) {
const std::string xml_path = GetTestDataFilePath(kMalformedFaceOBJPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(model, testing::IsNull());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
}
TEST_F(MujocoTest, FlippedFaceFails) {
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="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</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("faces have inconsistent orientation"));
}
TEST_F(MujocoTest, FlippedFaceAllowedWorld) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
@@ -206,10 +227,136 @@ TEST_F(MujocoTest, FlippedFaceFails) {
</mujoco>
)";
std::array<char, 1024> error;
LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation"));
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull());
mj_deleteModel(model);
}
TEST_F(MujocoTest, FlippedFaceAllowedNoMass) {
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="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" mass="0"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull());
mj_deleteModel(model);
}
TEST_F(MujocoTest, FlippedFaceAllowedInertial) {
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="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<inertial pos="0 0 0" mass="1"/>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull());
mj_deleteModel(model);
}
TEST_F(MujocoTest, 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" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</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 surface area is too small"));
}
TEST_F(MujocoTest, 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>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull());
mj_deleteModel(model);
}
TEST_F(MujocoTest, VolumeTooSmall) {
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>
<body>
<geom type="mesh" mesh="example_mesh"/>
</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"));
}
TEST_F(MujocoTest, 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>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull());
mj_deleteModel(model);
}
// ------------- test concave and shell inertia --------------------------------
const mjtNum max_abs_err = std::numeric_limits<float>::epsilon();
TEST_F(MujocoTest, ExactConcaveInertia) {