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
This commit is contained in:
Yuval Tassa
2022-06-22 12:48:04 -07:00
committed by Copybara-Service
parent 2ea01bf2f6
commit 228264c92b
13 changed files with 938 additions and 96 deletions
+8 -1
View File
@@ -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