Add efficient finite-difference Jacobians of mj_step.

- Add `qH` and `qHDiagInv` to `mjData` to save factorized modified inertia.
- Add `mj_EulerSkip`, `mj_implicitSkip`, to `engine_forward.c`.
- Using the above functions, implement `mj_stepSkip` in `engine_derivative.c`.
- Add `mjd_stepFD` and `mjd_transitionFD` to `engine_derivative.c` to compute `mj_step` Jacobians.
  - Exploit "Skip" functionality for speed.
  - Correctly handle quaternion derivatives.
  - Handle warmstarts and control limits.

PiperOrigin-RevId: 456584811
Change-Id: Iee8541f11e7b66feb8f431cb102d9bbe65461f79

include/mujoco/mjdata.h

@@ -286,6 +286,10 @@ struct mjData_ {
   mjtNum*   subtree_linvel;       // linear velocity of subtree com           (nbody x 3)
   mjtNum*   subtree_angmom;       // angular momentum about subtree com       (nbody x 3)
 
+  // computed by mj_Euler
+  mjtNum*   qH;                   // L'*D*L factorization of modified M       (nM x 1)
+  mjtNum*   qHDiagInv;            // 1/diag(D) of modified M                  (nv x 1)
+
   // computed by mj_implicit
   int*      D_rownnz;             // non-zeros in each row                    (nv x 1)
   int*      D_rowadr;             // address of each row in D_colind          (nv x 1)

include/mujoco/mjxmacro.h

@@ -509,6 +509,8 @@
     X( mjtNum,    efc_aref,          njmax,       1           ) \
     X( mjtNum,    subtree_linvel,    nbody,       3           ) \
     X( mjtNum,    subtree_angmom,    nbody,       3           ) \
+    X( mjtNum,    qH,                nM,          1           ) \
+    X( mjtNum,    qHDiagInv,         nv,          1           ) \
     X( int,       D_rownnz,          nv,          1           ) \
     X( int,       D_rowadr,          nv,          1           ) \
     X( int,       D_colind,          nD,          1           ) \

include/mujoco/mujoco.h

@@ -1000,7 +1000,7 @@ MJAPI int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus);
 MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]);
 
 
-//---------------------- Miscellaneous --------------------------------------------------
+//---------------------- Miscellaneous -------------------------------------------------------------
 
 // Muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax).
 MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
@@ -1080,6 +1080,13 @@ MJAPI char* mju_strncpy(char *dst, const char *src, int n);
 MJAPI mjtNum mju_sigmoid(mjtNum x);
 
 
+//---------------------- Derivatives ---------------------------------------------------------------
+
+// finite differenced state-transition and control-transition matrices dy = A*dx + B*du
+MJAPI void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte centered,
+                            mjtNum* A, mjtNum* B);
+
+
 #if defined(__cplusplus)
 }
 #endif