Add mju_sym2dense, document future breakage of mj_fullM

PiperOrigin-RevId: 910242375
Change-Id: Ibfbdef9cfb66088723499ea257da09aee0d80938

src/engine/engine_island.c

@@ -525,7 +525,7 @@ void mj_island(const mjModel* m, mjData* d) {
     d->island_dofadr[i] = d->map_idof2dof[d->island_idofadr[i]];
   }
 
-  // inertia: block-diagonalize both iLD <- qLD and iM <- qM
+  // inertia: block-diagonalize both iLD <- qLD and iM <- M
   mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind,
                       d->qLD,  m->M_rownnz, m->M_rowadr, m->M_colind,
                       nidof, nisland,

src/engine/engine_setconst.c

@@ -1307,7 +1307,7 @@ static void setStat(mjModel* m, mjData* d) {
   if (m->nv) {
     m->stat.meaninertia = 0;
     for (int i=0; i < m->nv; i++) {
-      m->stat.meaninertia += d->qM[m->dof_Madr[i]];
+      m->stat.meaninertia += d->M[m->M_rowadr[i] + m->M_rownnz[i] - 1];
     }
     m->stat.meaninertia /= m->nv;
   }

src/engine/engine_util_sparse.c

@@ -140,6 +140,23 @@ void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
 }
 
 
+// convert lower-triangular symmetric CSR matrix to full dense matrix
+void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n,
+                   const int* rownnz, const int* rowadr, const int* colind) {
+  mju_zero(res, n*n);
+  for (int i = 0; i < n; i++) {
+    int adr = rowadr[i];
+    for (int j = 0; j < rownnz[i]; j++) {
+      int col = colind[adr+j];
+      if (col <= i) {
+        res[i*n+col] = mat[adr+j];
+        res[col*n+i] = mat[adr+j];
+      }
+    }
+  }
+}
+
+
 // res[row, :] = mat[row, :]
 void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr,
                     const int* row, int nrow) {

src/engine/engine_util_sparse.h

@@ -46,6 +46,10 @@ MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
 MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz,
                             const int* rowadr, const int* colind);
 
+// convert lower-triangular symmetric CSR matrix to full dense matrix
+MJAPI void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n,
+                         const int* rownnz, const int* rowadr, const int* colind);
+
 // res[row, :] = mat[row, :]
 void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr,
                     const int* row, int nrow);