Merge pull request #3134 from kevinzakka:sensoracc-rnepost

PiperOrigin-RevId: 875703097
Change-Id: I564c1afa1cc1f88a712984c6714611056f6e26eb

src/engine/engine_forward.c

@@ -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);
 

test/engine/engine_sensor_test.cc

@@ -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