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

Don't normalize mjData->qpos quaternions in-place.

Browse Source 
PiperOrigin-RevId: 647927542
Change-Id: I13b0be55498d1da3af2cdc414cfed4c3908a6fe1
This commit is contained in:
Yuval Tassa
2024-06-29 03:06:07 -07:00
committed by Copybara-Service
parent 0e19722c61
commit 4d4b0bb2c3
10 changed files with 157 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
+13 -2
View File
@@ -223,11 +223,23 @@ TEST_F(DerivativeTest, StepSkip) {
mjINT_IMPLICITFAST}) {
model->opt.integrator = integrator;
// reset, take 20 steps, save initial state
// reset, take 20 steps
mj_resetData(model, data);
for (int i=0; i < 20; i++) {
mj_step(model, data);
}
// denormalize the quat, just to see that it doesn't make a difference
for (int j=0; j < model->njnt; j++) {
if (model->jnt_type[j] == mjJNT_BALL) {
int adr = model->jnt_qposadr[j];
for (int k=0; k < 4; k++) {
data->qpos[adr + k] *= 8;
}
}
}
// save state
std::vector<mjtNum> qpos = AsVector(data->qpos, nq);
std::vector<mjtNum> qvel = AsVector(data->qvel, nv);
@@ -279,7 +291,6 @@ TEST_F(DerivativeTest, StepSkip) {
mj_deleteModel(model);
}
// Analytic transition matrices for linear dynamical system xn = A*x + B*u
// given modified mass matrix H (`data->qH`) and
// Ac = H^-1 [diag(-stiffness) diag(-damping)]
test/engine/engine_forward_test.cc
+108
View File
@@ -27,11 +27,16 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include <mujoco/mjxmacro.h>
#include "src/cc/array_safety.h"
#include "src/engine/engine_callback.h"
#include "src/engine/engine_io.h"
#include "test/fixture.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#endif
namespace mujoco {
namespace {
@@ -607,6 +612,109 @@ TEST_F(ForwardTest, eq_active) {
mj_deleteModel(model);
}
// test that normalized and denormalized quats give the same result
TEST_F(ForwardTest, NormalizeQuats) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicit">
<flag warmstart="disable" energy="enable"/>
</option>
<worldbody>
<body name="free">
<freejoint/>
<geom size="1" pos=".1 .2 .3"/>
</body>
<body pos="3 0 0">
<joint name="ball" type="ball" stiffness="100" range="0 10"/>
<geom size="1" pos=".1 .2 .3"/>
</body>
</worldbody>
<sensor>
<ballquat joint="ball"/>
<framequat objtype="body" objname="free"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data_u = mj_makeData(model);
// we'll compare all the memory, so unpoison it first
#ifdef MEMORY_SANITIZER
__msan_unpoison(data_u->buffer, data_u->nbuffer);
__msan_unpoison(data_u->arena, data_u->narena);
#endif
// set quats to denormalized values, non-zero velocities
for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i;
for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i;
// copy data and normalize quats
mjData* data_n = mj_copyData(nullptr, model, data_u);
mj_normalizeQuat(model, data_n->qpos);
// call forward, expect quats to be untouched
mj_forward(model, data_u);
for (int i = 3; i < model->nq; i++) {
EXPECT_EQ(data_u->qpos[i], (mjtNum)i);
}
// expect that the ball joint limit is active
EXPECT_EQ(data_u->nl, 1);
// step both models
mj_step(model, data_u);
mj_step(model, data_n);
// expect everything to match
MJDATA_POINTERS_PREAMBLE(model)
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]);
MJDATA_POINTERS;
#undef X
// repeat the above with RK4 integrator
model->opt.integrator = mjINT_RK4;
// reset data, unpoison
mj_resetData(model, data_u);
#ifdef MEMORY_SANITIZER
__msan_unpoison(data_u->buffer, data_u->nbuffer);
__msan_unpoison(data_u->arena, data_u->narena);
#endif
// set quats to un-normalized values, non-zero velocities
for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i;
for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i;
// copy data and normalize quats
mj_copyData(data_n, model, data_u);
mj_normalizeQuat(model, data_n->qpos);
// step both models
mj_step(model, data_u);
mj_step(model, data_n);
// expect everything to match
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]);
MJDATA_POINTERS;
#undef X
mj_deleteData(data_n);
mj_deleteData(data_u);
mj_deleteModel(model);
#ifdef STOP_MSAN
#define MEMORY_SANITIZER
#endif
}
// user defined 2nd-order activation dynamics: frequency-controlled oscillator
// note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but
// since we have a vector output we write into act_dot directly
test/engine/engine_sensor_test.cc
+1 -5
View File
@@ -207,15 +207,11 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
}
mj_forward(model, data);
// note that in the loop above the quat is unnormalized, but that's ok,
// quaternions are automatically normalized in place:
EXPECT_NEAR(mju_norm(data->qpos+3, 4), 1.0, tol);
// get values from relative sensors after moving the object
std::vector actual_values(data->sensordata+nsensordata/2,
data->sensordata+nsensordata);
// object and reference have moved together, we expect values to not unchange
// object and reference have moved together, we expect values to not change
EXPECT_THAT(actual_values, Pointwise(DoubleNear(tol), expected_values));
mj_deleteData(data);