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

PiperOrigin-RevId: 867679545
Change-Id: I8c16d3a4c93d121fcc30cab5179ba44fe49aaa19

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<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

@@ -668,34 +668,13 @@ void mj_passive(const mjModel* m, mjData* d) {
   }
 
   if (has_gravcomp) {
-    int nbody = sleep_filter ? d->nbody_awake : m->nbody;
+    int ndof = sleep_filter ? d->nv_awake : nv;
-    for (int b=0; b < nbody; b++) {
+    for (int v=0; v < ndof; v++) {
-      int i = sleep_filter ? d->body_awake_ind[b] : b;
+      int dof = sleep_filter ? d->dof_awake_ind[v] : v;
 
-      // skip if no joints
+      // add gravity compensation force unless added via actuators
-      int jntnum = m->body_jntnum[i];
+      if (!m->jnt_actgravcomp[m->dof_jntid[dof]]) {
-      if (!jntnum) continue;
+        d->qfrc_passive[dof] += d->qfrc_gravcomp[dof];
-
-      // 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];
-        }
       }
     }
   }

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"(
+  <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;