Add mjd_quatIntegrate, expose mjd_subQuat.

PiperOrigin-RevId: 546252926
Change-Id: I7d6b2bd6536e7e8afc368f3c46037c20f6a1cb5e

test/engine/engine_derivative_test.cc

@@ -70,9 +70,9 @@ static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected,
   return max_error;
 }
 
-// utility function for matrix printing
+// utility function for matrix printing (debug)
+// NOLINTNEXTLINE(clang-diagnostic-unused-function)
 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++) {
@@ -836,5 +836,117 @@ TEST_F(DerivativeTest, SubQuat) {
   }
 }
 
+// utility: random quaternion, 3D velocity
+void randomQuatVel(mjtNum quat[4], mjtNum vel[3], int seed) {
+  // make distribution using seed
+  std::mt19937_64 rng;
+  rng.seed(seed);
+  std::normal_distribution<double> dist(0, 1);
+
+  // sample quat
+  for (int i=0; i < 4; i++) {
+    quat[i] = dist(rng);
+  }
+  mju_normalize4(quat);
+
+  // sample vel
+  for (int i=0; i < 3; i++) {
+    vel[i] = dist(rng);
+  }
+}
+
+// utility: finite-difference Jacobians of mju_quatIntegrate
+void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9],
+                         mjtNum Dvel[9], mjtNum Dh[3],
+                         const mjtNum quat[4], const mjtNum vel[3],
+                         mjtNum h, mjtNum eps) {
+  // compute y, output of mju_quatIntegrate(quat, vel, h)
+  mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]};
+  mju_quatIntegrate(y, vel, h);
+
+  mjtNum dx[3];      // nudged tangent-space input
+  mjtNum dq[4];      // quat output
+  mjtNum dy[3];      // nudged tangent-space output
+  mjtNum DquatT[9];  // Dquat transposed
+  mjtNum DsT[9];     // Ds transposed
+  mjtNum DvelT[9];   // Dvel transposed
+
+  for (int i = 0; i < 3; i++) {
+    // perturbation
+    mju_zero3(dx);
+    dx[i] = 1.0;
+
+    // d_y / d_quat
+    mju_copy4(dq, quat);
+    mju_quatIntegrate(dq, dx, eps);  // nudge dq
+    mju_quatIntegrate(dq, vel, h);  // compute nudged
+    mju_subQuat(dy, dq, y);          // subtract
+    mju_scl3(DquatT + i * 3, dy, 1.0 / eps);
+
+    // d_y / d_sv (scaled velocity)
+    mju_copy4(dq, quat);
+    mjtNum dsv[3] = {vel[0]*h, vel[1]*h, vel[2]*h};
+    mju_addToScl3(dsv, dx, eps);      // nudge dsv
+    mju_quatIntegrate(dq, dsv, 1.0);  // compute nudged
+    mju_subQuat(dy, dq, y);           // subtract
+    mju_scl3(DsT + i * 3, dy, 1.0 / eps);
+
+    // d_y / d_v (unscaled velocity)
+    mju_copy4(dq, quat);
+    mjtNum dv[3] = {vel[0], vel[1], vel[2]};
+    mju_addToScl3(dv, dx, eps);    // nudge dv
+    mju_quatIntegrate(dq, dv, h);  // compute nudged
+    mju_subQuat(dy, dq, y);        // subtract
+    mju_scl3(DvelT + i * 3, dy, 1.0 / eps);
+  }
+
+  // d_y / d_h (unscaled velocity)
+  mju_copy4(dq, quat);
+  mju_quatIntegrate(dq, vel, h + eps);  // compute nudged
+  mju_subQuat(dy, dq, y);               // subtract
+  mju_scl3(Dh, dy, 1.0 / eps);
+
+  // transpose
+  mju_transpose(Dquat, DquatT, 3, 3);
+  mju_transpose(Ds, DsT, 3, 3);
+  mju_transpose(Dvel, DsT, 3, 3);
+}
+
+TEST_F(DerivativeTest, quatIntegrate) {
+  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 h : {0.0, 1e-9, 1e-5, 1e-2, 1.0, 4.0}) {
+      // make random quaternion and velocity
+      mjtNum quat[4];
+      mjtNum vel[3];
+      randomQuatVel(quat, vel, seed++);
+
+      // analytic Jacobians
+      mjtNum Dquat[9];  // d_quatIntegrate(quat, vel, h) / d_quat
+      mjtNum Dvel[9];   // d_quatIntegrate(quat, vel, h) / d_vel
+      mjtNum Dh[3];     // d_quatIntegrate(quat, vel, h) / d_h
+      mjd_quatIntegrate(vel, h, Dquat, Dvel, Dh);
+
+      // finite-differenced Jacobians
+      mjtNum DquatFD[9];
+      mjtNum DsFD[9];
+      mjtNum DvelFD[9];
+      mjtNum DhFD[3];
+      mjd_quatIntegrateFD(DquatFD, DsFD, DvelFD, DhFD, quat, vel, h, eps);
+
+      // expect numerical equality of un/scaled velocity derivatives
+      EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(DoubleNear(eps), DsFD));
+
+      // expect numerical equality of analytic and FD derivatives
+      EXPECT_THAT(AsVector(DquatFD, 9), Pointwise(DoubleNear(eps), Dquat));
+      EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(DoubleNear(eps), Dvel));
+      EXPECT_THAT(AsVector(DhFD, 3), Pointwise(DoubleNear(eps), Dh));
+    }
+  }
+}
+
 }  // namespace
 }  // namespace mujoco