From 48e95817e47bad95faad1973caa27ad62c847c80 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 7 Feb 2023 14:01:43 -0800 Subject: [PATCH] Don't compute sensors in mjd_transitionFD, if not requested. PiperOrigin-RevId: 507878658 Change-Id: I97ece521fd177d97fa5758dda1eb012ba26bcabe --- src/engine/engine_derivative.c | 2 +- test/engine/engine_derivative_test.cc | 40 +++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 9da2f846..082de42c 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 2aaf79e1..476a344d 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -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"( + + + + + + + + + + + + + + + + + )"; + + 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) {