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

Fix: Include ancestor DOFs in flex interpolation factorization.

Browse Source 
The reduced dense factorization for flex interpolation now considers all DOFs in the kinematic chain of the body containing the flex, using mj_bodyChain, instead of only the DOFs directly associated with that body. This is necessary for correctly handling pinned flexes when their parent body is part of a larger kinematic structure.

PiperOrigin-RevId: 872854468
Change-Id: Idbe9fb459084dde9e8eb1076c70dbb685c1b0bdb
This commit is contained in:
Alessio Quaglino
2026-02-20 05:30:12 -08:00
committed by Copybara-Service
parent 8c9ba9bcf5
commit f9a39413cf
2 changed files with 111 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+47 -7
View File
@@ -27,6 +27,7 @@
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_derivative.h"
#include "engine/engine_core_util.h"
#include "engine/engine_inverse.h"
#include "engine/engine_island.h"
#include "engine/engine_macro.h"
@@ -1189,14 +1190,38 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
// flex: reduced dense factorization
if (has_flex_interp && !sleep_filter) {
// temporary allocations for body chain
int* chain_dofs = mjSTACKALLOC(d, nv, int);
int* seen_dof = mjSTACKALLOC(d, nv, int);
mju_fillInt(seen_dof, 0, nv);
// identify flex DOFs
// For pinned nodes (body_dofnum==0): use bodyChain to include parent DOFs
// For regular flex nodes: use body_dofadr for one-way coupling
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f]) {
int nodenum = m->flex_nodenum[f];
int nodeadr = m->flex_nodeadr[f];
for (int n=0; n < nodenum; n++) {
int b = m->flex_nodebodyid[nodeadr + n];
nflexdofs += m->body_dofnum[b];
int chain_nnz;
if (m->body_dofnum[b] == 0) {
// Pinned node: use bodyChain to get parent DOFs
chain_nnz = mj_bodyChain(m, b, chain_dofs);
} else {
// Regular flex node: use body's own DOFs only
chain_nnz = m->body_dofnum[b];
for (int j = 0; j < chain_nnz; j++) {
chain_dofs[j] = m->body_dofadr[b] + j;
}
}
for (int i=0; i < chain_nnz; i++) {
int dof = chain_dofs[i];
if (!seen_dof[dof]) {
seen_dof[dof] = 1;
nflexdofs++;
}
}
}
}
}
@@ -1207,19 +1232,34 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
int* global2local = mjSTACKALLOC(d, nv, int);
mju_fillInt(global2local, -1, nv);
// collect unique DOFs in order
int cnt = 0;
mju_fillInt(seen_dof, 0, nv);
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f]) {
int nodenum = m->flex_nodenum[f];
int nodeadr = m->flex_nodeadr[f];
for (int n=0; n < nodenum; n++) {
int b = m->flex_nodebodyid[nodeadr + n];
int dofnum = m->body_dofnum[b];
int dofadr = m->body_dofadr[b];
for (int j=0; j < dofnum; j++) {
flex_dof_indices[cnt] = dofadr + j;
global2local[dofadr + j] = cnt;
cnt++;
int chain_nnz;
if (m->body_dofnum[b] == 0) {
// Pinned node: use bodyChain to get parent DOFs
chain_nnz = mj_bodyChain(m, b, chain_dofs);
} else {
// Regular flex node: use body's own DOFs only
chain_nnz = m->body_dofnum[b];
for (int j = 0; j < chain_nnz; j++) {
chain_dofs[j] = m->body_dofadr[b] + j;
}
}
for (int i=0; i < chain_nnz; i++) {
int dof = chain_dofs[i];
if (!seen_dof[dof]) {
seen_dof[dof] = 1;
flex_dof_indices[cnt] = dof;
global2local[dof] = cnt;
cnt++;
}
}
}
}
test/engine/engine_forward_test.cc
+64
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_forward.h"
#include "src/engine/engine_derivative.h"
#include <array>
#include <cmath>
#include <cstdlib>
#include <limits>
@@ -1816,5 +1817,68 @@ TEST_F(ForwardTest, FlexParentCoupling) {
mj_deleteModel(model);
}
TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint type="free"/>
<geom type="box" size="0.13 0.18 0.036" pos="0 0 0.036"/>
<body name="parent">
<flexcomp name="test" type="grid"
count="3 3 3" spacing=".1 .02 .1" radius="0.001"
pos="0 0 0.1" dof="trilinear" xyaxes="0 1 0 0 0 1" mass="10" dim="3">
<contact selfcollide="none"/>
<elasticity young="1e5" poisson="0.3" damping="0.1"/>
<pin id="0 2 4 6"/>
</flexcomp>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
int parent_id = mj_name2id(m, mjOBJ_BODY, "parent");
ASSERT_GT(parent_id, 0);
EXPECT_EQ(m->nflexnode, 8);
EXPECT_EQ(m->body_dofnum[parent_id], 0) << "parent body should have 0 DOFs";
int freejoint_body = m->body_parentid[parent_id];
EXPECT_EQ(m->body_dofnum[freejoint_body], 6) << "freejoint body has 6 DOFs";
mj_resetData(m, d);
mj_forward(m, d);
for (int i = 0; i < 500; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation became unstable at step " << i;
ASSERT_FALSE(mju_isBad(d->qvel[0]))
<< "Velocity became unstable at step " << i;
for (int j = 0; j < m->nq; j++) {
ASSERT_LT(mju_abs(d->qpos[j]), 100.0)
<< "Position exploded at step " << i << ", qpos[" << j
<< "]=" << d->qpos[j];
}
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i << ", qvel[" << j
<< "]=" << d->qvel[j];
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco