From 55b43414dde2587416e5519d535cd82d9f7492cc Mon Sep 17 00:00:00 2001 From: Alessio Date: Mon, 27 Jul 2026 19:07:02 +0100 Subject: [PATCH] Fix stretch stiffness basis for flexes in rotated parent bodies The implicit effective metric assembles the stretch stiffness from world-space edge vectors, but a flex vertex body's slide dofs are expressed in its parent body's frame. When that frame is rotated the assembled operator is therefore not the Jacobian of the passive stretch force, which mj_flexPassiveStretch already maps into the dof frame with xmat^T. The metric is then inconsistent with the force it linearizes: implicit integration loses its stability guarantee, and models that the same flex handles comfortably in an unrotated frame diverge. Apply the matching change of basis in both places that build or apply the stretch stiffness: mjd_flexStretch_mul rotates the input dof vector into world and the scattered result back, and mjd_flexStiff_assemble sandwiches each 3x3 block as R_bi^T * blk * R_bj. Both are no-ops when the parent is unrotated. Bending needs no change: its blocks are isotropic, and R^T (q I) R = q I. This completes the fix in fe9dc584, which covered the passive force paths and the interp (trilinear) derivative but not the standard stretch one. On a mesh flex inside a body with a 90-degree rotation, the metric's directional agreement with the force Jacobian goes from cos = 0.57 to cos = 1.0, and a hanging sheet that previously reached 176% strain settles at 0.87%. --- src/engine/engine_derivative.c | 41 +++++++++++--- test/engine/engine_passive_test.cc | 90 +++++++++++++++++++++++++++++- 2 files changed, 123 insertions(+), 8 deletions(-) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index f7c3f413..20f3a5a2 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1455,12 +1455,20 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* int v0 = vert[edge[e][0]], v1 = vert[edge[e][1]]; int b0 = bodyid[v0], b1 = bodyid[v1]; g[e] = 0; + // the vertex bodies' slide dofs are expressed in their own (possibly rotated) frame while + // the stiffness is built from world-space edge vectors, so the operator must be sandwiched + // with R (dof -> world) and R^T (world -> dof); mj_flexPassiveStretch applies the same R^T + // to its world-space force. R = I for the common case of an unrotated parent body. + mjtNum w0[3] = {0}, w1[3] = {0}; + if (m->body_dofnum[b0]) { + mju_mulMatVec3(w0, d->xmat + 9*b0, vec + m->body_dofadr[b0]); + } + if (m->body_dofnum[b1]) { + mju_mulMatVec3(w1, d->xmat + 9*b1, vec + m->body_dofadr[b1]); + } for (int x = 0; x < 3; x++) { dvec[e][x] = xpos[3*v0+x] - xpos[3*v1+x]; - mjtNum dv = 0; - if (m->body_dofnum[b0]) dv += vec[m->body_dofadr[b0]+x]; - if (m->body_dofnum[b1]) dv -= vec[m->body_dofadr[b1]+x]; - g[e] += dvec[e][x]*dv; + g[e] += dvec[e][x]*(w0[x] - w1[x]); } } @@ -1483,9 +1491,21 @@ void mjd_flexStretch_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* } coef *= 2*scale; int b0 = bodyid[vert[edge[e][0]]], b1 = bodyid[vert[edge[e][1]]]; + mjtNum rw[3], rl[3]; for (int x = 0; x < 3; x++) { - if (m->body_dofnum[b0]) res[m->body_dofadr[b0]+x] += coef*dvec[e][x]; - if (m->body_dofnum[b1]) res[m->body_dofadr[b1]+x] -= coef*dvec[e][x]; + rw[x] = coef*dvec[e][x]; + } + if (m->body_dofnum[b0]) { // world -> dof frame + mju_mulMatTVec3(rl, d->xmat + 9*b0, rw); + for (int x = 0; x < 3; x++) { + res[m->body_dofadr[b0]+x] += rl[x]; + } + } + if (m->body_dofnum[b1]) { + mju_mulMatTVec3(rl, d->xmat + 9*b1, rw); + for (int x = 0; x < 3; x++) { + res[m->body_dofadr[b1]+x] -= rl[x]; + } } } } @@ -1945,11 +1965,18 @@ int mjd_flexStiff_assemble(const mjModel* m, mjData* d, int* rownnz, int* rowadr } } } + // blk is world-space but the destination dofs are the vertex bodies' own (possibly + // rotated) slide axes: blk_dof = R_bi^T * blk_world * R_bj, matching the force path + int bi = m->flex_vertbodyid[m->flex_vertadr[f] + vert[i]]; + int bj = m->flex_vertbodyid[m->flex_vertadr[f] + vert[j]]; + mjtNum tmp[9], blkd[9]; + mju_mulMatMat3(tmp, blk, d->xmat + 9*bj); // tmp = blk * R_bj + mju_mulMatTMat3(blkd, d->xmat + 9*bi, tmp); // blkd = R_bi^T * tmp int pos; FLEXSTIFF_BLOCK(si, sj, pos); for (int k = 0; k < 3; k++) { for (int c = 0; c < 3; c++) { - val[rowadr[vdof[si] + k] + 3*pos + c] += blk[3*k+c]; + val[rowadr[vdof[si] + k] + 3*pos + c] += blkd[3*k+c]; } } } diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 223d7b59..78db67e0 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -1093,6 +1093,94 @@ TEST_F(ElasticityTest, TrilinearParentBodyRotation) { EXPECT_LT(max_qacc, 1e4); } + +// A dim=2 flexcomp with stretch elasticity inside a parent body with a +// non-identity quaternion. The implicit metric assembles the stretch +// stiffness from world-space edge vectors, but the vertex bodies' slide +// dofs live in the (rotated) parent frame. Without the R^T (.) R change +// of basis the metric stops being the Jacobian of the passive force, +// which shows up as a loss of rotational invariance and, at stiffnesses +// the unrotated model handles comfortably, as divergence. +TEST_F(ElasticityTest, StretchParentBodyRotation) { + static constexpr char rotated_xml[] = R"( + + + )"; + static constexpr char nonrotated_xml[] = R"( + + + )"; + + char error[1024] = {0}; + + MjModelPtr m_rot = LoadModelFromString(rotated_xml, error, sizeof(error)); + ASSERT_THAT(m_rot.get(), NotNull()) << error; + MjDataPtr d_rot = MakeData(m_rot); + + MjModelPtr m_non = LoadModelFromString(nonrotated_xml, error, sizeof(error)); + ASSERT_THAT(m_non.get(), NotNull()) << error; + MjDataPtr d_non = MakeData(m_non); + + // stretch one dof; the response is expressed in the parent frame in + // both models, so the passive force and the implicit acceleration must + // agree regardless of the parent's orientation + d_rot->qpos[0] = 1e-4; + d_non->qpos[0] = 1e-4; + + mj_forward(m_rot.get(), d_rot.get()); + mj_forward(m_non.get(), d_non.get()); + + EXPECT_LT(d_rot->qfrc_passive[0], 0) + << "expected a restoring force on the stretched dof"; + + const mjtNum tol = MjTol(1e-12, 1e-5); + for (int i = 0; i < m_rot->nv; i++) { + EXPECT_NEAR(d_rot->qfrc_passive[i], d_non->qfrc_passive[i], tol) + << "rotated/non-rotated qfrc mismatch at dof " << i; + } + + // qacc exercises the metric itself (the force is only its right-hand + // side): a metric in the wrong basis breaks this invariance even though + // the force above is already correct + for (int i = 0; i < m_rot->nv; i++) { + EXPECT_NEAR(d_rot->qacc[i], d_non->qacc[i], MjTol(1e-9, 1e-3)) + << "rotated/non-rotated qacc mismatch at dof " << i; + } + + // and the rotated model must integrate stably + for (int step = 0; step < 200; step++) { + mj_step(m_rot.get(), d_rot.get()); + for (int i = 0; i < m_rot->nv; i++) { + ASSERT_TRUE(std::isfinite(d_rot->qacc[i])) + << "NaN/Inf in qacc at dof " << i << " at step " << step; + } + if (HasFatalFailure()) return; + } +} + } // namespace } // namespace mujoco -