diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 13ef24fd..fd80a5a2 100644 --- a/src/engine/engine_derivative.c +++ b/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 diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index 86f7cd66..52fdf5c8 100644 --- a/src/engine/engine_derivative.h +++ b/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); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 68a80c9d..5c55f7ff 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -14,6 +14,7 @@ // Tests for engine/engine_derivative.c. +#include #include #include @@ -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 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 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