Fix bug in inverse dynamics when tendon armature is present.

PiperOrigin-RevId: 773604419
Change-Id: Iaffe3d831c6a5aba8a0a99c05aa76cbd862745f5

doc/changelog.rst

@@ -2,6 +2,14 @@
 Changelog
 =========
 
+Upcoming version (not yet released)
+-----------------------------------
+
+Bug fixes
+^^^^^^^^^
+- Inverse dynamics were not being computed correctly when :ref:`tendon armature<tendon-spatial-armature>` was present,
+  now fixed.
+
 Version 3.3.3 (June 10, 2025)
 -----------------------------
 

src/engine/engine_inverse.c

@@ -46,8 +46,8 @@ void mj_invPosition(const mjModel* m, mjData* d) {
   mj_tendon(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
-  mj_makeM(m, d);           // timed internally (POS_INERTIA)
-  mj_factorM(m, d);         // timed internally (POS_INERTIA)
+  mj_makeM(m, d);      // timed internally (POS_INERTIA)
+  mj_factorM(m, d);    // timed internally (POS_INERTIA)
 
   mj_collision(m, d);  // timed internally (POS_COLLISION)
 
@@ -226,15 +226,22 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
 
   // acceleration-dependent
   mj_invConstraint(m, d);
-  mj_rne(m, d, 1, d->qfrc_inverse);
+
+  // sum of bias forces in qfrc_inverse = centripetal + Coriolis + tendon bias
+  mj_rne(m, d, 0, d->qfrc_inverse);
+  mj_tendonBias(m, d, d->qfrc_inverse);
+
   if (!skipsensor) {
     mj_sensorAcc(m, d);
   }
 
-  // qfrc_inverse += armature*qacc - qfrc_passive - qfrc_constraint
+  // compute Ma = M*qacc
+  mjtNum* Ma = mjSTACKALLOC(d, nv, mjtNum);
+  mj_mulM(m, d, Ma, d->qacc);
+
+  // qfrc_inverse += Ma - qfrc_passive - qfrc_constraint
   for (int i=0; i < nv; i++) {
-    d->qfrc_inverse[i] += m->dof_armature[i]*d->qacc[i]
-                          - d->qfrc_passive[i] - d->qfrc_constraint[i];
+    d->qfrc_inverse[i] += Ma[i] - d->qfrc_passive[i] - d->qfrc_constraint[i];
   }
 
   if (mjENABLED(mjENBL_INVDISCRETE)) {

test/testdata/model.xml

@@ -129,7 +129,7 @@
       <geom geom="wrapping" sidesite="sidesite"/>
       <site site="anchor2"/>
     </spatial>
-    <spatial name="knee" limited="true" range=".4 .5" rgba="1 0 1 1">
+    <spatial name="knee" limited="true" range=".4 .5" rgba="1 0 1 1" armature="0.1">
       <site site="anchor_knee"/>
       <site site="knee_2"/>
     </spatial>