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

Implement mesh extrema in a 3x3x3 grid corresponding to each feature of a unit cube. These are used as seeds for a better initial point in mesh hill climbing with up to a 2x speedup in mjc_Convex.

Browse Source 
PiperOrigin-RevId: 959645299
Change-Id: I032ae534704e0cc440ddb7691fd21a29a359f521
This commit is contained in:
Kyle Bayes
2026-08-05 07:22:01 -07:00
committed by Copybara-Service
parent 1362a8bded
commit 83e621d771
14 changed files with 2708 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+1
View File
@@ -34,6 +34,7 @@ Actuation
Engine
^^^^^^
- Optimized large-mesh convex collision detection with up to 2x speedup in certain cases.
- Replaced the per-step sparse Cholesky factorization of the flex block of the implicit effective metric M + K with
its prefactored per-vertex 3x3 diagonal blocks. The blocks precondition the CG constraint solver and drive an
iterative solve for ``qacc_smooth``, which now converges on :ref:`tolerance<option-tolerance>` rather than a fixed
doc/includes/references.h
+1
View File
@@ -967,6 +967,7 @@ typedef struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of texcoord (nmesh x 1)
int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1)
int* mesh_extrema; // extremum vertices in 3x3x3 directions (nmesh x 27)
float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2)
include/mujoco/mjmodel.h
+1
View File
@@ -639,6 +639,7 @@ typedef struct mjModel_ {
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
int* mesh_texcoordnum; // number of texcoord (nmesh x 1)
int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1)
int* mesh_extrema; // extremum vertices in 3x3x3 directions (nmesh x 27)
float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3)
float* mesh_normal; // normals for all meshes (nmeshnormal x 3)
float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2)
include/mujoco/mjxmacro.h
+1
View File
@@ -544,6 +544,7 @@
X ( int, mesh_texcoordadr, nmesh, 1 ) \
X ( int, mesh_texcoordnum, nmesh, 1 ) \
X ( int, mesh_graphadr, nmesh, 1 ) \
X ( int, mesh_extrema, nmesh, 27 ) \
XNV ( float, mesh_vert, nmeshvert, 3 ) \
XNV ( float, mesh_normal, nmeshnormal, 3 ) \
XNV ( float, mesh_texcoord, nmeshtexcoord, 2 ) \
python/mujoco/introspect/structs.py
+8
View File
@@ -3452,6 +3452,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='graph data address; -1: no graph',
array_extent=('nmesh',),
),
StructFieldDecl(
name='mesh_extrema',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='extremum vertices in 3x3x3 directions',
array_extent=('nmesh', 27),
),
StructFieldDecl(
name='mesh_vert',
type=PointerType(
src/engine/engine_collision_convex.c
+20 -1
View File
@@ -407,7 +407,24 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
mulMatTVec3(local_dir, mat, dir);
int prev = -1;
int imax = obj->meshindex >= 0 ? obj->meshindex : 0;
int imax;
// map continuous direction to discrete 3x3x3 grid (-1, 0, 1) indices
int cx = (local_dir[0] > 0.4) - (local_dir[0] < -0.4) + 1;
int cy = (local_dir[1] > 0.4) - (local_dir[1] < -0.4) + 1;
int cz = (local_dir[2] > 0.4) - (local_dir[2] < -0.4) + 1;
int grid_idx = obj->data.mesh.extrema[cx*9 + cy*3 + cz];
if (obj->meshindex >= 0) {
// warm start: pick the better of cached vertex vs grid seed
mjtNum cached_dot = dot3f(local_dir, verts + 3*vert_globalid[obj->meshindex]);
mjtNum seed_dot = dot3f(local_dir, verts + 3*vert_globalid[grid_idx]);
imax = (seed_dot > cached_dot) ? grid_idx : obj->meshindex;
} else {
// cold start: use grid seed
imax = grid_idx;
}
mjtNum max = dot3f(local_dir, verts + 3*vert_globalid[imax]);
// hillclimb until no change
@@ -734,9 +751,11 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
polyadr = m->mesh_polyadr[m->geom_dataid[g]];
if (graphadr < 0 || m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
obj->data.mesh.graph = NULL;
obj->data.mesh.extrema = NULL;
obj->support = mjc_meshSupport;
} else {
obj->data.mesh.graph = m->mesh_graph + graphadr;
obj->data.mesh.extrema = m->mesh_extrema + 27 * m->geom_dataid[g];
obj->support = mjc_hillclimbSupport;
}
obj->data.mesh.vert = m->mesh_vert + 3*vertadr;
src/engine/engine_collision_convex.h
+2 -1
View File
@@ -62,7 +62,8 @@ struct _mjCCDObj {
const int* polyvertnum;
const int* polyvert;
const mjtNum* polynormal;
const int*graph;
const int* graph;
const int* extrema;
} mesh;
// hfield prism data
src/user/user_model.cc
+37
View File
@@ -26,6 +26,7 @@
#include <exception>
#include <filesystem> // NOLINT(build/c++17)
#include <functional>
#include <limits>
#include <mutex>
#include <set>
#include <sstream>
@@ -3534,9 +3535,45 @@ void mjCModel::CopyObjects(mjModel* m) {
} else {
memset(m->mesh_facetexcoord + 3*face_adr, 0, 3*pme->nface()*sizeof(int));
}
memset(m->mesh_extrema + 27*i, 0, 27*sizeof(int));
if (pme->szgraph()) {
pme->CopyGraph(m->mesh_graph + graph_adr);
// compute grid extrema (local indices in graph)
float max_val[27];
for (int k = 0; k < 27; k++) {
max_val[k] = std::numeric_limits<float>::lowest();
}
const int* graph = m->mesh_graph + graph_adr;
int numgraphvert = graph[0];
const int* vert_globalid = graph + 2 + numgraphvert;
const float* verts = m->mesh_vert + 3*vert_adr;
// map the 27 features (8 vertices, 6 faces, 12 edges) of a unit cube to the farthest
// vertex in the mesh
for (int local_id = 0; local_id < numgraphvert; local_id++) {
int global_id = vert_globalid[local_id];
float x = verts[3 * global_id + 0];
float y = verts[3 * global_id + 1];
float z = verts[3 * global_id + 2];
int k = 0;
for (int cx = -1; cx <= 1; cx++) {
for (int cy = -1; cy <= 1; cy++) {
for (int cz = -1; cz <= 1; cz++) {
float dot = x * cx + y * cy + z * cz;
if (dot > max_val[k]) {
max_val[k] = dot;
m->mesh_extrema[27*i + k] = local_id;
}
k++;
}
}
}
}
}
pme->CopyPolygonNormals(m->mesh_polynormal + 3*poly_adr);
pme->CopyPolygons(m->mesh_polyvert + polyvert_adr, m->mesh_polyvertadr + poly_adr,
m->mesh_polyvertnum + poly_adr, polyvert_adr);
test/engine/testdata/collision_convex/100spheremesh.xml
Vendored
+18
View File
@@ -0,0 +1,18 @@
<mujoco model="100 sphere meshes in a box">
<option solver="CG" iterations="10"/>
<asset>
<mesh name="sphere" file="sphere.obj" scale=".1 .1 .1"/>
</asset>
<worldbody>
<geom name="ground" type="plane" size=".5 .5 .01" condim="1"/>
<replicate count="4" euler="0 0 90">
<geom type="plane" size=".1 .5 .01" zaxis="1 0 0" pos="-.5 0 .1"/>
</replicate>
<replicate count="100" offset="0 0 .5">
<body pos="0 0 1">
<freejoint/>
<geom type="mesh" mesh="sphere"/>
</body>
</replicate>
</worldbody>
</mujoco>
test/engine/testdata/collision_convex/sphere.obj
Vendored
+2563
View File
File diff suppressed because it is too large Load Diff
test/user/user_model_test.cc
+51
View File
@@ -350,6 +350,57 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1);
}
TEST_F(UserModelTest, MeshExtremaValidIndices) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="test_mesh" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 0 3 2 1 2 3"/>
</asset>
<worldbody>
<geom name="test_geom" type="mesh" mesh="test_mesh"/>
</worldbody>
</mujoco>)";
std::array<char, 1024> error;
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull()) << error.data();
int mesh_id = mj_name2id(model.get(), mjOBJ_MESH, "test_mesh");
EXPECT_NE(mesh_id, -1);
EXPECT_NE(model->mesh_graphadr[mesh_id], -1);
const float* verts = model->mesh_vert + 3 * model->mesh_vertadr[mesh_id];
const int* graph = model->mesh_graph + model->mesh_graphadr[mesh_id];
const int* vert_globalid = graph + 2 + graph[0];
int k = 0;
for (int cx = -1; cx <= 1; cx++) {
for (int cy = -1; cy <= 1; cy++) {
for (int cz = -1; cz <= 1; cz++) {
int v_idx = model->mesh_extrema[mesh_id * 27 + k];
EXPECT_GE(v_idx, 0);
EXPECT_LT(v_idx, graph[0]);
int global_id = vert_globalid[v_idx];
float max_dot = verts[3 * global_id + 0] * cx +
verts[3 * global_id + 1] * cy +
verts[3 * global_id + 2] * cz;
// verify no other vertex yields a strictly greater dot product.
for (int i = 0; i < graph[0]; i++) {
int other_gid = vert_globalid[i];
float dot = verts[3 * other_gid + 0] * cx +
verts[3 * other_gid + 1] * cy +
verts[3 * other_gid + 2] * cz;
EXPECT_LE(dot, max_dot);
}
k++;
}
}
}
}
TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
static constexpr char xml[] = R"(
<mujoco>
unity/Runtime/Bindings/MjBindings.cs
+1
View File
@@ -1387,6 +1387,7 @@ public unsafe struct mjModel_ {
public int* mesh_texcoordadr;
public int* mesh_texcoordnum;
public int* mesh_graphadr;
public int* mesh_extrema;
public float* mesh_vert;
public float* mesh_normal;
public float* mesh_texcoord;
wasm/codegen/generated/bindings.cc
+1
View File
@@ -5122,6 +5122,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
.property("mat_texuniform", &MjModel::mat_texuniform)
.property("mesh_bvhadr", &MjModel::mesh_bvhadr)
.property("mesh_bvhnum", &MjModel::mesh_bvhnum)
.property("mesh_extrema", &MjModel::mesh_extrema)
.property("mesh_face", &MjModel::mesh_face)
.property("mesh_faceadr", &MjModel::mesh_faceadr)
.property("mesh_facenormal", &MjModel::mesh_facenormal)
wasm/codegen/generated/bindings.h
+3
View File
@@ -5100,6 +5100,9 @@ struct MjModel {
emscripten::val mesh_graphadr() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nmesh, ptr_->mesh_graphadr));
}
emscripten::val mesh_extrema() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nmesh * 27, ptr_->mesh_extrema));
}
emscripten::val mesh_vert() const {
return emscripten::val(emscripten::typed_memory_view(ptr_->nmeshvert * 3, ptr_->mesh_vert));
}