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

Add implicit bending stiffness for standard flex.

Browse Source 
Standard flex (flex_interp=0) with thin-plate bending treated bending forces purely explicitly. This caused contact-induced vertex vibrations and non-physical energy injection for flat resting sheets, because the solver treated each vertex as an independent mass during contact and contact normals are orthogonal to stretch constraints.

Fix: extend the existing preconditioned CG solver to include the constant bending stiffness K_bend in the implicit operator via matrix-free mat-vec.
PiperOrigin-RevId: 914774020
Change-Id: I45e0d6749abb6f873566203bccae956514b2576b
This commit is contained in:
Alessio Quaglino
2026-05-13 03:57:41 -07:00
committed by Copybara-Service
parent 7bfdbad80b
commit 35cdc779e6
7 changed files with 221 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
model/flex/poncho.xml
+2 -2
View File
@@ -15,7 +15,7 @@
<mujoco model="Poncho">
<include file="mannequin.xml"/>
<option timestep="0.005" integrator="implicitfast" viscosity=".3" solver="CG" tolerance="1e-6">
<option integrator="implicitfast" solver="CG" tolerance="1e-6">
<flag energy="enable"/>
</option>
@@ -1414,7 +1414,7 @@
398 399 418
398 376 378">
<edge equality="vert" damping="0.1"/>
<elasticity young="3e5" poisson="0" thickness="8e-3" elastic2d="bend"/>
<elasticity young="3e5" poisson="0" thickness="8e-3" elastic2d="bend" damping="0.02"/>
<contact solref="0.003"/>
</flexcomp>
model/flex/poncho_edgeequality.xml
+2 -2
View File
@@ -15,7 +15,7 @@
<mujoco model="Poncho">
<include file="mannequin.xml"/>
<option timestep="0.005" integrator="implicitfast" viscosity=".3" solver="CG" tolerance="1e-6">
<option integrator="implicitfast" solver="CG" tolerance="1e-6">
<flag energy="enable"/>
</option>
@@ -1414,7 +1414,7 @@
398 399 418
398 376 378">
<edge equality="true" damping="0.1"/>
<elasticity poisson="0" young="3e5" thickness="8e-3" elastic2d="bend"/>
<elasticity poisson="0" young="3e5" thickness="8e-3" elastic2d="bend" damping="0.02"/>
<contact solref="0.003"/>
</flexcomp>
src/engine/engine_derivative.c
+56 -10
View File
@@ -1127,25 +1127,71 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
// compute res += (h^2 + h*damping) * J'*K*J * vec, for all interpolated flexes
void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) {
// s1=h*h, s2=h => scale = h*h + h*damping
mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h * h, h, NULL, 0, 0);
// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes
void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2) {
mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, s1, s2, NULL, 0, 0);
}
// compute res += h * J'*K*J * vec, for all interpolated flexes (stiffness only, no damping)
void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) {
// s1=h, s2=0 => scale = h (no damping contribution)
mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h, 0, NULL, 0, 0);
// compute res += scale * K_bend * vec for standard (non-interp) flex bending
// scale = s1 + s2 * flex_damping[f] per flex
// for stiffness+damping: s1=h^2, s2=h => scale = h^2 + h*damping
// for stiffness only: s1=h, s2=0 => scale = h
void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2) {
for (int f = 0; f < m->nflex; f++) {
// skip interp, rigid, or non-2D
if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] != 2) {
continue;
}
int bendingadr = m->flex_bendingadr[f];
if (bendingadr < 0) {
continue;
}
mjtNum scale = s1 + s2 * m->flex_damping[f];
if (!scale) {
continue;
}
const mjtNum* b = m->flex_bending + bendingadr;
const int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f];
int edgenum = m->flex_edgenum[f];
int edgeadr = m->flex_edgeadr[f];
for (int e = 0; e < edgenum; e++) {
const int* edge = m->flex_edge + 2*(e + edgeadr);
const int* flap = m->flex_edgeflap + 2*(e + edgeadr);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
// skip boundary edges (no second flap vertex)
if (v[3] == -1) {
continue;
}
// apply 4x4 bending stencil, coordinate-wise
for (int i = 0; i < 4; i++) {
int dof_i = m->body_dofadr[bodyid[v[i]]];
for (int x = 0; x < 3; x++) {
mjtNum val = 0;
for (int j = 0; j < 4; j++) {
int dof_j = m->body_dofadr[bodyid[v[j]]];
val += b[17*e + 4*i + j] * vec[dof_j + x];
}
res[dof_i + x] += scale * val;
}
}
}
}
}
// add (d qfrc_actuator / d qvel) to qDeriv
void mjd_actuator_vel(const mjModel* m, mjData* d) {
int nu = m->nu;
src/engine/engine_derivative.h
+7 -8
View File
@@ -43,15 +43,14 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d);
// subtract (d qfrc_bias / d qvel) from qDeriv (dense version)
MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d);
// derivative of flex_interp generalized force w.r.t position: res = (d qfrc_flexinterp / d qpos) * vec
// res and vec are vectors of size m->nv
MJAPI void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h);
// derivative of flex_interp generalized force w.r.t position (stiffness only, no damping)
MJAPI void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h);
// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes
MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2);
// compute res += scale * K_bend * vec for standard (non-interp) flex bending
// scale = s1 + s2 * flex_damping[f] per flex
MJAPI void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec,
mjtNum s1, mjtNum s2);
#ifdef __cplusplus
src/engine/engine_forward.c
+27 -13
View File
@@ -1371,14 +1371,25 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
}
// return 1 if any flex needs implicit interp treatment
static int flexInterp_has_active(const mjModel* m) {
// return 1 if any flex needs implicit stiffness treatment (interp or bending)
static int flex_has_implicit_stiffness(const mjModel* m) {
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f] && !m->flex_rigid[f] &&
if (m->flex_rigid[f]) {
continue;
}
// interpolated flex with stiffness
if (m->flex_interp[f] &&
m->flex_edgeequality[f] != 3 &&
m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) {
return 1;
}
// standard flex with bending
if (!m->flex_interp[f] && m->flex_dim[f] == 2 &&
m->flex_bendingadr[f] >= 0) {
return 1;
}
}
return 0;
}
@@ -1403,24 +1414,27 @@ static void flexInterp_cgsolve(const mjModel* m, mjData* d,
mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum);
mjtNum* temp = mjSTACKALLOC(d, nv, mjtNum);
// build RHS: rhs = qfrc - h*K*qvel (velocity correction from flex stiffness)
// build RHS: rhs = qfrc
mju_copy(rhs, qfrc, nv);
// flex_interp velocity correction: rhs -= h*K_interp*qvel
mju_zero(temp, nv);
mjd_flexInterp_mulK(m, d, temp, d->qvel, h); // temp = h*K*v (stiffness only)
mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K*v
mjd_flexInterp_mul(m, d, temp, d->qvel, h, 0); // temp = h*K_interp*v
mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K_interp*v
// standard flex bending velocity correction: rhs -= h*K_bend*qvel
mjd_flexBend_mul(m, d, rhs, d->qvel, -h, 0); // rhs -= h*K_bend*v
// --- helper lambda-style inline: compute Ap = A*x ---
// A*x = (M - h*qDeriv)*x - (h^2+h*d)*K*x
// A*x = (M - h*qDeriv)*x - (h^2+h*d)*K_interp*x + (h^2+h*d)*K_bend*x
#define FLEX_CG_MATVEC(Ap_out, x_in) \
mju_mulMatVecSparse(Ap_out, d->qDeriv, x_in, nv, m->D_rownnz, m->D_rowadr, \
m->D_colind, NULL); \
mju_zero(temp, nv); \
mju_mulSymVecSparse(temp, d->M, x_in, nv, m->M_rownnz, m->M_rowadr, \
m->M_colind); \
mju_addScl(Ap_out, temp, Ap_out, -h, nv); \
mju_zero(temp, nv); \
mjd_flexInterp_mulKD(m, d, temp, x_in, h); \
mju_addToScl(Ap_out, temp, -1.0, nv)
mjd_flexInterp_mul(m, d, Ap_out, x_in, -(h*h), -h); \
mjd_flexBend_mul(m, d, Ap_out, x_in, h*h, h)
// --- helper: preconditioner solve z = (M - h*qDeriv)^{-1} * r ---
#define FLEX_CG_PRECOND(z_out, r_in) \
@@ -1857,7 +1871,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
}
// check for flex_interp that needs implicit treatment
int has_flex_interp = !sleep_filter && flexInterp_has_active(m);
int has_flex_stiffness = !sleep_filter && flex_has_implicit_stiffness(m);
// factorization
if (!skipfactor) {
@@ -1911,7 +1925,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
}
// flex: CG correction for implicit flex stiffness
if (has_flex_interp) {
if (has_flex_stiffness) {
flexInterp_cgsolve(m, d, qacc, qfrc, m->nv);
}
test/engine/engine_derivative_test.cc
+2 -2
View File
@@ -1474,7 +1474,7 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
}
// Helper: assemble flex stiffness into dense matrix via matrix-vector products.
// Builds K column-by-column using mjd_flexInterp_mulKD.
// Builds K column-by-column using mjd_flexInterp_mul.
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
// convention where stiffness is subtracted from the system matrix).
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense,
@@ -1485,7 +1485,7 @@ static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense,
mju_zero(e_i.data(), nv);
mju_zero(col.data(), nv);
e_i[i] = 1.0;
mjd_flexInterp_mulKD(m, d, col.data(), e_i.data(), h);
mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h);
// col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K)
for (int j = 0; j < nv; j++) {
H_dense[j * nv + i] = -col[j];
test/engine/engine_forward_test.cc
+125 -1
View File
@@ -3147,7 +3147,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) {
// using mulKD for legacy check consistency, but we know it applies h^2+h*d
// scaling; actually, let's stick to the high-level property checks from
// FlexStiffnessSign which used mulKD
mjd_flexInterp_mulKD(model, data, flex_Kv.data(), v.data(), h);
mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h);
// compute v^T*M*v and v^T*scale*K*v
mjtNum vMv = mju_dot(v.data(), Mv.data(), nv);
@@ -3836,5 +3836,129 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) {
mj_deleteModel(m);
}
// flex sheet dropping on a plane should not gain energy from implicit bending
TEST_F(ImplicitIntegratorTest, FlexContactEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -10" timestep="0.001" integrator="implicitfast"
solver="CG" tolerance="1e-6">
<flag energy="enable"/>
</option>
<default>
<geom solref="0.003 1"/>
</default>
<worldbody>
<geom type="plane" size="5 5 0.1"/>
<flexcomp type="grid" count="8 8 1" spacing=".04 .04 .04"
radius=".01" name="sheet" dim="2" pos="0 0 0.02" mass="0.1">
<edge equality="true" damping="0.1"/>
<elasticity young="3e6" poisson="0" thickness="2e-2"
elastic2d="bend" damping="0"/>
<contact solref="0.003 1" internal="false" selfcollide="none"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
ASSERT_EQ(m->nflex, 1);
mjData* d = mj_makeData(m);
// compute initial energy
mj_forward(m, d);
mjtNum initial_energy = d->energy[0] + d->energy[1];
ASSERT_GT(initial_energy, 0);
// simulate
mjtNum max_energy = initial_energy;
int max_energy_step = 0;
int nsteps = 500;
for (int i = 0; i < nsteps; i++) {
mj_step(m, d);
mjtNum total_energy = d->energy[0] + d->energy[1];
if (total_energy > max_energy) {
max_energy = total_energy;
max_energy_step = i + 1;
}
}
mjtNum energy_ratio = max_energy / initial_energy;
EXPECT_LE(energy_ratio, 1.01)
<< "contact solver injected energy: max_energy/initial_energy = "
<< energy_ratio << " (max at step " << max_energy_step << ")"
<< "\n initial_energy = " << initial_energy
<< "\n max_energy = " << max_energy;
mj_deleteData(d);
mj_deleteModel(m);
}
// bending damping on a flat flex must dissipate energy with implicit integrator
TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast">
<flag energy="enable"/>
</option>
<worldbody>
<flexcomp type="grid" count="6 6 1" spacing=".1 .1 .1"
radius=".005" name="sheet" dim="2" mass="0.1">
<edge equality="false" damping="0" stiffness="0"/>
<elasticity young="1e6" poisson="0" thickness="0.02"
elastic2d="bend" damping="0.1"/>
<contact solref="0.01" internal="false" selfcollide="none"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
ASSERT_EQ(m->nflex, 1);
ASSERT_GT(m->flex_damping[0], 0) << "flex_damping not set";
mjData* d = mj_makeData(m);
// perturb a central vertex with upward velocity
// vertex layout is 6x6 grid; pick a central vertex (row=3, col=3 -> id=21)
int center_vert = 21;
int bid = m->flex_vertbodyid[m->flex_vertadr[0] + center_vert];
int dofadr = m->body_dofadr[bid];
d->qvel[dofadr + 2] = 1.0; // z-velocity
// initial forward to compute energy
mj_forward(m, d);
mjtNum initial_energy = d->energy[0] + d->energy[1];
ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero";
// step forward and check energy decay
mjtNum max_energy = initial_energy;
int nsteps = 100;
for (int i = 0; i < nsteps; i++) {
mj_step(m, d);
mjtNum total_energy = d->energy[0] + d->energy[1];
max_energy = mju_max(max_energy, total_energy);
}
// energy must never exceed initial (system must not go unstable)
EXPECT_LE(max_energy, initial_energy * 1.01)
<< "energy exceeded initial by more than 1%: max=" << max_energy
<< ", initial=" << initial_energy;
// after 100 steps (0.1 seconds), energy should have decayed significantly
mjtNum final_energy = d->energy[0] + d->energy[1];
EXPECT_LT(final_energy, 0.5 * initial_energy)
<< "energy did not decay by at least 50% after " << nsteps << " steps"
<< " (initial=" << initial_energy << ", final=" << final_energy << ")";
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco