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

yuvaltassa · copybara-github · commit 08edde5fd867 · 2025-01-26T12:01:12.000-08:00
PiperOrigin-RevId: 719916567
Change-Id: I7f74bd3c3864f43f5f68d458d5b128cfee1d7e6c
diff --git 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
@@ -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>
