chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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
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);
}