Add mjd_inverseFD for finite-difference approximations of inverse dynamics Jacobians.

Fixes #703.

PiperOrigin-RevId: 527899700
Change-Id: I10e41a381dcecf62c53b3b9aa72a4ce666161366

doc/APIreference/functions.rst

@@ -3068,10 +3068,13 @@ mjd_transitionFD
 
 .. mujoco-include:: mjd_transitionFD
 
-Finite differenced transition matrices. Letting :math:`x, u` denote the current :ref:`state<gePhysicsState>` and control
-vectors and letting :math:`y, s` denote the next state and sensor values, the top-level :ref:`mj_step` function computes
-:math:`(x,u) \rightarrow (y,s)`. :ref:`mjd_transitionFD` computes the four associated Jacobians using
-finite-differencing. These matrices and their dimensions are:
+Finite-differenced discrete-time transition matrices.
+
+Letting :math:`x, u` denote the current :ref:`state<gePhysicsState>` and :ref:`control<geInput>`
+vector in an :ref:`mjData` instance, and letting :math:`y, s` denote the next state and sensor
+values, the top-level :ref:`mj_step` function computes :math:`(x,u) \rightarrow (y,s)`.
+:ref:`mjd_transitionFD` computes the four associated Jacobians using finite-differencing.
+These matrices and their dimensions are:
 
 .. csv-table::
    :header: "matrix", "Jacobian", "dimension"
@@ -3083,10 +3086,47 @@ finite-differencing. These matrices and their dimensions are:
    ``C``, :math:`\partial s / \partial x`, ``nsensordata x 2*nv+na``
    ``D``, :math:`\partial s / \partial u`, ``nsensordata x nu``
 
-- All four matrix outputs are optional (can be NULL).
+- All outputs are optional (can be NULL).
 - ``eps`` is the finite-differencing epsilon.
 - ``flg_centered`` denotes whether to use forward (0) or centered (1) differences.
 
+.. _mjd_inverseFD:
+
+mjd_inverseFD
+~~~~~~~~~~~~~
+
+.. mujoco-include:: mjd_inverseFD
+
+Finite differenced continuous-time inverse-dynamics Jacobians.
+
+Letting :math:`x, a` denote the current :ref:`state<gePhysicsState>` and acceleration vectors in an :ref:`mjData` instance, and
+letting :math:`f, s` denote the forces computed by the inverse dynamics (``qfrc_inverse``), the function
+:ref:`mj_inverse` computes :math:`(x,a) \rightarrow (f,s)`. :ref:`mjd_inverseFD` computes seven associated Jacobians
+using finite-differencing. These matrices and their dimensions are:
+
+.. csv-table::
+   :header: "matrix", "Jacobian", "dimension"
+   :widths: auto
+   :align: left
+
+   ``DfDq``, :math:`\partial f / \partial q`, ``nv x nv``
+   ``DfDv``, :math:`\partial f / \partial v`, ``nv x nv``
+   ``DfDa``, :math:`\partial f / \partial a`, ``na x nv``
+   ``DsDq``, :math:`\partial s / \partial q`, ``nv x nsensordata``
+   ``DsDv``, :math:`\partial s / \partial v`, ``nv x nsensordata``
+   ``DsDa``, :math:`\partial s / \partial a`, ``nv x nsensordata``
+   ``DmDq``, :math:`\partial M / \partial q`, ``nv x nM``
+
+- All outputs are optional (can be NULL).
+- All outputs are transposed relative to Control Theory convention (i.e., column major).
+- ``DmDq``, which contains a sparse representation of the ``nv x nv x nv`` tensor :math:`\partial M / \partial q`, is
+  not strictly an inverse dynamics Jacobian but is useful in related applications. It is provided as a convenience to
+  the user, since the required values are already computed if either of the other two :math:`\partial / \partial q`
+  Jacobians are requested.
+- ``eps`` is the (forward) finite-differencing epsilon.
+- ``flg_actuation`` denotes whether to subtract actuation forces (``qfrc_actuator``) from the output of the inverse
+  dynamics. If this flag is positive, actuator forces are not considered as external.
+
 .. _Plugins-api:
 
 Plugins

doc/APIreference/functions_override.rst

@@ -371,10 +371,13 @@ Symmetrize square matrix :math:`R = \frac{1}{2}(M + M^T)`.
 
 .. _mjd_transitionFD:
 
-Finite differenced transition matrices. Letting :math:`x, u` denote the current :ref:`state<gePhysicsState>` and control
-vectors and letting :math:`y, s` denote the next state and sensor values, the top-level :ref:`mj_step` function computes
-:math:`(x,u) \rightarrow (y,s)`. :ref:`mjd_transitionFD` computes the four associated Jacobians using
-finite-differencing. These matrices and their dimensions are:
+Finite-differenced discrete-time transition matrices.
+
+Letting :math:`x, u` denote the current :ref:`state<gePhysicsState>` and :ref:`control<geInput>`
+vector in an mjData instance, and letting :math:`y, s` denote the next state and sensor
+values, the top-level :ref:`mj_step` function computes :math:`(x,u) \rightarrow (y,s)`.
+:ref:`mjd_transitionFD` computes the four associated Jacobians using finite-differencing.
+These matrices and their dimensions are:
 
 .. csv-table::
    :header: "matrix", "Jacobian", "dimension"
@@ -386,7 +389,38 @@ finite-differencing. These matrices and their dimensions are:
    ``C``, :math:`\partial s / \partial x`, ``nsensordata x 2*nv+na``
    ``D``, :math:`\partial s / \partial u`, ``nsensordata x nu``
 
