Add implicit integrator.

Added analytic derivatives of smooth (unconstrained) dynamics forces, with respect to velocities:
  - Centripetal and Coriolis forces computed by the Recursive Newton-Euler algorithm.
  - Damping and fluid-drag passive forces.
  - Actuation forces.

A new implicit-in-velocity integrator is implemented using the analytic derivatives. This integrator lies between the Euler and Runge Kutta integrators in terms of both stability and computational cost.

PiperOrigin-RevId: 450377010
Change-Id: Ie192b441876c22e732fb749333926f296e0a09cc

src/engine/engine_support.c

@@ -852,6 +852,98 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
 
 
 
+// construct sparse matrix representations matching qM
+void mj_makeMSparse(const mjModel* m, mjData* d, int* rownnz, int* rowadr, int* colind) {
+  int nv = m->nv;
+
+  mjMARKSTACK;
+  int *remaining = (int*) mj_stackAlloc(d, nv);
+
+  // compute rownnz
+  memset(rownnz, 0, nv*sizeof(int));
+  for (int i=nv-1; i>=0; i--) {
+    // init at diagonal
+    int j = i;
+    rownnz[i]++;
+
+    // process below diagonal
+    while ((j=m->dof_parentid[j]) >= 0) {
+      rownnz[i]++;
+      rownnz[j]++;
+    }
+  }
+
+  // accumulate rowadr
+  rowadr[0] = 0;
+  for (int i=1; i<nv; i++) {
+    rowadr[i] = rowadr[i-1] + rownnz[i-1];
+  }
+
+  // populate colind
+  memcpy(remaining, rownnz, nv*sizeof(int));
+  for (int i=nv-1; i>=0; i--) {
+    // init at diagonal
+    remaining[i]--;
+    colind[rowadr[i] + remaining[i]] = i;
+
+    // process below diagonal
+    int j = i;
+    while ((j = m->dof_parentid[j]) >= 0) {
+      remaining[i]--;
+      colind[rowadr[i] + remaining[i]] = j;
+
+      remaining[j]--;
+      colind[rowadr[j] + remaining[j]] = i;
+    }
+  }
+
+  // sanity check; SHOULD NOT OCCUR
+  for (int i=0; i<nv; i++) {
+    if (remaining[i]!=0) {
+      mju_error("Error in mj_makeMSparse: unexpected remaining");
+    }
+  }
+
+  mjFREESTACK
+}
+
+
+
+// set dst = qM, handle different sparsity representations
+void mj_setMSparse(const mjModel* m, mjData* d, mjtNum* dst,
+                   const int *rownnz, const int *rowadr, const int *colind) {
+  int nv = m->nv;
+
+  mjMARKSTACK;
+  int *remaining = (int*) mj_stackAlloc(d, nv);
+
+  // copy data
+  memcpy(remaining, rownnz, nv*sizeof(int));
+  for (int i=nv-1; i>=0; i--) {
+    // init at diagonal
+    int adr = m->dof_Madr[i];
+    remaining[i]--;
+    dst[rowadr[i] + remaining[i]] = d->qM[adr];
+    adr++;
+
+    // process below diagonal
+    int j = i;
+    while ((j = m->dof_parentid[j]) >= 0) {
+      remaining[i]--;
+      dst[rowadr[i] + remaining[i]] = d->qM[adr];
+
+      remaining[j]--;
+      dst[rowadr[j] + remaining[j]] = d->qM[adr];
+
+      adr++;
+    }
+  }
+
+  mjFREESTACK
+}
+
+
+
 //-------------------------- perturbations ---------------------------------------------------------
 
 // add cartesian force and torque to qfrc_target