Add mjd_subQuat (private function)

PiperOrigin-RevId: 529418945
Change-Id: I6764ae866b13482ef3aa951f427b58cae007c301

src/engine/engine_derivative.c

@@ -219,6 +219,54 @@ static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10])
 
 
 
+// derivative of mju_subQuat w.r.t inputs
+void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9])
+{
+  // no outputs, quick return
+  if (!Da && !Db) {
+    return;
+  }
+
+  // compute axis-angle quaternion difference
+  mjtNum axis[3];
+  mju_subQuat(axis, qa, qb);
+
+  // normalize axis, get half-angle
+  mjtNum half_angle = 0.5 * mju_normalize3(axis);
+
+  // identity
+  mjtNum Da_tmp[9] = {
+    1, 0, 0,
+    0, 1, 0,
+    0, 0, 1
+  };
+
+  // add term linear in cross product matrix K
+  mjtNum K[9] = {
+    0, -axis[2], axis[1],
+    axis[2], 0, -axis[0],
+    -axis[1], axis[0], 0
+  };
+  mju_addToScl(Da_tmp, K, half_angle, 9);
+
+  // add term linear in K * K
+  mjtNum KK[9];
+  mju_mulMatMat(KK, K, K, 3, 3, 3);
+  mjtNum coef = 1.0 - (half_angle < 6e-8 ? 1.0 : half_angle / mju_tan(half_angle));
+  mju_addToScl(Da_tmp, KK, coef, 9);
+
+  if (Da) {
+    mju_copy(Da, Da_tmp, 9);
+  }
+
+  if (Db) {  // Db = -Da^T
+    mju_transpose(Db, Da_tmp, 3, 3);
+    mju_scl(Db, Db, -1.0, 9);
+  }
+}
+
+
+
 //------------------------- dense derivatives of component functions -------------------------------
 // no longer used, except in tests
 

src/engine/engine_derivative.h

@@ -23,6 +23,9 @@
 extern "C" {
 #endif
 
+// derivative of mju_subQuat w.r.t inputs
+MJAPI void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]);
+
 // analytical derivative of smooth forces w.r.t velocities:
 //   d->qDeriv = d (qfrc_actuator + qfrc_passive - [qfrc_bias]) / d qvel
 MJAPI void mjd_smooth_vel(const mjModel* m, mjData* d, int flg_bias);

test/engine/engine_derivative_test.cc

@@ -14,6 +14,7 @@
 
 // Tests for engine/engine_derivative.c.
 
+#include <random>
 #include <vector>
 
 #include <gmock/gmock.h>
@@ -71,6 +72,7 @@ static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected,
 
 // utility function for matrix printing
 static void PrintMatrix(mjtNum* mat, int nrow, int ncol) {
+  // NOLINT(clang-diagnostic-unused-function)
   std::cerr.precision(5);
   std::cerr << "\n";
   for (int r=0; r < nrow; r++) {
@@ -79,7 +81,7 @@ static void PrintMatrix(mjtNum* mat, int nrow, int ncol) {
     }
     std::cerr << "\n";
   }
-}  // NOLINT(clang-diagnostic-unused-function)
+}
 
 
 std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
@@ -737,5 +739,102 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
   mj_deleteModel(model);
 }
 
+// utility: generate two random quaternions with a given angle difference
+void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
+  // make distribution using seed
+  std::mt19937_64 rng;
+  rng.seed(seed);
+  std::normal_distribution<double> dist(0, 1);
+
+  // sample qa = qb
+  for (int i=0; i < 4; i++) {
+    qa[i] = qb[i] = dist(rng);
+  }
+  mju_normalize4(qa);
+  mju_normalize4(qb);
+
+  // integrate qb in random direction by angle
+  mjtNum dir[3];
+  for (int i=0; i < 3; i++) {
+    dir[i] = dist(rng);
+  }
+  mju_normalize3(dir);
+  mju_quatIntegrate(qb, dir, angle);
+}
+
+// utility: finite-difference Jacobians of mju_subQuat
+void mjd_subQuatFD(mjtNum Da[9], mjtNum Db[9],
+                   const mjtNum qa[4], const mjtNum qb[4], mjtNum eps) {
+  // subQuat
+  mjtNum y[3];
+  mju_subQuat(y, qa, qb);
+
+  mjtNum dq[3];   // nudge input direction
+  mjtNum dqa[4];  // nudged qa input
+  mjtNum dqb[4];  // nudged qb input
+  mjtNum dy[3];   // nudged output
+  mjtNum DaT[9];  // Da transposed
+  mjtNum DbT[9];  // Db transposed
+
+  for (int i = 0; i < 3; i++) {
+    // perturbation
+    mju_zero3(dq);
+    dq[i] = 1.0;
+
+    // Jacobian: d_y / d_qa
+    mju_copy4(dqa, qa);
+    mju_quatIntegrate(dqa, dq, eps);
+    mju_subQuat(dy, dqa, qb);
+
+    mju_sub3(DaT + i * 3, dy, y);
+    mju_scl3(DaT + i * 3, DaT + i * 3, 1.0 / eps);
+
+    // Jacobian: d_y / d_qb
+    mju_copy4(dqb, qb);
+    mju_quatIntegrate(dqb, dq, eps);
+    mju_subQuat(dy, qa, dqb);
+
+    mju_sub3(DbT + i * 3, dy, y);
+    mju_scl3(DbT + i * 3, DbT + i * 3, 1.0 / eps);
+  }
+
+  // transpose result
+  mju_transpose(Da, DaT, 3, 3);
+  mju_transpose(Db, DbT, 3, 3);
+}
+
+TEST_F(DerivativeTest, SubQuat) {
+  const int nrepeats = 10;  // number of repeats
+  const mjtNum eps = 1e-7;  // epsilon for finite-differencing and comparison
+
+  int seed = 1;
+  for (int i = 0; i < nrepeats; i++) {
+    for (mjtNum angle : {0.0, 1e-9, 1e-5, 1e-2, 1.0, 4.0}) {
+      // random quaternions
+      mjtNum qa[4];
+      mjtNum qb[4];
+
+      // make random quaternion pair with given relative angle
+      randomQuatPair(qa, qb, angle, seed++);
+
+      // analytic Jacobians
+      mjtNum Da[9];  // d_subQuat(qa, qb) / d_qa
+      mjtNum Db[9];  // d_subQuat(qa, qb) / d_qb
+      mjd_subQuat(qa, qb, Da, Db);
+
+      // finite-differenced Jacobians
+      mjtNum DaFD[9];
+      mjtNum DbFD[9];
+      mjd_subQuatFD(DaFD, DbFD, qa, qb, eps);
+
+      // expect numerical equality
+      EXPECT_THAT(AsVector(DaFD, 9),
+                  Pointwise(DoubleNear(eps), AsVector(Da, 9)));
+      EXPECT_THAT(AsVector(DbFD, 9),
+                  Pointwise(DoubleNear(eps), AsVector(Db, 9)));
+    }
+  }
+}
+
 }  // namespace
 }  // namespace mujoco