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
@@ -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