From 7927e908a3f509afaa4f04f0c00ece55defcc8a0 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 3 Aug 2026 07:28:04 -0700 Subject: [PATCH] Fix flex bending frame transformation when parent body is rotated. PiperOrigin-RevId: 958362895 Change-Id: I0f6ba12a441bd7daa54292bfe2c70f0d836b008d --- src/engine/engine_derivative.c | 37 +++++++++--- src/engine/engine_passive.c | 12 +++- test/engine/engine_derivative_test.cc | 80 ++++++++++++++++++++++++ test/engine/engine_passive_test.cc | 87 +++++++++++++++++++++++++++ 4 files changed, 205 insertions(+), 11 deletions(-) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index b2fc369a..25985303 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1387,16 +1387,37 @@ void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* ve continue; } - // apply 4x4 bending stencil, coordinate-wise + // apply 4x4 bending stencil, coordinate-wise. Pinned vertices (zero-dof bodies) contribute + // nothing, as in mjd_flexStretch_mul: they have no dof to write a row into and none to read a + // displacement from, and body_dofadr is negative there, so an unguarded index runs off both + // res and vec. + // The stencil is built from WORLD-space vertex positions while the slide dofs live in each + // vertex body's own (possibly rotated) frame, so the operator is sandwiched with R (dof -> + // world) and R^T (world -> dof), as mjd_flexStretch_mul does. Without it the operator is not + // the Jacobian of mj_flexPassiveBend's force whenever a flex parent is rotated. 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]; + int bi = bodyid[v[i]]; + if (!m->body_dofnum[bi]) { + continue; + } + mjtNum vw[3] = {0, 0, 0}; + for (int j = 0; j < 4; j++) { + int bj = bodyid[v[j]]; + if (!m->body_dofnum[bj]) { + continue; } - res[dof_i + x] += scale * val; + mjtNum wj[3]; + mji_mulMatVec3(wj, d->xmat + 9*bj, vec + m->body_dofadr[bj]); + mjtNum q = b[17*e + 4*i + j]; + for (int x = 0; x < 3; x++) { + vw[x] += q * wj[x]; + } + } + mjtNum vl[3]; + mji_mulMatTVec3(vl, d->xmat + 9*bi, vw); + int dof_i = m->body_dofadr[bi]; + for (int x = 0; x < 3; x++) { + res[dof_i + x] += scale * vl[x]; } } } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index f977f814..dd1385ed 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -532,10 +532,16 @@ static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f, // which is what the pin constrains). for (int i = 0; i < 4; i++) { if (!isfree[i]) continue; - int body_dofadr = m->body_dofadr[bodyid[v[i]]]; + int bi = bodyid[v[i]]; + int body_dofadr = m->body_dofadr[bi]; + // spring/damper are world-space; the slide dofs are in the body frame, so rotate before + // accumulating (mj_flexPassiveStretch reaches the same frame through mj_applyFT). + mjtNum sl[3], dl[3]; + mji_mulMatTVec3(sl, d->xmat + 9*bi, spring + 3*i); + mji_mulMatTVec3(dl, d->xmat + 9*bi, damper + 3*i); for (int x = 0; x < 3; x++) { - if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x]; - if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f]; + if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= sl[x]; + if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= dl[x] * m->flex_damping[f]; } } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 0c2a4c39..876b6277 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1842,6 +1842,86 @@ static void stretchK_dense(mjModel* m, mjData* d, mjtNum* K, int nv, } } +// The same frame mismatch seen through the derivative: mjd_flexBend_mul is the +// Jacobian of the bending force only if operator and force agree on the frame. +// An unrotated flex cannot catch it, because the stencil is one scalar per +// vertex pair applied coordinate-wise and so commutes with a rotation shared by +// every vertex. The pin also covers the zero-dof vertex path, where body_dofadr +// is negative and an unguarded stencil indexes out of bounds. +TEST_F(DerivativeTest, FlexBendDerivativesRotated) { + static const char* const kXml = R"( + + + )"; + + char error[1024]; + MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + int nv = model->nv; + ASSERT_EQ(model->nq, nv); // all slide dofs + ASSERT_EQ(model->nflex, 2); + MjDataPtr data = MakeData(model); + + // deform both flexes out of plane so the bending stencil carries real force + for (int i = 0; i < nv; i++) { + data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5); + } + mj_forward(model.get(), data.get()); + + std::vector vec(nv), res(nv, 0); + for (int i = 0; i < nv; i++) { + vec[i] = mju_Halton(i, 3) - 0.5; + } + mjd_flexBend_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0); + + mjtNum eps = MjTol(1e-7, 1e-4); + mjData* perturbed = mj_copyData(NULL, model.get(), data.get()); + mju_addToScl(perturbed->qpos, vec.data(), eps, nv); + mj_forward(model.get(), perturbed); + + // check each flex on its own: the unrotated one is the control that isolates + // the rotation + for (int f = 0; f < model->nflex; f++) { + SCOPED_TRACE(model->names + model->name_flexadr[f]); + mjtNum max_err = 0, scale = 0; + for (int k = 0; k < model->flex_vertnum[f]; k++) { + int body = model->flex_vertbodyid[model->flex_vertadr[f] + k]; + int adr = model->body_dofadr[body]; + for (int x = 0; x < model->body_dofnum[body]; x++) { + mjtNum fd = -(perturbed->qfrc_passive[adr+x] - + data->qfrc_passive[adr+x]) / eps; + max_err = mju_max(max_err, mju_abs(res[adr+x] - fd)); + scale = mju_max(scale, mju_abs(fd)); + } + } + // bending is linear in position, so the difference is exact up to roundoff + EXPECT_GT(scale, 1e-3) + << "test should exercise nontrivial bending stiffness"; + EXPECT_LT(max_err, MjTol(1e-4, 1e-2) * scale) + << "mjd_flexBend_mul is not the Jacobian of the flex bending force"; + } + mj_deleteData(perturbed); +} + // verify mjd_flexStretch_mul (Gauss-Newton Hessian of the standard-flex // stretch force) against finite differences of qfrc_passive, plus symmetry, // positive semi-definiteness and (s1, s2) scale linearity. The model covers diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 78db67e0..de167caa 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -1094,6 +1094,93 @@ TEST_F(ElasticityTest, TrilinearParentBodyRotation) { } +// A dim=2 flexcomp with bending elasticity inside a parent body with a +// non-identity quaternion. The implicit metric assembles the bending +// stiffness from world-space vertex positions, 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, BendParentBodyRotation) { + 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); + + // lift one corner out of plane: that bends at first order but stretches only + // at second, so the bending force is not swamped. As in the stretch case the + // displacement is applied in the parent frame, so the response must not + // depend on the parent's orientation + d_rot->qpos[2] = 1e-2; + d_non->qpos[2] = 1e-2; + + mj_forward(m_rot.get(), d_rot.get()); + mj_forward(m_non.get(), d_non.get()); + + EXPECT_LT(d_non->qfrc_passive[2], -1e-6) + << "expected a restoring force on the lifted 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, so this also covers mjd_flexBend_mul + 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; + } +} + // 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