diff --git a/doc/changelog.rst b/doc/changelog.rst index 6832af733f..ae29a32343 100644 --- 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 index 8d1e2144be..4016df9765 100644 --- 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 index 72f3bd12e8..6c269b7785 100644 --- 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"( + + + + + + + + + + + )"; + 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