-- All four matrix outputs are optional (can be NULL).
+- All outputs are optional (can be NULL).
 - ``eps`` is the finite-differencing epsilon.
 - ``flg_centered`` denotes whether to use forward (0) or centered (1) differences.
 
+.. _mjd_inverseFD:
+
+Finite differenced continuous-time inverse-dynamics Jacobians.
+
+Letting :math:`x, a` denote the current :ref:`state<gePhysicsState>` and acceleration vectors in an mjData instance, and
+letting :math:`f, s` denote the forces computed by the inverse dynamics (``qfrc_inverse``), the function
+:ref:`mj_inverse` computes :math:`(x,a) \rightarrow (f,s)`. :ref:`mjd_inverseFD` computes seven associated Jacobians
+using finite-differencing. These matrices and their dimensions are:
+
+.. csv-table::
+   :header: "matrix", "Jacobian", "dimension"
+   :widths: auto
+   :align: left
+
+   ``DfDq``, :math:`\partial f / \partial q`, ``nv x nv``
+   ``DfDv``, :math:`\partial f / \partial v`, ``nv x nv``
+   ``DfDa``, :math:`\partial f / \partial a`, ``na x nv``
+   ``DsDq``, :math:`\partial s / \partial q`, ``nv x nsensordata``
+   ``DsDv``, :math:`\partial s / \partial v`, ``nv x nsensordata``
+   ``DsDa``, :math:`\partial s / \partial a`, ``nv x nsensordata``
+   ``DmDq``, :math:`\partial M / \partial q`, ``nv x nM``
+
+- All outputs are optional (can be NULL).
+- All outputs are transposed relative to Control Theory convention (i.e., column major).
+- ``DmDq``, which contains a sparse representation of the ``nv x nv x nv`` tensor :math:`\partial M / \partial q`, is
+  not strictly an inverse dynamics Jacobian but is useful in related applications. It is provided as a convenience to
+  the user, since the required values are already computed if either of the other two :math:`\partial / \partial q`
+  Jacobians are requested.
+- ``eps`` is the (forward) finite-differencing epsilon.
+- ``flg_actuation`` denotes whether to subtract actuation forces (``qfrc_actuator``) from the output of the inverse
+  dynamics. If this flag is positive, actuator forces are not considered as external.

doc/computation.rst

@@ -1590,24 +1590,20 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu
 Derivatives
 -----------
 
-MuJoCo's entire computational pipline and uniquely -- its constraint solver -- are analytically differentiable. Writing
+MuJoCo's entire computational pipline including its constraint solver are analytically differentiable. Writing
 efficient implementations of these derivatives is a long term goal of the development team. Analytic derivatives of the
-smooth dynamics with respect to velocity are already in place and power the :ref:`implicit integrator<geIntegration>`.
+smooth dynamics (excluding constraints) with respect to velocity are already computed and enable the two
+:ref:`implicit integrators<geIntegration>`.
 
-The function :ref:`mjd_transitionFD` computes state-transition and control-transition Jacobians. Given any valid MuJoCo
-model ``mjModel* m`` with an initial :ref:`simulation state<geState>` in ``mjData* d``,
+Two functions are currently available which use efficient finite-differencing in order to compute dynamics Jacobians:
 
-- Let :math:`x` denote the :ref:`physics state<gePhysicsState>` of the simulation at time :math:`t` -- the concatenation
-  of positions, velocities and actuator states ``[d->qpos; d->qvel; d->act]``.
-- Let :math:`u` denote the vector of controls at time :math:`t`, corresponding to ``d->ctrl``.
-- Let :math:`y` denote the physical state of the simulation at time :math:`t+h`, where :math:`h` corresponds to
-  ``m->opt.timstep``.
-- Let :math:`s` denote the values of the sensors defined in the model.
-- The high level function :ref:`mj_step` computes :math:`(x,u) \rightarrow (y,s)`: the next state and
-  sensor values as a function of the current state and control.
-- ``mjd_transitionFD`` computes the Jacobians :math:`A = \frac{\partial y}{\partial x}`,
-  :math:`B = \frac{\partial y}{\partial u}`, :math:`C = \frac{\partial s}{\partial x}` and
-  :math:`D = \frac{\partial s}{\partial u}` using efficient finite-differencing of :ref:`mj_step`.
+:ref:`mjd_transitionFD`:
+  Computes state-transition and control-transition Jacobians for the discrete-time forward dynamics (:ref:`mj_step`).
+  See :ref:`API documentation<mjd_transitionFD>`.
+
+:ref:`mjd_inverseFD`:
+  Computes Jacobians for the continuous-time inverse dynamics (:ref:`mj_inverse`).
+  See :ref:`API documentation<mjd_inverseFD>`.
 
 These derivatives are made efficient by exploiting MuJoCo's configurable computation pipeline so that quantities are not
 recomputed when not required. For example when differencing with respect to controls, quantities which depend only on

doc/includes/references.h

@@ -2441,6 +2441,10 @@ char* mju_strncpy(char *dst, const char *src, int n);
 mjtNum mju_sigmoid(mjtNum x);
 void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
                       mjtNum* A, mjtNum* B, mjtNum* C, mjtNum* D);
+void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuation,
+                   mjtNum *DfDq, mjtNum *DfDv, mjtNum *DfDa,
+                   mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa,
+                   mjtNum *DmDq);
 void mjp_defaultPlugin(mjpPlugin* plugin);
 int mjp_registerPlugin(const mjpPlugin* plugin);
 int mjp_pluginCount();