chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Don't normalize mocap quaternions in-place.

Browse Source 
PiperOrigin-RevId: 648394295
Change-Id: I348a0a045c1697f6ef782280d92c90a30759eb33
This commit is contained in:
Yuval Tassa
2024-07-01 09:29:02 -07:00
committed by Copybara-Service
parent c5cefdc2f6
commit 0c3698f278
3 changed files with 48 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+3 -2
View File
@@ -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
~~~
src/engine/engine_core_smooth.c
+4 -7
View File
@@ -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;
test/engine/engine_forward_test.cc
+41 -3
View File
@@ -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