diff --git a/doc/changelog.rst b/doc/changelog.rst index 5a913b0a..d7d456ec 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -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` 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) -------------------------------- diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 3ed27063..6653b4ca 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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]; } } } diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 2dba9c00..c4b981b6 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -73,6 +73,35 @@ TEST_F(PassiveTest, DisableFlags) { mj_deleteModel(m); } +TEST_F(PassiveTest, GravcompNestedBody) { + static constexpr char xml[] = R"( + + + )"; + + 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;