Merge pull request #3134 from kevinzakka:sensoracc-rnepost

PiperOrigin-RevId: 875703097 Change-Id: I564c1afa1cc1f88a712984c6714611056f6e26eb
2026-02-26 06:58:48 -08:00
parent 137a504625 43cde9716c
commit 19b583a421
2 changed files with 36 additions and 0 deletions
@@ -1530,6 +1530,7 @@ void mj_step2(const mjModel* m, mjData* d) {
  mj_fwdActuation(m, d);
  mj_fwdAcceleration(m, d);
  mj_fwdConstraint(m, d);
+  d->flg_rnepost = 0;  // clear flag for lazy evaluation
  mj_sensorAcc(m, d);
  mj_checkAcc(m, d);

@@ -1591,5 +1591,40 @@ TEST_F(SensorTest, ReadSensor) {
  mj_deleteModel(model);
 }

+// mj_sensorAcc returns correct accelerometer after mj_step1 (issue #3133)
+TEST_F(SensorTest, AccelerometerAfterStep1) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom type="plane" size="10 10 0.1"/>
+      <body pos="0 0 0.1">
+        <freejoint/>
+        <geom type="box" size="0.1 0.1 0.1" mass="10"/>
+        <site name="imu"/>
+      </body>
+    </worldbody>
+    <sensor>
+      <accelerometer site="imu"/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+
+  // settle the simulation with split-step loop
+  for (int i = 0; i < 100; i++) {
+    mj_step1(model, data);
+    mj_step2(model, data);
+  }
+
+  // call mj_step1 + mj_sensorAcc, expect gravity reading (~9.81 m/s^2)
+  mj_step1(model, data);
+  mj_sensorAcc(model, data);
+  EXPECT_NEAR(data->sensordata[2], 9.81, 1e-2);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco