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/user/user_model.cc

@@ -267,6 +267,7 @@ void mjCModel::Clear(void) {
   nnames = 0;
   nemax = 0;
   nM = 0;
+  nD = 0;
   njmax = -1;
   nconmax = -1;
 
@@ -1543,6 +1544,10 @@ void mjCModel::CopyTree(mjModel* m) {
   }
   m->nM = nM;
 
+  // set nD
+  nD = 2*nM - nv;
+  m->nD = nD;
+
   // set dof_simplenum
   int scnt = 0;
   for (i=nv-1; i>=0; i--) {
@@ -2637,8 +2642,8 @@ 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 || nemax!=m->nemax ||
-      nconmax!=m->nconmax || njmax!=m->njmax) {
+      ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD ||
+      nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax) {
     errInfo = mjCError(0, "incompatible models in CopyBack");
     return false;
   }

src/user/user_model.h

@@ -218,6 +218,7 @@ class mjCModel {
   int ntupledata;                 // number of objects in all tuple fields
   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
 
   //------------------------ object lists
   // objects created here