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

Fix gravcomp being ignored for movable bodies without joints. Fixes #3066

Browse Source 
PiperOrigin-RevId: 867679545
Change-Id: I8c16d3a4c93d121fcc30cab5179ba44fe49aaa19
This commit is contained in:
Yuval Tassa
2026-02-09 10:54:07 -08:00
committed by Copybara-Service
parent b637d7eb76
commit c1b3b3063e
3 changed files with 37 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+2
View File
@@ -123,6 +123,8 @@ Bug fixes
available cores.
- The ``vertid`` argument of :ref:`mj_rayFlex` and :ref:`mju_raySkin` was marked as nullable but was not; it is now
nullable.
- Fixed :ref:`gravcomp<body-gravcomp>` being ignored for bodies with no joints nested inside jointed parent bodies
(:issue:`3066`, reported by :github:user:`Alex108306`).
Version 3.4.0 (December 5, 2025)
--------------------------------
src/engine/engine_passive.c
+6 -27
View File
@@ -668,34 +668,13 @@ void mj_passive(const mjModel* m, mjData* d) {
}
if (has_gravcomp) {
int nbody = sleep_filter ? d->nbody_awake : m->nbody;
for (int b=0; b < nbody; b++) {
int i = sleep_filter ? d->body_awake_ind[b] : b;
int ndof = sleep_filter ? d->nv_awake : nv;
for (int v=0; v < ndof; v++) {
int dof = sleep_filter ? d->dof_awake_ind[v] : v;
// skip if no joints
int jntnum = m->body_jntnum[i];
if (!jntnum) continue;
// skip if no gravity compensation
if (!m->body_gravcomp[i]) continue;
int start = m->body_jntadr[i];
int end = start + jntnum;
for (int j=start; j < end; j++) {
// skip if gravity compensation added via actuators
if (m->jnt_actgravcomp[j]) {
continue;
}
// get number of dofs for this joint
const int jnt_dofnum[4] = {6, 3, 1, 1};
int dofnum = jnt_dofnum[m->jnt_type[j]];
// add gravity compensation force
int dofadr = m->jnt_dofadr[j];
for (int k=0; k < dofnum; k++) {
d->qfrc_passive[dofadr+k] += d->qfrc_gravcomp[dofadr+k];
}
// add gravity compensation force unless added via actuators
if (!m->jnt_actgravcomp[m->dof_jntid[dof]]) {
d->qfrc_passive[dof] += d->qfrc_gravcomp[dof];
}
}
}
test/engine/engine_passive_test.cc
+29
View File
@@ -73,6 +73,35 @@ TEST_F(PassiveTest, DisableFlags) {
mj_deleteModel(m);
}
TEST_F(PassiveTest, GravcompNestedBody) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body pos="0 0 2">
<freejoint/>
<body gravcomp="1.2">
<geom size="0.2" mass="1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_forward(m, d);
EXPECT_GT(d->qacc[2], 0);
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------ ellipsoid fluid model ------------------------------
using EllipsoidFluidTest = MujocoTest;