Apply unsymmetrized fluid derivatives to standalone free bodies in implicitfast.

PiperOrigin-RevId: 948899583
Change-Id: Icfb5a713f89a94e597c7607e9aa10a9e151dc2aa

src/engine/engine_derivative.c

@@ -818,31 +818,35 @@ void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36])
 }
 
 
+// return 1 if body is a standalone free body (single free joint, no children)
+mjtBool mj_isFreeBody(const mjModel* m, int body) {
+  // must have exactly one joint, of free type
+  if (m->body_jntnum[body] != 1 || m->jnt_type[m->body_jntadr[body]] != mjJNT_FREE) {
+    return false;
+  }
+
+  int adr = m->jnt_dofadr[m->body_jntadr[body]];
+
+  // must be a standalone 6-DOF tree with no children
+  if (m->tree_dofnum[m->dof_treeid[adr]] != 6 ||
+      m->body_subtreemass[body] != m->body_mass[body]) {
+    return false;
+  }
+
+  return true;
+}
+
+
 // 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
 //   returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
 //   requires valid d->qDeriv rows for the block, computed with flg_bias = 0; the bias
-//   derivative excluded from qDeriv is added here via freeBias_vel_blocks
+//   derivative excluded from qDeriv is added here via mjd_freeBias_vel
 int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]) {
-  // must be a free joint
-  if (m->jnt_type[jnt] != mjJNT_FREE) {
-    return 0;
-  }
-
   int body = m->jnt_bodyid[jnt];
   int adr = m->jnt_dofadr[jnt];
-  int tree = m->dof_treeid[adr];
-  mjtNum mass = m->body_mass[body];
 
-  // must be a standalone 6-DOF tree with no children, awake
-  if (m->tree_dofnum[tree] != 6 ||
-      m->body_subtreemass[body] != mass ||
-      !d->tree_awake[tree]) {
-    return 0;
-  }
-
-  // D rows of a standalone free body are exactly the 6x6 block (D sparsity is tree-local);
-  // guard the gathers below against any violation of this invariant
-  if (m->D_rownnz[adr] != 6) {
+  // must be a standalone free body, awake
+  if (!mj_isFreeBody(m, body) || !d->tree_awake[m->dof_treeid[adr]]) {
     return 0;
   }
 
@@ -872,6 +876,7 @@ int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[
   mjtNum s[3];
   mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body);
 
+  mjtNum mass = m->body_mass[body];
   mjtNum lin[9], rot[9];
   freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*body,
                       m->body_inertia + 3*body, s, d->qvel + adr + 3, lin, rot);
@@ -2482,8 +2487,8 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
 
     mjd_addedMassForces(B, lvel, m->opt.density, virtual_mass, virtual_inertia);
 
-    // make B symmetric if integrator is IMPLICITFAST
-    if (m->opt.integrator == mjINT_IMPLICITFAST) {
+    // make B symmetric if integrator is IMPLICITFAST, except for standalone free bodies
+    if (m->opt.integrator == mjINT_IMPLICITFAST && !mj_isFreeBody(m, bodyid)) {
       mju_symmetrize(B, B, 6);
     }
 

src/engine/engine_derivative.h

@@ -43,9 +43,11 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d);
 // subtract (d qfrc_bias / d qvel) from qDeriv (dense version)
 MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d);
 
+// return 1 if body is a standalone free body: a free joint with no children
+mjtBool mj_isFreeBody(const mjModel* m, int body);
+
 // 6x6 block B = d qfrc_bias / d qvel for the free joint of a standalone body
-MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt,
-                            mjtNum B[36]);
+MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]);
 
 // 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
 //   returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise

src/engine/engine_forward.c

@@ -1377,7 +1377,7 @@ int mj_flexCG(const mjModel* m) {
 // fully implicit in velocity, possibly skipping factorization
 void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
   TM_START;
-  int nD = m->nD, nC = m->nC;
+  int nD = m->nD, nC = m->nC, njnt = m->njnt;
 
   mj_markStack(d);
   mjtNum* qfrc = mjSTACKALLOC(d, m->nv, mjtNum);
@@ -1427,6 +1427,18 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
 
       // set qH = M - dt*qDeriv
       mju_addScl(d->qH, d->M, d->qH, -m->opt.timestep, nC);
+
+      // standalone free bodies: reset qH block rows to M; their qDeriv rows may be asymmetric and
+      // are handled by the local LU solve; we reset to keep LTL well-defined
+      for (int j=0; j < njnt; j++) {
+        if (m->jnt_type[j] != mjJNT_FREE || !mj_isFreeBody(m, m->jnt_bodyid[j])) {
+          continue;
+        }
+        int adr = m->jnt_dofadr[j];
+        for (int r=0; r < 6; r++) {
+          mju_copy(d->qH + m->M_rowadr[adr+r], d->M + m->M_rowadr[adr+r], m->M_rownnz[adr+r]);
+        }
+      }
     } else {
       mjERROR("integrator must be implicit or implicitfast");
     }