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

Fix flexcomp instability when parent body has non-identity quaternion

Browse Source 
Fast path in mj_flexPassiveInterp, mj_flexPassiveBendInterp, and
mj_flexPassiveStretch assumed body slide joints are world-aligned
(J = I). When parent body has a non-identity quaternion, joint axes
are rotated (J = R_body), causing wrong force mapping and instability
(NaN/Inf in QACC).

Fix: project world-frame forces onto body local frame via
mju_mulMatTVec3(R_body^T, force) before adding to qfrc_spring/damper.
Also fix the derivative paths:
- mjd_flexInterp_kernel fast path: R^T * K_rot * R * vec
- mjd_flexStiff_assemble (CSR): R_bi^T * K_rot_block * R_bj

The CSR-assembled stiffness matrix is the actual path used by the
implicit CG solver (mj_flexCG gate). The test uses solver="CG" to
activate this path; without it, flex stiffness is integrated
explicitly and no derivative fix can help.

Ported from GitHub PR https://github.com/google-deepmind/mujoco/pull/3379
Original author: Devansh (https://github.com/devansh0703)
Fixes https://github.com/google-deepmind/mujoco/issues/3364

PiperOrigin-RevId: 952789284
Change-Id: If924f7160dd16c0cc88170d80e6e605da0fe2e04
This commit is contained in:
Alessio Quaglino
2026-07-23 09:01:51 -07:00
committed by Copybara-Service
parent b04a18a00a
commit fe9dc58477
3 changed files with 146 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_passive_test.cc
+88
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_core_smooth.c.
#include <cmath>
#include <limits>
#include <string>
@@ -1006,5 +1007,92 @@ TEST_F(ElasticityTest, PinnedVertexBendingForce) {
EXPECT_TRUE(has_nonzero) << "bending should produce nonzero spring forces";
}
// verify that a flexcomp with dof="trilinear" inside a parent body
// with non-identity quaternion produces:
// 1. restoring forces (not expanding) for small perturbations
// 2. identical qfrc_passive between rotated and non-rotated models
// 3. stable simulation with implicit integration (no NaN, bounded qacc)
TEST_F(ElasticityTest, TrilinearParentBodyRotation) {
static constexpr char rotated_xml[] = R"(
<mujoco>
<option gravity="0 0 0" integrator="implicitfast"
timestep="0.0005" solver="CG"/>
<worldbody>
<body name="base" pos="0 0 0" quat="0.7071 0 0.7071 0">
<flexcomp type="grid" count="3 3 3" spacing=".05 .05 .05"
dim="3" radius=".001" mass=".005" name="soft" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.1"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
static constexpr char nonrotated_xml[] = R"(
<mujoco>
<option gravity="0 0 0" integrator="implicitfast"
timestep="0.0005" solver="CG"/>
<worldbody>
<body name="base" pos="0 0 0">
<flexcomp type="grid" count="3 3 3" spacing=".05 .05 .05"
dim="3" radius=".001" mass=".005" name="soft" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.1"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
// --- test 1: force sign and rotational invariance ---
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);
// apply small +X perturbation to DOF 0 (node 0, X slide)
d_rot->qpos[0] = 1e-6;
d_non->qpos[0] = 1e-6;
mj_forward(m_rot.get(), d_rot.get());
mj_forward(m_non.get(), d_non.get());
// check force sign on the displaced DOF: positive qpos -> negative qfrc (restoring)
EXPECT_LT(d_rot->qfrc_passive[0], 0)
<< "rotated model: expected restoring force on displaced DOF 0";
EXPECT_LT(d_non->qfrc_passive[0], 0)
<< "non-rotated model: expected restoring force on displaced DOF 0";
// check rotational invariance: forces match within numerical precision
// (tol ~ 1e-13 due to floating-point differences in polar decomposition path)
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;
}
// --- test 2: implicit integration stability (200 steps, no NaN) ---
mjtNum max_qacc = 0;
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;
mjtNum a = mju_abs(d_rot->qacc[i]);
if (a > max_qacc) max_qacc = a;
}
if (HasFatalFailure()) return;
}
EXPECT_LT(max_qacc, 1e4);
}
} // namespace
} // namespace mujoco