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