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.

doc/changelog.rst

@@ -8,9 +8,12 @@ Upcoming version (not yet released)
 General
 ^^^^^^^
 
+- Added primitive collider for sphere-cylinder contacts, previously this pair used the generic convex-convex collider.
+- Added analytic derivatives for quaternion :ref:`subtraction<mjd_subQuat>` and :ref:`integration<mjd_quatIntegrate>`
+  (rotation with an angular velocity). Derivatives are in the 3D tangent space.
 - Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient
   "from-to" argument parametrization. :ref:`mjv_makeConnector` is now deprecated.
-- Added primitive collider for sphere-cylinder contacts, previously this pair used the generic convex-convex collider.
+
 
 Version 2.3.6 (June 20, 2023)
 -----------------------------

doc/includes/references.h

@@ -2494,6 +2494,9 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
                    mjtNum *DfDq, mjtNum *DfDv, mjtNum *DfDa,
                    mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa,
                    mjtNum *DmDq);
+void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]);
+void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale,
+                       mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]);
 void mjp_defaultPlugin(mjpPlugin* plugin);
 int mjp_registerPlugin(const mjpPlugin* plugin);
 int mjp_pluginCount();