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

Add flex arrays to CompareModel check.

Browse Source 
Due to a bug, all flex mjModel fields were previously ignored.

PiperOrigin-RevId: 912546202
Change-Id: Ica95ecfcbbe366fd81de6e8f7d83ed57a7d23033
This commit is contained in:
Alessio Quaglino
2026-05-08 08:47:10 -07:00
committed by Copybara-Service
parent 8cef5bb978
commit fa912dffa0
7 changed files with 158 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_setconst.c
+2
View File
@@ -447,6 +447,8 @@ static void makeFlexSparse(mjModel* m, mjData* d) {
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge);
mju_zero(m->flex_vertmetric, 4 * m->nflexvert);
mju_zeroInt(m->flexedge_J_colind, m->nJfe);
mju_zeroInt(m->flexvert_J_colind, 2 * m->nJfv);
int current_adj_offset = 0;
// compute lengths and Jacobians of edges
src/user/user_flexcomp.cc
+3 -110
View File
@@ -13,14 +13,12 @@
// limitations under the License.
#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <queue>
#include <sstream>
#include <stdexcept>
#include <string>
@@ -107,116 +105,11 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points,
int cx = flex->spec.cellcount[0];
int cy = flex->spec.cellcount[1];
int cz = flex->spec.cellcount[2];
int ncells = cx * cy * cz;
int order = flex->spec.order;
// determine which cells contain mesh elements (not just vertices)
// for each element, compute its AABB and mark all overlapping cells
std::vector<bool> has_element(ncells, false);
double dx = minmax[3] - minmax[0];
double dy = minmax[4] - minmax[1];
double dz = minmax[5] - minmax[2];
// vertices per element: dim+1 (edges=2, triangles=3, tets=4)
int nvpe = flex->spec.dim + 1;
if (nvpe > 0 && !element.empty()) {
int nelem = element.size() / nvpe;
for (int e = 0; e < nelem; e++) {
// compute element AABB
double elo[3] = {1e30, 1e30, 1e30};
double ehi[3] = {-1e30, -1e30, -1e30};
for (int v = 0; v < nvpe; v++) {
int vid = element[nvpe * e + v];
for (int j = 0; j < 3; j++) {
elo[j] = std::min(elo[j], points[3 * vid + j]);
ehi[j] = std::max(ehi[j], points[3 * vid + j]);
}
}
// map element AABB to cell range
auto cellIdx = [](double coord, double lo, double d, int nc) {
if (d <= 0) return 0;
int c = (int)((coord - lo) / d * nc);
return std::max(0, std::min(nc - 1, c));
};
int ci0 = cellIdx(elo[0], minmax[0], dx, cx);
int ci1 = cellIdx(ehi[0], minmax[0], dx, cx);
int cj0 = cellIdx(elo[1], minmax[1], dy, cy);
int cj1 = cellIdx(ehi[1], minmax[1], dy, cy);
int ck0 = cellIdx(elo[2], minmax[2], dz, cz);
int ck1 = cellIdx(ehi[2], minmax[2], dz, cz);
// mark all overlapping cells as containing elements
for (int ci = ci0; ci <= ci1; ci++) {
for (int cj = cj0; cj <= cj1; cj++) {
for (int ck = ck0; ck <= ck1; ck++) {
has_element[ci * cy * cz + cj * cz + ck] = true;
}
}
}
}
}
// default: all cells non-empty (only exterior cells will be empty)
flex->cell_empty.assign(ncells, false);
// for dim=2 (surface mesh): check watertightness and flood-fill
if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) {
// flood-fill from grid boundary to find exterior cells
// cells reachable from the boundary through non-element cells
// are outside the mesh volume; cells NOT reachable are interior
std::vector<bool> visited(ncells, false);
std::queue<std::array<int, 3>> bfs;
// seed BFS from boundary cells that have no elements
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
if (ci == 0 || ci == cx - 1 ||
cj == 0 || cj == cy - 1 ||
ck == 0 || ck == cz - 1) {
int idx = ci * cy * cz + cj * cz + ck;
if (!has_element[idx] && !visited[idx]) {
visited[idx] = true;
flex->cell_empty[idx] = true;
bfs.push({ci, cj, ck});
}
}
}
}
}
// BFS: spread through non-element cells
const int dirs[6][3] = {
{-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
{0, 1, 0}, {0, 0, -1}, {0, 0, 1}};
while (!bfs.empty()) {
auto [ci, cj, ck] = bfs.front();
bfs.pop();
for (auto& d : dirs) {
int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2];
if (ni < 0 || ni >= cx ||
nj < 0 || nj >= cy ||
nk < 0 || nk >= cz) {
continue;
}
int nidx = ni * cy * cz + nj * cz + nk;
if (!visited[nidx] && !has_element[nidx]) {
visited[nidx] = true;
flex->cell_empty[nidx] = true;
bfs.push({ni, nj, nk});
}
}
}
} else {
// dim!=2 (e.g., tet mesh): cells without element overlap are empty
for (int c = 0; c < ncells; c++) {
flex->cell_empty[c] = !has_element[c];
}
}
// delegate cell_empty computation to mjCFlex
int nelem = element.size() / (flex->spec.dim + 1);
flex->ComputeCellEmpty(points, element.data(), npnt, nelem, flex->spec.dim, minmax);
// pin nodes that belong exclusively to empty cells
for (int gi = 0; gi < nx; gi++) {
src/user/user_mesh.cc
+137 -2
View File
@@ -25,6 +25,7 @@
#include <limits>
#include <map>
#include <memory>
#include <queue>
#include <set>
#include <string>
#include <string_view>
@@ -4698,8 +4699,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// no elemtexcoord: copy from faces
if (elemtexcoord_.empty() && !texcoord_.empty()) {
elemtexcoord_.assign(3*nelem, 0);
memcpy(elemtexcoord_.data(), elem_.data(), 3*nelem*sizeof(int));
elemtexcoord_.assign((dim + 1) * nelem, 0);
memcpy(elemtexcoord_.data(), elem_.data(), (dim + 1) * nelem * sizeof(int));
}
// resolve material name
@@ -4914,6 +4915,15 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// create shell fragments and element-vertex collision pairs
CreateShellPair();
// recompute cell_empty from vertex/element geometry
// (survives XML round-trips where flexcomp data is lost)
if (interpolated && cell_empty.empty()) {
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
if (cx * cy * cz > 1) {
ComputeCellEmpty(vertxpos.data(), elem_.data(), nvert, nelem, dim);
}
}
// compute linear stiffness for interpolated elements (cached)
bool stiffness_cached = false;
if (young > 0 && interpolated) {
@@ -5223,6 +5233,131 @@ std::vector<double> mjCFlex::ComputeUnrotatedNodePositions(
}
// identify cells with no mesh content from vertex/element geometry
void mjCFlex::ComputeCellEmpty(const double* vpos, const int* elems,
int nv, int ne, int fdim,
const double* bbox) {
int cx = spec.cellcount[0];
int cy = spec.cellcount[1];
int cz = spec.cellcount[2];
int ncells = cx * cy * cz;
// use precomputed bounding box if provided, otherwise compute from vertices
double minmax[6];
if (bbox) {
for (int j = 0; j < 6; j++) minmax[j] = bbox[j];
} else {
minmax[0] = minmax[1] = minmax[2] = 1e30;
minmax[3] = minmax[4] = minmax[5] = -1e30;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < 3; j++) {
minmax[j+0] = std::min(minmax[j+0], vpos[3*i+j]);
minmax[j+3] = std::max(minmax[j+3], vpos[3*i+j]);
}
}
}
double dx = minmax[3] - minmax[0];
double dy = minmax[4] - minmax[1];
double dz = minmax[5] - minmax[2];
// determine which cells contain mesh elements
std::vector<bool> has_element(ncells, false);
int nvpe = fdim + 1;
if (nvpe > 0 && ne > 0) {
for (int e = 0; e < ne; e++) {
// compute element AABB
double elo[3] = {1e30, 1e30, 1e30};
double ehi[3] = {-1e30, -1e30, -1e30};
for (int v = 0; v < nvpe; v++) {
int vid = elems[nvpe * e + v];
for (int j = 0; j < 3; j++) {
elo[j] = std::min(elo[j], vpos[3 * vid + j]);
ehi[j] = std::max(ehi[j], vpos[3 * vid + j]);
}
}
// map element AABB to cell range
auto cellIdx = [](double coord, double lo, double d, int nc) {
if (d <= 0) return 0;
int c = (int)((coord - lo) / d * nc);
return std::max(0, std::min(nc - 1, c));
};
int ci0 = cellIdx(elo[0], minmax[0], dx, cx);
int ci1 = cellIdx(ehi[0], minmax[0], dx, cx);
int cj0 = cellIdx(elo[1], minmax[1], dy, cy);
int cj1 = cellIdx(ehi[1], minmax[1], dy, cy);
int ck0 = cellIdx(elo[2], minmax[2], dz, cz);
int ck1 = cellIdx(ehi[2], minmax[2], dz, cz);
for (int ci = ci0; ci <= ci1; ci++) {
for (int cj = cj0; cj <= cj1; cj++) {
for (int ck = ck0; ck <= ck1; ck++) {
has_element[ci * cy * cz + cj * cz + ck] = true;
}
}
}
}
}
cell_empty.assign(ncells, false);
// for dim=2 (surface mesh): flood-fill from boundary to find exterior cells
if (fdim == 2 && nvpe == 3 && ne > 0) {
std::vector<bool> visited(ncells, false);
std::queue<std::array<int, 3>> bfs;
// seed BFS from boundary cells that have no elements
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
if (ci == 0 || ci == cx - 1 ||
cj == 0 || cj == cy - 1 ||
ck == 0 || ck == cz - 1) {
int idx = ci * cy * cz + cj * cz + ck;
if (!has_element[idx] && !visited[idx]) {
visited[idx] = true;
cell_empty[idx] = true;
bfs.push({ci, cj, ck});
}
}
}
}
}
// BFS: spread through non-element cells
const int dirs[6][3] = {
{-1, 0, 0}, {1, 0, 0}, {0, -1, 0},
{0, 1, 0}, {0, 0, -1}, {0, 0, 1}};
while (!bfs.empty()) {
auto [ci, cj, ck] = bfs.front();
bfs.pop();
for (auto& d : dirs) {
int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2];
if (ni < 0 || ni >= cx ||
nj < 0 || nj >= cy ||
nk < 0 || nk >= cz) {
continue;
}
int nidx = ni * cy * cz + nj * cz + nk;
if (!visited[nidx] && !has_element[nidx]) {
visited[nidx] = true;
cell_empty[nidx] = true;
bfs.push({ni, nj, nk});
}
}
}
} else {
// dim!=2: cells without element overlap are empty
for (int c = 0; c < ncells; c++) {
cell_empty[c] = !has_element[c];
}
}
}
// create flex BVH
void mjCFlex::CreateBVH() {
int nbvh = 0;
src/user/user_objects.h
+3
View File
@@ -1055,6 +1055,9 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
void Compile(const mjVFS* vfs); // compiler
void CreateBVH(void); // create flex BVH
void CreateShellPair(void); // create shells and evpairs
void ComputeCellEmpty(const double* vpos, const int* elems, // identify cells
int nv, int ne, int fdim, // with no mesh content
const double* bbox = nullptr); // optional precomputed bbox
std::vector<double> vert0_; // vertex positions in [0, 1]^d in the bounding box
std::vector<double> node0_; // node Cartesian positions
src/xml/xml_native_reader.cc
+1 -1
View File
@@ -1553,7 +1553,7 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
flex->internal = (n == 1);
}
MapValue(cont, "selfcollide", &flex->selfcollide, flexself_map, 5);
if (MapValue(cont, "passive", &flex->passive, bool_map, 2)) {
if (MapValue(cont, "passive", &n, bool_map, 2)) {
flex->passive = (n == 1);
}
ReadAttrInt(cont, "activelayers", &flex->activelayers);
src/xml/xml_native_writer.cc
+2 -1
View File
@@ -190,6 +190,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
WriteAttrKey(cont, "internal", bool_map, 2, flex->internal, defflex.internal);
WriteAttrKey(cont, "selfcollide", flexself_map, 5, flex->selfcollide, defflex.selfcollide);
WriteAttrInt(cont, "activelayers", flex->activelayers, defflex.activelayers);
WriteAttrKey(cont, "passive", bool_map, 2, flex->passive, defflex.passive);
// remove contact is no attributes
if (!cont->FirstAttribute()) {
@@ -202,7 +203,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson);
WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness);
WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping);
WriteAttrKey(elastic, "elastic2d", elastic2d_map, 2, flex->elastic2d, defflex.elastic2d);
WriteAttrKey(elastic, "elastic2d", elastic2d_map, 4, flex->elastic2d, defflex.elastic2d);
// edge subelement
XMLElement* edge = InsertEnd(elem, "edge");
test/fixture.cc
+10 -3
View File
@@ -251,10 +251,17 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2,
// compare arrays, apart from bvh-related ones (which includes flex_vert0), as
// those are sensitive to numerical differences when meshes are perfectly
// symmetric.
// symmetric. Also skip flex fields derived from node local positions and
// cell geometry that are not fully serialized to XML.
#define X(type, name, nr, nc) \
if (strncmp(#name, "bvh_", 4) && strncmp(#name, "flex_vert0", 4) && \
strncmp(#name, "mesh_poly", 4)) { \
if (strncmp(#name, "bvh_", 4) && \
strncmp(#name, "flex_vert", 9) && \
strncmp(#name, "mesh_poly", 9) && \
strcmp(#name, "flex_centered") && \
strcmp(#name, "flex_size") && \
strcmp(#name, "flexedge_length0") && \
strcmp(#name, "flexedge_invweight0") && \
strncmp(#name, "flex_node", 9)) { \
for (int r = 0; r < m1->nr; r++) { \
for (int c = 0; c < nc; c++) { \
dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \