Add actuatorgravcomp joint attribute, to treat gravity compensation forces as applied by actuators, rather than passive buoyancy.

PiperOrigin-RevId: 620049593
Change-Id: I2c8a9dc152c087b408e4f904838034271a7dd910

src/engine/engine_passive.c

@@ -255,8 +255,39 @@ void mj_passive(const mjModel* m, mjData* d) {
 
   // add passive forces into qfrc_passive
   mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
-  if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv);
   if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
+  if (has_gravcomp) {
+    int njnt = m->njnt;
+    for (int i=0; i < njnt; i++) {
+      // skip if gravcomp added via actuators
+      if (m->jnt_actgravcomp[i]) {
+        continue;
+      }
+
+      // get number of dofs for this joint
+      int dofnum;
+      switch (m->jnt_type[i]) {
+      case mjJNT_HINGE:
+      case mjJNT_SLIDE:
+        dofnum = 1;
+        break;
+
+      case mjJNT_BALL:
+        dofnum = 3;
+        break;
+
+      case mjJNT_FREE:
+        dofnum = 6;
+        break;
+      }
+
+      // add gravcomp force
+      int dofadr = m->jnt_dofadr[i];
+      for (int j=0; j < dofnum; j++) {
+        d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
+      }
+    }
+  }
 
   // user callback: add custom passive forces
   if (mjcb_passive) {