Add mjd_quatIntegrate, expose mjd_subQuat.

PiperOrigin-RevId: 546252926
Change-Id: I7d6b2bd6536e7e8afc368f3c46037c20f6a1cb5e

doc/APIreference/APIfunctions.rst

@@ -29,7 +29,7 @@ API function can be classified as:
 - :ref:`Poses transformations<Poses>`.
 - :ref:`Matrix decompositions and solvers<Decompositions>`.
 - :ref:`Miscellaneous<Miscellaneous>` functions.
-- :ref:`Dynamics derivatives<Derivatives-api>`.
+- :ref:`Derivatives<Derivatives-api>`.
 - :ref:`Plugin<Plugins-api>` related functions.
 - :ref:`Macros<Macros>`.
 

doc/APIreference/functions.rst

@@ -3185,6 +3185,10 @@ Sigmoid function over 0<=x<=1 using quintic polynomial.
 Derivatives
 ^^^^^^^^^^^
 
+The functions below provide useful derivatives of various functions, both analytic and
+finite-differenced. The latter have names with the suffix ``FD``. Note that unlike much of the API,
+outputs of derivative functions are the trailing rather than leading arguments.
+
 .. _mjd_transitionFD:
 
 mjd_transitionFD
@@ -3251,6 +3255,42 @@ using finite-differencing. These matrices and their dimensions are:
 - ``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.
 
+.. _mjd_subQuat:
+
+mjd_subQuat
+~~~~~~~~~~~
+
+.. mujoco-include:: mjd_subQuat
+
+Derivatives of :ref:`mju_subQuat` (quaternion difference).
+
+.. _mjd_quatIntegrate:
+
+mjd_quatIntegrate
+~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mjd_quatIntegrate
+
+Derivatives of :ref:`mju_quatIntegrate`.
+
+:math:`{\tt \small mju\_quatIntegrate}(q, v, h)` performs the in-place rotation :math:`q \leftarrow q + v h`,
+where :math:`q \in \mathbf{S}^3` is a unit quaternion, :math:`v \in \mathbf{R}^3` is a 3D angular velocity and
+:math:`h \in \mathbf{R^+}` is a timestep. This is equivalent to :math:`{\tt \small mju\_quatIntegrate}(q, s, 1.0)`,
+where :math:`s` is the scaled velocity :math:`s = h v`.
+
+:math:`{\tt \small mjd\_quatIntegrate}(v, h, D_q, D_v, D_h)` computes the Jacobians of the output :math:`q` with respect
+to the inputs. Below, :math:`\bar q` denotes the pre-modified quaternion:
+
+.. math::
+   \begin{aligned}
+      D_q &= \partial q / \partial \bar q \\
+      D_v &= \partial q / \partial v \\
+      D_h &= \partial q / \partial h
+   \end{aligned}
+
+Note that derivatives depend only on :math:`h` and :math:`v` (in fact, on :math:`s = h v`).
+All outputs are optional.
+
 .. _Plugins-api:
 
 Plugins

doc/APIreference/functions_override.rst

@@ -435,6 +435,10 @@ Symmetrize square matrix :math:`R = \frac{1}{2}(M + M^T)`.
 
 .. _Derivatives-api:
 
+The functions below provide useful derivatives of various functions, both analytic and
+finite-differenced. The latter have names with the suffix ``FD``. Note that unlike much of the API,
+outputs of derivative functions are the trailing rather than leading arguments.
+
 .. _mjd_transitionFD:
 
 Finite-differenced discrete-time transition matrices.
@@ -490,3 +494,29 @@ using finite-differencing. These matrices and their dimensions are:
 - ``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.
+
+.. _mjd_subQuat:
+
+Derivatives of :ref:`mju_subQuat` (quaternion difference).
+
+.. _mjd_quatIntegrate:
+
+Derivatives of :ref:`mju_quatIntegrate`.
+
+:math:`{\tt \small mju\_quatIntegrate}(q, v, h)` performs the in-place rotation :math:`q \leftarrow q + v h`,
+where :math:`q \in \mathbf{S}^3` is a unit quaternion, :math:`v \in \mathbf{R}^3` is a 3D angular velocity and
+:math:`h \in \mathbf{R^+}` is a timestep. This is equivalent to :math:`{\tt \small mju\_quatIntegrate}(q, s, 1.0)`,
+where :math:`s` is the scaled velocity :math:`s = h v`.
+
+:math:`{\tt \small mjd\_quatIntegrate}(v, h, D_q, D_v, D_h)` computes the Jacobians of the output :math:`q` with respect
+to the inputs. Below, :math:`\bar q` denotes the pre-modified quaternion:
+
+.. math::
+   \begin{aligned}
+      D_q &= \partial q / \partial \bar q \\
+      D_v &= \partial q / \partial v \\
+      D_h &= \partial q / \partial h
+   \end{aligned}
+
+Note that derivatives depend only on :math:`h` and :math:`v` (in fact, on :math:`s = h v`).
+All outputs are optional.