chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
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