Add reduced sparse dof-dof matrix structures to mjData.

PiperOrigin-RevId: 670919263
Change-Id: I3592de802ae84648d97230115c2e9c3437424968

src/user/user_model.cc

@@ -2362,6 +2362,20 @@ void mjCModel::CopyTree(mjModel* m) {
     }
     m->dof_simplenum[i] = count;
   }
+
+  // compute nC
+  int nOD = 0;  // number of off-diagonal (non-simple) parent dofs
+  for (int i=0; i<nv; i++) {
+    // count ancestor (off-diagonal) dofs
+    if (!m->dof_simplenum[i]) {
+      int j = i;
+      while (j >= 0) {
+        if (j != i) nOD++;
+        j = m->dof_parentid[j];
+      }
+    }
+  }
+   m->nC = nC = 2 * nOD + nv;
 }
 
 
@@ -4100,7 +4114,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
       nmat != m->nmat || ntex != m->ntex || npair!=m->npair || nexclude!=m->nexclude ||
       neq!=m->neq || ntendon!=m->ntendon || nwrap!=m->nwrap || nsensor!=m->nsensor ||
       nnumeric!=m->nnumeric || nnumericdata!=m->nnumericdata || ntext!=m->ntext ||
-      ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD ||
+      ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD || nC!=m->nC ||
       nB!=m->nB || nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax ||
       npaths!=m->npaths) {
     errInfo = mjCError(0, "incompatible models in CopyBack");

src/user/user_model.h

@@ -114,8 +114,9 @@ class mjCModel_ : public mjsElement {
   int nnames;          // number of chars in all names
   int npaths;          // number of chars in all paths
   int nM;              // number of non-zeros in sparse inertia matrix
-  int nD;              // number of non-zeros in sparse dof-dof matrix
   int nB;              // number of non-zeros in sparse body-dof matrix
+  int nC;              // number of non-zeros in reduced sparse dof-dof matrix
+  int nD;              // number of non-zeros in sparse dof-dof matrix
 
   // statistics, as computed by mj_setConst
   double meaninertia_auto;  // mean diagonal inertia, as computed by mj_setConst