Don't normalize mocap quaternions in-place.

PiperOrigin-RevId: 648394295 Change-Id: I348a0a045c1697f6ef782280d92c90a30759eb33
google-deepmind · Jul 1, 2024 · 0c3698f · 0c3698f
1 parent c5cefdc
commit 0c3698f
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 12 deletions.
diff --git a/doc/changelog.rst b/doc/changelog.rst
@@ -44,8 +44,9 @@ General
    point, as usual. If ``mjUSESINGLE`` is defined, MuJoCo will use single-precision floating point. See :ref:`mjtNum`.
 
    Relatedly, fixed various type errors that prevented building with single-precision.
-10. Quaternions in ``mjData->qpos`` are no longer normalized in-place by :ref:`mj_kinematics`. Instead they are
-    normalized when they are used. After the first step, quaternions will be normalized.
+10. Quaternions in ``mjData->qpos`` and ``mjData->mocap_quat`` are no longer normalized in-place by
+    :ref:`mj_kinematics`. Instead they are normalized when they are used. After the first step, quaternions in
+    ``mjData->qpos`` will be normalized.
 
 MJX
 ~~~

diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c
@@ -44,11 +44,6 @@ void mj_kinematics(const mjModel* m, mjData* d) {
   d->xmat[0] = d->xmat[4] = d->xmat[8] = 1;
   d->ximat[0] = d->ximat[4] = d->ximat[8] = 1;
 
-  // normalize mocap quaternions
-  for (int i=0; i < m->nmocap; i++) {
-    mju_normalize4(d->mocap_quat+4*i);
-  }
-
   // compute global cartesian positions and orientations of all bodies
   for (int i=1; i < m->nbody; i++) {
     mjtNum xpos[3], xquat[4];
@@ -75,10 +70,12 @@ void mj_kinematics(const mjModel* m, mjData* d) {
       int pid = m->body_parentid[i];
 
       // get body pos and quat: from model or mocap
-      mjtNum *bodypos, *bodyquat;
+      mjtNum *bodypos, *bodyquat, quat[4];
       if (m->body_mocapid[i] >= 0) {
         bodypos = d->mocap_pos + 3*m->body_mocapid[i];
-        bodyquat = d->mocap_quat + 4*m->body_mocapid[i];
+        mju_copy4(quat, d->mocap_quat + 4*m->body_mocapid[i]);
+        mju_normalize4(quat);
+        bodyquat = quat;
       } else {
         bodypos = m->body_pos+3*i;
         bodyquat = m->body_quat+4*i;

diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc
@@ -709,10 +709,48 @@ TEST_F(ForwardTest, NormalizeQuats) {
   mj_deleteData(data_n);
   mj_deleteData(data_u);
   mj_deleteModel(model);
+}
 
-#ifdef STOP_MSAN
-  #define MEMORY_SANITIZER
-#endif
+// test that normalized and denormalized quats give the same result
+TEST_F(ForwardTest, MocapQuats) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="mocap" mocap="true" quat="1 1 1 1">
+        <geom size="1"/>
+      </body>
+    </worldbody>
+    <sensor>
+      <framequat objtype="body" objname="mocap"/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, NotNull());
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  // expect mocap_quat to be normalized (by the compiler)
+  for (int i = 0; i < 4; i++) {
+    EXPECT_EQ(data->mocap_quat[i], 0.5);
+    EXPECT_EQ(data->xquat[4+i], 0.5);
+  }
+
+  // write denormalized quats to mocap_quat, call forward again
+  for (int i = 0; i < 4; i++) {
+    data->mocap_quat[i] = 1;
+  }
+  mj_forward(model, data);
+
+  // expect mocap_quat to remain denormalized, but xquat to be normalized
+  for (int i = 0; i < 4; i++) {
+    EXPECT_EQ(data->mocap_quat[i], 1);
+    EXPECT_EQ(data->xquat[4+i], 0.5);
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
 }
 
 // user defined 2nd-order activation dynamics: frequency-controlled oscillator