Don't compute sensors in mjd_transitionFD, if not requested.

PiperOrigin-RevId: 507878658 Change-Id: I97ece521fd177d97fa5758dda1eb012ba26bcabe
2023-02-07 14:01:43 -08:00
parent 15b6e6f060
commit 48e95817e4
2 changed files with 41 additions and 1 deletions
@@ -1510,7 +1510,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte centered,
  }

  // step input
-  mj_step(m, d);
+  mj_stepSkip(m, d, mjSTAGE_NONE, skipsensor);

  // save output
  getState(m, d, next, sensor);
@@ -504,6 +504,46 @@ TEST_F(DerivativeTest, SensorDerivatives) {
  mj_deleteModel(model);
 }

+// if sensor derivatives aren't requested, don't compute sensors
+TEST_F(DerivativeTest, SensorSkip) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint name="joint" type="slide"/>
+        <geom size=".1"/>
+      </body>
+    </worldbody>
+
+    <actuator>
+      <general name="actuator" joint="joint" gainprm="3"/>
+    </actuator>
+
+    <sensor>
+      <jointpos joint="joint"/>
+    </sensor>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  int nv = model->nv, nu = model->nu;
+  mjData* data = mj_makeData(model);
+
+  // set a sentinel value in the sensor
+  data->sensordata[0] = 1337;
+
+  // finite differenced B
+  mjtNum eps = 1e-6;
+  mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu);
+  mjd_transitionFD(model, data, eps, /*centered=*/0,
+                   nullptr, BFD, nullptr, nullptr);
+
+  EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed";
+
+  mju_free(BFD);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}

 // derivatives don't mutate the state
 TEST_F(DerivativeTest, NoStateMutation) {