New implicitfast integrator and sparse RNE derivatives for implicit.

PiperOrigin-RevId: 516910733
Change-Id: I29a0465c0f0b1749a73e3d7e01925200d025ddd0

src/user/user_model.cc

@@ -281,6 +281,7 @@ void mjCModel::Clear(void) {
   nemax = 0;
   nM = 0;
   nD = 0;
+  nB = 0;
   njmax = -1;
   nconmax = -1;
 
@@ -1622,10 +1623,39 @@ void mjCModel::CopyTree(mjModel* m) {
   }
   m->nM = nM;
 
-  // set nD
-  nD = 2*nM - nv;
+  // compute nD
+  nD = 2 * nM - nv;
   m->nD = nD;
 
+  // compute subtreedofs in backward pass over bodies
+  for (i = nbody - 1; i > 0; i--) {
+    // add body dofs to self count
+    bodies[i]->subtreedofs += bodies[i]->dofnum;
+
+    // add to parent count
+    bodies[bodies[i]->parentid]->subtreedofs += bodies[i]->subtreedofs;
+  }
+
+  // make sure all dofs are in world "subtree", SHOULD NOT OCCUR
+  if (bodies[0]->subtreedofs != nv) {
+    throw mjCError(0, "all DOFs should be in world subtree");
+  }
+
+  // compute nB
+  nB = 0;
+  for (i = 0; i < nbody; i++) {
+    // add subtree dofs (including self)
+    nB += bodies[i]->subtreedofs;
+
+    // add dofs in ancestor bodies
+    j = bodies[i]->parentid;
+    while (j > 0) {
+      nB += bodies[j]->dofnum;
+      j = bodies[j]->parentid;
+    }
+  }
+  m->nB = nB;
+
   // set dof_simplenum
   int scnt = 0;
   for (i=nv-1; i>=0; i--) {
@@ -2846,7 +2876,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
       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 ||
-      nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax) {
+      nB!=m->nB || nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax) {
     errInfo = mjCError(0, "incompatible models in CopyBack");
     return false;
   }

src/user/user_model.h

@@ -235,7 +235,8 @@ class mjCModel {
   int npluginattr;                // number of chars in all plugin config attributes
   int nnames;                     // number of chars in all names
   int nM;                         // number of non-zeros in sparse inertia matrix
-  int nD;                         // number of non-zeros in sparse derivative matrix
+  int nD;                         // number of non-zeros in sparse dof-dof matrix
+  int nB;                         // number of non-zeros in sparse body-dof matrix
 
   //------------------------ object lists
   // objects created here

src/user/user_objects.cc

@@ -327,6 +327,7 @@ mjCBody::mjCBody(mjCModel* _model) {
   weldid = -1;
   dofnum = 0;
   lastdof = -1;
+  subtreedofs = 0;
   gravcomp = 0;
   userdata.clear();
 

src/user/user_objects.h

@@ -209,7 +209,9 @@ class mjCBody : public mjCBase {
   int mocapid;                    // mocap id, -1: not mocap
   bool explicitinertial;          // whether to save the body with an explicit inertial clause
 
-  int lastdof;                    // id of last dof (used by compiler)
+  // used internally by compiler
+  int lastdof;                    // id of last dof
+  int subtreedofs;                // number of dofs in subtree, including self
 
   // objects allocated by Add functions
   std::vector<mjCBody*>    bodies;     // child bodies