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
+28
View File
@@ -0,0 +1,28 @@
<mujoco>
<!-- linear dynamical system with fixed, anaylticaly computable derivatives -->
<default>
<joint axis="1 0 0" damping=".01" stiffness=".1"/>
<geom type="box" size=".05 .05 .05"/>
</default>
<worldbody>
<light pos="0 0 1"/>
<body>
<joint name="joint0"/>
<geom/>
<body pos=".15 0 0">
<joint name="joint1"/>
<geom/>
<body pos=".15 0 0">
<joint/>
<geom/>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor joint="joint0" ctrllimited="true" ctrlrange="-1 1"/>
<motor joint="joint1" ctrllimited="true" ctrlrange="-1 1"/>
</actuator>
</mujoco>