Enable interior nodes for interpolated flex shell mode.
Previously shell mode required cellcount=1 along at least one axis. This CL adds support for cellcount > 1 in all three axes by pinning interior grid nodes to the parent body and reconstructing their positions from boundary nodes via Transfinite Interpolation (TFI). PiperOrigin-RevId: 924314800 Change-Id: I8c2438f4866dd4133feed65f535a1ab69f0c9188
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
4548e81e4d
commit
91c92279d2
@@ -0,0 +1,43 @@
|
||||
<!-- Copyright 2024 DeepMind Technologies Limited
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<mujoco model="Trilinear">
|
||||
<include file="scene.xml"/>
|
||||
|
||||
<option solver="CG" tolerance="1e-6" integrator="implicitfast"/>
|
||||
|
||||
<size memory="10M"/>
|
||||
|
||||
<visual>
|
||||
<map stiffness="100"/>
|
||||
</visual>
|
||||
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="press" type="slide" axis="0 0 1" damping="500"/>
|
||||
<geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
|
||||
</body>
|
||||
<flexcomp type="mesh" file="bunny.obj" pos="0 0 0" dim="2" euler="90 0 0" cellcount="3 3 3"
|
||||
radius=".001" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="trilinear">
|
||||
<elasticity young="1e3" poisson="0.1" elastic2d="bend" thickness="0.02"/>
|
||||
<edge equality="strain"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -277,10 +277,18 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
mju_addToScl3(coord, m->flex_vert0 + 3*v[i], mju_abs(vweight[i]));
|
||||
}
|
||||
|
||||
int order = m->flex_interp[f];
|
||||
order = order < 0 ? -order : order;
|
||||
int interp = m->flex_interp[f];
|
||||
int order = interp < 0 ? -interp : interp;
|
||||
int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell
|
||||
|
||||
// grid dimensions for shell mode
|
||||
int nx = 0, ny = 0, nz = 0;
|
||||
if (interp < 0) {
|
||||
nx = m->flex_cellnum[3*f+0] * order + 1;
|
||||
ny = m->flex_cellnum[3*f+1] * order + 1;
|
||||
nz = m->flex_cellnum[3*f+2] * order + 1;
|
||||
}
|
||||
|
||||
// cell lookup: get local coords and node indices
|
||||
mjtNum local[3];
|
||||
int nodeindices[27]; // max npc for quadratic: 3^3 = 27
|
||||
@@ -290,14 +298,46 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
int nb = 0;
|
||||
|
||||
if (!m->flex_nodebodyid) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (npc > 27) {
|
||||
for (int j = 0; j < npc; j++) {
|
||||
mjtNum w = mju_evalBasis(local, j, order);
|
||||
if (w < 1e-5) {
|
||||
continue;
|
||||
}
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]];
|
||||
|
||||
int idx = nodeindices[j];
|
||||
|
||||
// shell mode: map interior nodes to boundary
|
||||
if (interp < 0) {
|
||||
int k_idx = idx % nz;
|
||||
int rest = idx / nz;
|
||||
int j_idx = rest % ny;
|
||||
int i_idx = rest / ny;
|
||||
|
||||
if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) {
|
||||
mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// add node, check for duplicates (especially needed when combining with TFI)
|
||||
int b = m->flex_nodebodyid[nstart + idx];
|
||||
int found = 0;
|
||||
for (int k = 0; k < nb; k++) {
|
||||
if (body[k] == b) {
|
||||
if (bweight) bweight[k] += sign * w;
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = b;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
mjtNum basis[27];
|
||||
@@ -308,11 +348,40 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
|
||||
if (w < 1e-5) {
|
||||
continue;
|
||||
}
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]];
|
||||
|
||||
int idx = nodeindices[j];
|
||||
|
||||
// shell mode: map interior nodes to boundary
|
||||
if (interp < 0) {
|
||||
int k_idx = idx % nz;
|
||||
int rest = idx / nz;
|
||||
int j_idx = rest % ny;
|
||||
int i_idx = rest / ny;
|
||||
|
||||
if (i_idx > 0 && i_idx < nx-1 && j_idx > 0 && j_idx < ny-1 && k_idx > 0 && k_idx < nz-1) {
|
||||
mju_shellTFIWeights(nx, ny, nz, i_idx, j_idx, k_idx, sign * w, &nb, body, bweight, m->flex_nodebodyid, nstart);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// add node, check for duplicates (especially needed when combining with TFI)
|
||||
int b = m->flex_nodebodyid[nstart + idx];
|
||||
int found = 0;
|
||||
for (int k = 0; k < nb; k++) {
|
||||
if (body[k] == b) {
|
||||
if (bweight) bweight[k] += sign * w;
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
if (bweight) bweight[nb] = sign * w;
|
||||
body[nb++] = b;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return nb;
|
||||
}
|
||||
|
||||
|
||||
@@ -66,10 +66,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d);
|
||||
void mj_instantiateContact(const mjModel* m, mjData* d);
|
||||
|
||||
// compute Jacobian for contact, return number of DOFs affected
|
||||
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||
mjtNum* jac1r, mjtNum* jac2r, int* chain);
|
||||
MJAPI int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||
mjtNum* jac1r, mjtNum* jac2r, int* chain);
|
||||
|
||||
|
||||
//------------------------ parameter computation/extraction ----------------------------------------
|
||||
|
||||
@@ -607,6 +607,11 @@ void mj_flex(const mjModel* m, mjData* d) {
|
||||
mjERROR("flex_interp_order mismatch");
|
||||
}
|
||||
|
||||
// shell mode: reconstruct interior node positions from boundary via TFI
|
||||
if (interp < 0) {
|
||||
mju_shellTrackInterior(nodexpos, nx_g, ny_g, nz_g);
|
||||
}
|
||||
|
||||
for (int i=vstart; i < vend; i++) {
|
||||
mju_zero3(d->flexvert_xpos+3*i);
|
||||
|
||||
|
||||
@@ -1020,6 +1020,23 @@ void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos,
|
||||
mju_addTo3(vel + 3*i, cross);
|
||||
}
|
||||
}
|
||||
|
||||
// shell mode: reconstruct interior node positions and velocities via TFI
|
||||
int interp = m->flex_interp[f];
|
||||
if (interp < 0) {
|
||||
int order = -interp;
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int nx_g = cx * order + 1;
|
||||
int ny_g = cy * order + 1;
|
||||
int nz_g = cz * order + 1;
|
||||
|
||||
mju_shellTrackInterior(xpos, nx_g, ny_g, nz_g);
|
||||
if (vel) {
|
||||
mju_shellTrackInterior(vel, nx_g, ny_g, nz_g);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -898,6 +898,151 @@ void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3],
|
||||
}
|
||||
|
||||
|
||||
// helper: get nodexpos value for node (i,j,k) in an nx*ny*nz grid
|
||||
static inline const mjtNum* nodeAt(const mjtNum* nodexpos, int ny, int nz, int i, int j, int k) {
|
||||
return nodexpos + 3*(i*ny*nz + j*nz + k);
|
||||
}
|
||||
|
||||
// reconstruct interior node positions from boundary nodes via Transfinite Interpolation
|
||||
void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz) {
|
||||
// need at least 3 nodes in each direction to have interior nodes
|
||||
if (nx < 3 || ny < 3 || nz < 3) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 1; i < nx-1; i++) {
|
||||
for (int j = 1; j < ny-1; j++) {
|
||||
for (int k = 1; k < nz-1; k++) {
|
||||
// parametric coordinates in [0, 1]
|
||||
mjtNum s = (mjtNum)i / (nx-1);
|
||||
mjtNum t = (mjtNum)j / (ny-1);
|
||||
mjtNum u = (mjtNum)k / (nz-1);
|
||||
|
||||
mjtNum result[3] = {0, 0, 0};
|
||||
|
||||
// --- face contributions (bilinear interpolation on each face pair) ---
|
||||
// x-faces: i=0 and i=nx-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-s) * nodeAt(nodexpos, ny, nz, 0, j, k)[d]
|
||||
+ s * nodeAt(nodexpos, ny, nz, nx-1, j, k)[d];
|
||||
}
|
||||
// y-faces: j=0 and j=ny-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-t) * nodeAt(nodexpos, ny, nz, i, 0, k)[d]
|
||||
+ t * nodeAt(nodexpos, ny, nz, i, ny-1, k)[d];
|
||||
}
|
||||
// z-faces: k=0 and k=nz-1
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-u) * nodeAt(nodexpos, ny, nz, i, j, 0)[d]
|
||||
+ u * nodeAt(nodexpos, ny, nz, i, j, nz-1)[d];
|
||||
}
|
||||
|
||||
// --- edge corrections (subtract 12 edges, each linearly interpolated) ---
|
||||
// edges along x (4 edges: (j,k) at corners of y-z face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-t)*(1-u) * nodeAt(nodexpos, ny, nz, i, 0, 0)[d];
|
||||
result[d] -= (1-t)* u * nodeAt(nodexpos, ny, nz, i, 0, nz-1)[d];
|
||||
result[d] -= t *(1-u) * nodeAt(nodexpos, ny, nz, i, ny-1, 0)[d];
|
||||
result[d] -= t * u * nodeAt(nodexpos, ny, nz, i, ny-1, nz-1)[d];
|
||||
}
|
||||
// edges along y (4 edges: (i,k) at corners of x-z face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-s)*(1-u) * nodeAt(nodexpos, ny, nz, 0, j, 0)[d];
|
||||
result[d] -= (1-s)* u * nodeAt(nodexpos, ny, nz, 0, j, nz-1)[d];
|
||||
result[d] -= s *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, j, 0)[d];
|
||||
result[d] -= s * u * nodeAt(nodexpos, ny, nz, nx-1, j, nz-1)[d];
|
||||
}
|
||||
// edges along z (4 edges: (i,j) at corners of x-y face)
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] -= (1-s)*(1-t) * nodeAt(nodexpos, ny, nz, 0, 0, k)[d];
|
||||
result[d] -= (1-s)* t * nodeAt(nodexpos, ny, nz, 0, ny-1, k)[d];
|
||||
result[d] -= s *(1-t) * nodeAt(nodexpos, ny, nz, nx-1, 0, k)[d];
|
||||
result[d] -= s * t * nodeAt(nodexpos, ny, nz, nx-1, ny-1, k)[d];
|
||||
}
|
||||
|
||||
// --- corner corrections (add 8 corners back) ---
|
||||
for (int d = 0; d < 3; d++) {
|
||||
result[d] += (1-s)*(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, 0, 0, 0)[d];
|
||||
result[d] += (1-s)*(1-t)* u * nodeAt(nodexpos, ny, nz, 0, 0, nz-1)[d];
|
||||
result[d] += (1-s)* t *(1-u) * nodeAt(nodexpos, ny, nz, 0, ny-1, 0)[d];
|
||||
result[d] += (1-s)* t * u * nodeAt(nodexpos, ny, nz, 0, ny-1, nz-1)[d];
|
||||
result[d] += s *(1-t)*(1-u) * nodeAt(nodexpos, ny, nz, nx-1, 0, 0)[d];
|
||||
result[d] += s *(1-t)* u * nodeAt(nodexpos, ny, nz, nx-1, 0, nz-1)[d];
|
||||
result[d] += s * t *(1-u) * nodeAt(nodexpos, ny, nz, nx-1, ny-1, 0)[d];
|
||||
result[d] += s * t * u * nodeAt(nodexpos, ny, nz, nx-1, ny-1, nz-1)[d];
|
||||
}
|
||||
|
||||
// write result to interior node
|
||||
mju_copy3(nodexpos + 3*(i*ny*nz + j*nz + k), result);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// helper to accumulate weights in a sparse list
|
||||
static void addWeight(int* nb, int* body, mjtNum* bweight, int b, mjtNum w) {
|
||||
for (int i = 0; i < *nb; i++) {
|
||||
if (body[i] == b) {
|
||||
if (bweight) {
|
||||
bweight[i] += w;
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
body[*nb] = b;
|
||||
if (bweight) {
|
||||
bweight[*nb] = w;
|
||||
}
|
||||
(*nb)++;
|
||||
}
|
||||
|
||||
// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes
|
||||
void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k,
|
||||
mjtNum w, int* nb, int* body, mjtNum* bweight,
|
||||
const int* nodebodyid, int nstart) {
|
||||
mjtNum s = (mjtNum)i / (nx-1);
|
||||
mjtNum t = (mjtNum)j / (ny-1);
|
||||
mjtNum u = (mjtNum)k / (nz-1);
|
||||
|
||||
// face contributions
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + k], w * (1-s));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + k], w * s);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + k], w * (1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + k], w * t);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + 0], w * (1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + j*nz + (nz-1)], w * u);
|
||||
|
||||
// edge corrections
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + 0], -w * (1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + 0*nz + (nz-1)], -w * (1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + 0], -w * t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + i*ny*nz + (ny-1)*nz + (nz-1)], -w * t*u);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + 0], -w * (1-s)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + j*nz + (nz-1)], -w * (1-s)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + 0], -w * s*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + j*nz + (nz-1)], -w * s*u);
|
||||
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + k], -w * (1-s)*(1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + k], -w * (1-s)*t);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + k], -w * s*(1-t));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + k], -w * s*t);
|
||||
|
||||
// corner corrections
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + 0], w * (1-s)*(1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + 0*nz + (nz-1)], w * (1-s)*(1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + 0], w * (1-s)*t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + 0*ny*nz + (ny-1)*nz + (nz-1)], w * (1-s)*t*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + 0], w * s*(1-t)*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + 0*nz + (nz-1)], w * s*(1-t)*u);
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + 0], w * s*t*(1-u));
|
||||
addWeight(nb, body, bweight, nodebodyid[nstart + (nx-1)*ny*nz + (ny-1)*nz + (nz-1)], w * s*t*u);
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ actuator models ---------------------------------------------------
|
||||
|
||||
// normalized muscle length-gain curve
|
||||
|
||||
@@ -147,6 +147,14 @@ static inline mjtNum mju_flexDphi(mjtNum s, int i, int order) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
// reconstruct interior node positions from boundary nodes via Transfinite Interpolation
|
||||
MJAPI void mju_shellTrackInterior(mjtNum* nodexpos, int nx, int ny, int nz);
|
||||
|
||||
// compute TFI weights for an interior node (i,j,k) and distribute to boundary nodes
|
||||
MJAPI void mju_shellTFIWeights(int nx, int ny, int nz, int i, int j, int k,
|
||||
mjtNum w, int* nb, int* body, mjtNum* bweight,
|
||||
const int* nodebodyid, int nstart);
|
||||
|
||||
|
||||
// ----------------------------- Base64 ------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -873,14 +873,21 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||
mju_cellLookup(coord, m->flex_cellnum+3*i, order, loc, nodeindices);
|
||||
|
||||
// find node with largest weight in this cell
|
||||
// in shell mode, skip interior nodes (pinned to worldbody)
|
||||
int nodeid = -1;
|
||||
int nstart = m->flex_nodeadr[i];
|
||||
mjtNum w = 0;
|
||||
int shell_mode = m->flex_interp[i] < 0;
|
||||
for (int j = 0; j < npc; j++) {
|
||||
mjtNum ww = mju_evalBasis(loc, j, order);
|
||||
int nid = nodeindices[j];
|
||||
// skip interior nodes in shell mode (they map to worldbody)
|
||||
if (shell_mode && m->body_dofnum[m->flex_nodebodyid[nstart + nid]] == 0) {
|
||||
continue;
|
||||
}
|
||||
if (ww > w) {
|
||||
w = ww;
|
||||
nodeid = nodeindices[j];
|
||||
nodeid = nid;
|
||||
}
|
||||
}
|
||||
flexbodyid = m->flex_nodebodyid[nstart + nodeid];
|
||||
|
||||
@@ -1496,6 +1496,8 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
int NY = cy * order + 1;
|
||||
int NZ = cz * order + 1;
|
||||
|
||||
int shell_mode = m->flex_interp[f] < 0;
|
||||
|
||||
for (int i=0; i < NX; i++) {
|
||||
for (int j=0; j < NY; j++) {
|
||||
for (int k=0; k < NZ; k++) {
|
||||
@@ -1506,36 +1508,58 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
continue;
|
||||
}
|
||||
|
||||
// shell mode: skip interior nodes entirely
|
||||
int is_boundary = (i == 0 || i == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (shell_mode && !is_boundary) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int offset = 3*n0;
|
||||
int offset1 = 3*((i+1)*NY*NZ + j*NZ + k);
|
||||
int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k);
|
||||
int offset3 = 3*(i*NY*NZ + j*NZ + (k+1));
|
||||
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
// edge along i: draw if neighbor is also on boundary (shell) or has joints
|
||||
if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
|
||||
int nb_boundary = ((i+1) == 0 || (i+1) == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
}
|
||||
if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
int nb_boundary = (i == 0 || i == NX-1 ||
|
||||
(j+1) == 0 || (j+1) == NY-1 ||
|
||||
k == 0 || k == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
int nb_boundary = (i == 0 || i == NX-1 ||
|
||||
j == 0 || j == NY-1 ||
|
||||
(k+1) == 0 || (k+1) == NZ-1);
|
||||
if (!shell_mode || nb_boundary) {
|
||||
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
|
||||
if (!thisgeom) {
|
||||
return;
|
||||
}
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
|
||||
releaseGeom(&thisgeom, scn);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -644,8 +644,26 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
|
||||
int nz = flex->spec.cellcount[2] * flex->spec.order + 1;
|
||||
int nnode = nx * ny * nz;
|
||||
|
||||
// mark empty cells and pin nodes exclusively in empty cells
|
||||
MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz);
|
||||
// mark empty cells and pin nodes exclusively in empty cells (volume mode only)
|
||||
if (!dflex->elastic2d) {
|
||||
MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz);
|
||||
}
|
||||
|
||||
// shell mode: pin all interior (non-boundary) nodes
|
||||
if (dflex->elastic2d) {
|
||||
for (int gi = 0; gi < nx; gi++) {
|
||||
for (int gj = 0; gj < ny; gj++) {
|
||||
for (int gk = 0; gk < nz; gk++) {
|
||||
bool is_boundary = (gi == 0 || gi == nx-1 ||
|
||||
gj == 0 || gj == ny-1 ||
|
||||
gk == 0 || gk == nz-1);
|
||||
if (!is_boundary) {
|
||||
pinned[gi*ny*nz + gj*nz + gk] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if MarkEmptyCells pinned any nodes, force centered=false
|
||||
// so that pf->node (local positions) is saved to the model
|
||||
|
||||
@@ -4698,11 +4698,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) {
|
||||
throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0");
|
||||
}
|
||||
if (elastic2d && !(spec.cellcount[0] == 1 || spec.cellcount[1] == 1 || spec.cellcount[2] == 1)) {
|
||||
throw mjCError(this,
|
||||
"shell trilinear flex requires at least one dimension "
|
||||
"with cell count equal to one (no interior nodes)");
|
||||
}
|
||||
|
||||
int expected_nodes = (spec.cellcount[0] * spec.order + 1) *
|
||||
(spec.cellcount[1] * spec.order + 1) *
|
||||
(spec.cellcount[2] * spec.order + 1);
|
||||
@@ -4944,9 +4940,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
// create shell fragments and element-vertex collision pairs
|
||||
CreateShellPair();
|
||||
|
||||
// recompute cell_empty from vertex/element geometry
|
||||
// recompute cell_empty from vertex/element geometry (volume mode only)
|
||||
// (survives XML round-trips where flexcomp data is lost)
|
||||
if (interpolated && cell_empty.empty()) {
|
||||
if (interpolated && !elastic2d && 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);
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
// Tests for engine/engine_core_constraint.c.
|
||||
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -1091,5 +1092,103 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
}
|
||||
);
|
||||
|
||||
TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option jacobian="dense"/>
|
||||
<worldbody>
|
||||
<flexcomp name="flex" type="grid" count="3 3 3" spacing=".1 .1 .1" dim="3" dof="trilinear">
|
||||
<elasticity elastic2d="stretch" thickness="0.01"/>
|
||||
<contact selfcollide="none"/>
|
||||
</flexcomp>
|
||||
<geom type="plane" size="1 1 1" pos="0 0 -1"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model, testing::NotNull()) << error;
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
mj_forward(model, data);
|
||||
|
||||
// find central vertex index (13 for 3x3x3 grid)
|
||||
int central_idx = 13;
|
||||
|
||||
// verify it is interior
|
||||
int nx = 3, ny = 3, nz = 3;
|
||||
int k = central_idx / (nx * ny);
|
||||
int rest = central_idx % (nx * ny);
|
||||
int j = rest / nx;
|
||||
int i = rest % nx;
|
||||
ASSERT_TRUE(i > 0 && i < nx-1 && j > 0 && j < ny-1 && k > 0 && k < nz-1);
|
||||
|
||||
// create manual contact with central vertex
|
||||
mjContact con;
|
||||
memset(&con, 0, sizeof(mjContact));
|
||||
con.flex[0] = -1;
|
||||
con.flex[1] = -1;
|
||||
con.vert[0] = -1;
|
||||
con.vert[1] = -1;
|
||||
con.geom[0] = model->ngeom - 1; // plane geom
|
||||
con.geom[1] = -1; // must be -1 to trigger flex branch in mj_contactJacobian
|
||||
con.flex[1] = 0;
|
||||
con.vert[1] = central_idx;
|
||||
con.dim = 1;
|
||||
mju_copy3(con.pos, data->flexvert_xpos + 3*central_idx);
|
||||
con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal
|
||||
|
||||
// buffer for Jacobian
|
||||
std::vector<mjtNum> jacdif(3*model->nv, 0.0);
|
||||
|
||||
// call mj_contactJacobian
|
||||
mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(),
|
||||
nullptr, nullptr, nullptr,
|
||||
nullptr, nullptr, nullptr,
|
||||
nullptr);
|
||||
|
||||
// check that boundary nodes have non-zero entries, and central node has zero
|
||||
|
||||
|
||||
bool boundary_has_dof = false;
|
||||
bool interior_has_dof = false;
|
||||
|
||||
for (int n = 0; n < model->flex_nodenum[0]; n++) {
|
||||
int b = model->flex_nodebodyid[model->flex_nodeadr[0] + n];
|
||||
int dofadr = model->body_dofadr[b];
|
||||
int dofnum = model->body_dofnum[b];
|
||||
|
||||
bool has_jac = false;
|
||||
if (dofadr >= 0) {
|
||||
for (int d = 0; d < dofnum; d++) {
|
||||
if (mju_abs(jacdif[dofadr + d]) > 1e-6) {
|
||||
has_jac = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int kn = n / (nx * ny);
|
||||
int restn = n % (nx * ny);
|
||||
int jn = restn / nx;
|
||||
int in = restn % nx;
|
||||
bool is_interior = (in > 0 && in < nx - 1 && jn > 0 && jn < ny - 1 &&
|
||||
kn > 0 && kn < nz - 1);
|
||||
|
||||
if (is_interior) {
|
||||
if (has_jac) interior_has_dof = true;
|
||||
} else {
|
||||
if (has_jac) boundary_has_dof = true;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_TRUE(boundary_has_dof)
|
||||
<< "Boundary nodes should receive contact force";
|
||||
EXPECT_FALSE(interior_has_dof)
|
||||
<< "Interior nodes should not receive contact force";
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -88,6 +88,55 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<flexcomp name="flex0" type="grid" count="3 3 3" spacing=".1 .1 .1"
|
||||
dim="3" mass="1" radius="0.01" dof="trilinear" cellcount="2 2 2">
|
||||
<elasticity young="5e4" poisson="0.2" elastic2d="stretch" thickness="0.02"/>
|
||||
<contact selfcollide="none"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024];
|
||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
ASSERT_EQ(model->nflex, 1);
|
||||
int f = 0;
|
||||
model->flex_interp[f] = -1;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mj_forward(model, data);
|
||||
|
||||
int nodenum = model->flex_nodenum[f];
|
||||
int nstart = model->flex_nodeadr[f];
|
||||
|
||||
// Move boundary nodes, keep interior node stuck (it is pinned)
|
||||
mjtNum shift[3] = {0.1, 0.2, 0.3};
|
||||
for (int i = 0; i < nodenum; i++) {
|
||||
if (i == 13) continue; // Skip center node
|
||||
int b = model->flex_nodebodyid[nstart + i];
|
||||
data->xpos[3*b + 0] += shift[0];
|
||||
data->xpos[3*b + 1] += shift[1];
|
||||
data->xpos[3*b + 2] += shift[2];
|
||||
}
|
||||
|
||||
std::vector<mjtNum> xpos(3 * nodenum);
|
||||
mju_flexGatherState(model, data, f, xpos.data(), NULL);
|
||||
|
||||
// Verify that gathered xpos for center node (13) is the TFI reconstructed position
|
||||
EXPECT_NEAR(xpos[3*13 + 0], shift[0], 1e-5);
|
||||
EXPECT_NEAR(xpos[3*13 + 1], shift[1], 1e-5);
|
||||
EXPECT_NEAR(xpos[3*13 + 2], shift[2], 1e-5);
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
using AngMomMatTest = MujocoTest;
|
||||
|
||||
|
||||
@@ -1550,5 +1550,167 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) {
|
||||
}
|
||||
}
|
||||
|
||||
// ------------------------------ Shell TFI Interpolation ----------------------
|
||||
|
||||
using ShellTFITest = MujocoTest;
|
||||
|
||||
// helper: set up a regular nx*ny*nz grid with positions at grid indices
|
||||
static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) {
|
||||
for (int i = 0; i < nx; i++) {
|
||||
for (int j = 0; j < ny; j++) {
|
||||
for (int k = 0; k < nz; k++) {
|
||||
int idx = i*ny*nz + j*nz + k;
|
||||
nodexpos[3*idx+0] = (mjtNum)i;
|
||||
nodexpos[3*idx+1] = (mjtNum)j;
|
||||
nodexpos[3*idx+2] = (mjtNum)k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(ShellTFITest, IdentityGrid) {
|
||||
// 3x3x3 grid: 1 interior node at (1,1,1)
|
||||
constexpr int nx = 3, ny = 3, nz = 3;
|
||||
mjtNum nodexpos[3*nx*ny*nz];
|
||||
MakeRegularGrid(nodexpos, nx, ny, nz);
|
||||
|
||||
// save expected interior position
|
||||
mjtNum expected[3] = {1.0, 1.0, 1.0};
|
||||
|
||||
// run TFI
|
||||
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
||||
|
||||
// interior node at (1,1,1) should match
|
||||
int idx = 1*ny*nz + 1*nz + 1;
|
||||
EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-5));
|
||||
EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-5));
|
||||
EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-5));
|
||||
}
|
||||
|
||||
TEST_F(ShellTFITest, UniformScaling) {
|
||||
// 3x3x3: scale all boundary nodes by 2x, interior should follow
|
||||
constexpr int nx = 3, ny = 3, nz = 3;
|
||||
mjtNum nodexpos[3*nx*ny*nz];
|
||||
MakeRegularGrid(nodexpos, nx, ny, nz);
|
||||
|
||||
// scale all nodes
|
||||
for (int i = 0; i < 3*nx*ny*nz; i++) {
|
||||
nodexpos[i] *= 2.0;
|
||||
}
|
||||
|
||||
// run TFI — interior should be reconstructed to 2*original
|
||||
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
||||
|
||||
int idx = 1*ny*nz + 1*nz + 1;
|
||||
EXPECT_NEAR(nodexpos[3*idx+0], 2.0, MjTol(1e-12, 1e-5));
|
||||
EXPECT_NEAR(nodexpos[3*idx+1], 2.0, MjTol(1e-12, 1e-5));
|
||||
EXPECT_NEAR(nodexpos[3*idx+2], 2.0, MjTol(1e-12, 1e-5));
|
||||
}
|
||||
|
||||
TEST_F(ShellTFITest, AffineDeformation) {
|
||||
// 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary,
|
||||
// then verify TFI reproduces the same affine transform on interior nodes.
|
||||
constexpr int nx = 4, ny = 4, nz = 4;
|
||||
mjtNum nodexpos[3*nx*ny*nz];
|
||||
MakeRegularGrid(nodexpos, nx, ny, nz);
|
||||
|
||||
// affine: F(x,y,z) = A*[x,y,z]^T + b
|
||||
// A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30]
|
||||
auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) {
|
||||
out[0] = 2.0*x + 0.5*y + 10.0;
|
||||
out[1] = 0.3*x + 1.5*y + 20.0;
|
||||
out[2] = z + 30.0;
|
||||
};
|
||||
|
||||
// apply affine to all nodes
|
||||
for (int i = 0; i < nx; i++) {
|
||||
for (int j = 0; j < ny; j++) {
|
||||
for (int k = 0; k < nz; k++) {
|
||||
int idx = i*ny*nz + j*nz + k;
|
||||
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3*idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// corrupt interior nodes to verify TFI actually reconstructs them
|
||||
for (int i = 1; i < nx-1; i++) {
|
||||
for (int j = 1; j < ny-1; j++) {
|
||||
for (int k = 1; k < nz-1; k++) {
|
||||
int idx = i*ny*nz + j*nz + k;
|
||||
nodexpos[3*idx+0] = -999;
|
||||
nodexpos[3*idx+1] = -999;
|
||||
nodexpos[3*idx+2] = -999;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// run TFI
|
||||
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
||||
|
||||
// check all interior nodes match affine
|
||||
for (int i = 1; i < nx-1; i++) {
|
||||
for (int j = 1; j < ny-1; j++) {
|
||||
for (int k = 1; k < nz-1; k++) {
|
||||
int idx = i*ny*nz + j*nz + k;
|
||||
mjtNum expected[3];
|
||||
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected);
|
||||
EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-4))
|
||||
<< "i=" << i << " j=" << j << " k=" << k;
|
||||
EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-4))
|
||||
<< "i=" << i << " j=" << j << " k=" << k;
|
||||
EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-4))
|
||||
<< "i=" << i << " j=" << j << " k=" << k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(ShellTFITest, BoundaryUnmodified) {
|
||||
// verify that boundary nodes are not modified by TFI
|
||||
constexpr int nx = 4, ny = 4, nz = 4;
|
||||
mjtNum nodexpos[3*nx*ny*nz];
|
||||
MakeRegularGrid(nodexpos, nx, ny, nz);
|
||||
|
||||
// save boundary node values
|
||||
mjtNum saved[3*nx*ny*nz];
|
||||
mju_copy(saved, nodexpos, 3*nx*ny*nz);
|
||||
|
||||
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
||||
|
||||
// check all boundary nodes unchanged
|
||||
for (int i = 0; i < nx; i++) {
|
||||
for (int j = 0; j < ny; j++) {
|
||||
for (int k = 0; k < nz; k++) {
|
||||
bool is_boundary = (i == 0 || i == nx-1 ||
|
||||
j == 0 || j == ny-1 ||
|
||||
k == 0 || k == nz-1);
|
||||
if (is_boundary) {
|
||||
int idx = i*ny*nz + j*nz + k;
|
||||
EXPECT_EQ(nodexpos[3*idx+0], saved[3*idx+0]);
|
||||
EXPECT_EQ(nodexpos[3*idx+1], saved[3*idx+1]);
|
||||
EXPECT_EQ(nodexpos[3*idx+2], saved[3*idx+2]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(ShellTFITest, NoInteriorSmallGrid) {
|
||||
// 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op
|
||||
constexpr int nx = 2, ny = 3, nz = 2;
|
||||
mjtNum nodexpos[3*nx*ny*nz];
|
||||
MakeRegularGrid(nodexpos, nx, ny, nz);
|
||||
|
||||
mjtNum saved[3*nx*ny*nz];
|
||||
mju_copy(saved, nodexpos, 3*nx*ny*nz);
|
||||
|
||||
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
||||
|
||||
// all nodes unchanged
|
||||
for (int i = 0; i < 3*nx*ny*nz; i++) {
|
||||
EXPECT_EQ(nodexpos[i], saved[i]);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
Reference in New Issue
Block a user