diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 9c930bc4..16a52f43 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -162,6 +162,47 @@ TEST_F(ForwardTest, DamperDampens) { mj_deleteModel(model); } +static const char* const kArmatureEquivalencePath = + "engine/testdata/armature_equivalence.xml"; + +// test that adding joint armature is equivalent to a coupled rotating mass with +// a gear ratio enforced by an equality +TEST_F(ForwardTest, ArmatureEquivalence) { + const std::string xml_path = GetTestDataFilePath(kArmatureEquivalencePath); + char error[1000]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // with actuators + mjtNum qpos_mse = 0; + int nstep = 0; + while (data->time < 4) { + data->ctrl[0] = data->ctrl[1] = mju_sin(2*data->time); + mj_step(model, data); + nstep++; + mjtNum err = data->qpos[0] - data->qpos[2]; + qpos_mse += err * err; + } + EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + + // no actuators + model->opt.disableflags |= mjDSBL_ACTUATION; + qpos_mse = 0; + nstep = 0; + mj_resetData(model, data); + while (data->time < 4) { + mj_step(model, data); + nstep++; + mjtNum err = data->qpos[0] - data->qpos[2]; + qpos_mse += err * err; + } + EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + + mj_deleteData(data); + mj_deleteModel(model); +} + // --------------------------- implicit integrator ----------------------------- using ImplicitIntegratorTest = MujocoTest; diff --git a/test/engine/testdata/armature_equivalence.xml b/test/engine/testdata/armature_equivalence.xml new file mode 100644 index 00000000..78f16e02 --- /dev/null +++ b/test/engine/testdata/armature_equivalence.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +