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

Introduce trilinear flex parametrization.

Browse Source 
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh.

On an 8x8x8 cube, the performance using DOFs at all vertices is

```
 Simulation time      : 18.74 s
 Steps per second     : 533
 Realtime factor      : 0.53 x
 Time per step        : 1874.4 µs

 Contacts per step    : 114.88
 Constraints per step : 3322.51
 Degrees of freedom   : 1536
```

With the new implementation, it is the following:

```
 Simulation time      : 1.82 s
 Steps per second     : 5507
 Realtime factor      : 5.51 x
 Time per step        : 181.6 µs

 Contacts per step    : 38.84
 Constraints per step : 155.36
 Degrees of freedom   : 24
```

PiperOrigin-RevId: 721008829
Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
This commit is contained in:
Alessio Quaglino
2025-01-29 09:37:37 -08:00
committed by Copybara-Service
parent 1a4b821b6b
commit 7cdf180641
47 changed files with 8967 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_flexcomp.cc
+65 -12
View File
@@ -86,6 +86,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
centered = false;
doftype = mjFCOMPDOF_FULL;
mjs_defaultPlugin(&plugin);
mjs_defaultOrientation(&alt);
@@ -102,10 +103,6 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
mjCModel* model = static_cast<mjCBody*>(body->element)->model;
mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
mjsFlex* dflex = def.spec.flex;
bool radial = (type == mjFCOMPTYPE_BOX ||
type == mjFCOMPTYPE_CYLINDER ||
type == mjFCOMPTYPE_ELLIPSOID);
bool direct = (type == mjFCOMPTYPE_DIRECT ||
type == mjFCOMPTYPE_MESH ||
type == mjFCOMPTYPE_GMSH);
@@ -122,7 +119,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
// check counts
for (int i=0; i < 3; i++) {
if (count[i] < 1 || ((radial && count[i] < 2) && dflex->dim == 3)) {
if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
return comperr(error, "Count too small", error_sz);
}
}
@@ -260,6 +257,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
point[3*i+2] = newp[2];
}
// compute bounding box of points
double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
for (int i=0; i < npnt; i++) {
for (int j=0; j < 3; j++) {
minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
}
}
// construct pinned array
pinned = vector<bool>(npnt, rigid);
@@ -337,7 +343,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// center of radial body is always pinned
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
pinned[0] = true;
}
@@ -434,8 +440,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
continue;
}
// pinned: parent body
if (pinned[i]) {
// pinned or trilinear: parent body
if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_appendString(pf->vertbody, mjs_getString(body->name));
// add plugin
@@ -448,7 +454,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
// not pinned: new body
// not pinned and not trilinear: new body
else {
// add new body at vertex coordinates
mjsBody* pb = mjs_addBody(body, 0);
@@ -465,7 +471,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
pb->explicitinertial = true;
// add radial slider
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
// set properties
@@ -476,7 +482,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// add three orthogonal sliders
else {
else if (doftype == mjFCOMPDOF_FULL) {
for (int j=0; j < 3; j++) {
// add joint to body
mjsJoint* jnt = mjs_addJoint(pb, 0);
@@ -513,7 +519,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
if (!centered) {
// create nodal mesh for trilinear interpolation
if (doftype == mjFCOMPDOF_TRILINEAR) {
std::vector<double> node(24, 0);
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (pinned[i*4+j*2+k]) {
node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3];
node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4];
node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5];
mjs_appendString(pf->nodebody, mjs_getString(body->name));
continue;
}
mjsBody* pb = mjs_addBody(body, 0);
pb->pos[0] = i == 0 ? minmax[0] : minmax[3];
pb->pos[1] = j == 0 ? minmax[1] : minmax[4];
pb->pos[2] = k == 0 ? minmax[2] : minmax[5];
mjuu_zerovec(pb->ipos, 3);
pb->mass = mass / 8;
pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->explicitinertial = true;
for (int d=0; d < 3; d++) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[d] = 1;
}
// construct node name, add to nodebody
char txt[100];
mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k);
mjs_setString(pb->name, txt);
mjs_appendString(pf->nodebody, mjs_getString(pb->name));
}
}
}
if (!centered) {
mjs_setDouble(pf->node, node.data(), node.size());
}
}
if (!centered || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_setDouble(pf->vert, point.data(), point.size());
}