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

Add test for armature, model will be used in upcoming improved documentation for armature.

Browse Source 
PiperOrigin-RevId: 719916567
Change-Id: I7f74bd3c3864f43f5f68d458d5b128cfee1d7e6c
This commit is contained in:
Yuval Tassa
2025-01-26 11:59:53 -08:00
committed by Copybara-Service
parent 2c3d0becb2
commit 08edde5fd8
2 changed files with 75 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_forward_test.cc
+41
View File
@@ -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;
test/engine/testdata/armature_equivalence.xml
Vendored
+34
View File
@@ -0,0 +1,34 @@
<mujoco>
<worldbody>
<body name="link1">
<!-- inertia of link1 is 1.0 -->
<joint name="link1" axis="0 -1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 1 0 0" mass="0"/>
<geom type="sphere" size=".1" mass="1" pos="1 0 0"/>
</body>
<body name="motor1">
<!-- inertia of motor1 is 1/12 * m * (0.2^2 + 0.2^2) = 0.2
see https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
<joint name="motor1" axis="0 -1 0"/>
<geom type="box" size=".1 .1 .1" mass="30" contype="0" conaffinity="0"/>
</body>
<body name="link2" pos="1.5 0 0">
<!-- armature from reflected inertia of motor1 is 0.2 * 3^2 = 1.8 -->
<joint name="link2" armature="1.8" axis="0 -1 0"/>
<geom type="capsule" size=".02" fromto="0 0 0 1 0 0" mass="0"/>
<geom type="sphere" size=".1" mass="1" pos="1 0 0"/>
</body>
</worldbody>
<equality>
<!-- gear ratio between motor1 and link1 is 3:1 -->
<joint joint1="motor1" joint2="link1" polycoef="0 3"/>
</equality>
<actuator>
<position name="link1" joint="motor1" kp="1" kv=".1" ctrlrange="-5 5"/>
<!-- actuators are made equivalent by setting link2 gear to link1 gear ratio -->
<position name="link2" joint="link2" kp="1" kv=".1" ctrlrange="-5 5" gear="3"/>
</actuator>
</mujoco>