Add a disable flag for sensors.

PiperOrigin-RevId: 469473066
Change-Id: I296ed790c6261c4fb15b349540edcb77e53e0567

doc/APIreference.rst

@@ -86,8 +86,9 @@ mjtDisableBit
        mjDSBL_FILTERPARENT = 1<<9,     // remove collisions with parent body
        mjDSBL_ACTUATION    = 1<<10,    // apply actuation forces
        mjDSBL_REFSAFE      = 1<<11,    // integrator safety: make ref[0]>=2*timestep
+       mjDSBL_SENSOR       = 1<<12,    // sensors
 
-       mjNDISABLE          = 12        // number of disable flags
+       mjNDISABLE          = 13        // number of disable flags
    } mjtDisableBit;
 
 | Defined in `mjmodel.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjmodel.h>`_

doc/XMLreference.rst

@@ -94,9 +94,9 @@ in the second column of the table have the following meaning:
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
 |                          |    |    | :at:`filterparent`      | :at:`actuation`         | :at:`refsafe`           | |
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
-|                          |    |    | :at:`override`          | :at:`energy`            | :at:`fwdinv`            | |
+|                          |    |    | :at:`sensor`            | :at:`override`          | :at:`energy`            | |
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
-|                          |    |    | :at:`sensornoise`       | :at:`multiccd`          |                         | |
+|                          |    |    | :at:`fwdinv`            | :at:`sensornoise`       | :at:`multiccd`          | |
 |                          |    |    +-------------------------+-------------------------+-------------------------+ |
 +--------------------------+----+------------------------------------------------------------------------------------+
 | |_|:el:`size`            | \* | .. table::                                                                         |
@@ -1847,6 +1847,9 @@ from its default.
    simulation timestep. Recall that solref[0] is the stiffness of the virtual spring-damper used for constraint
    stabilization. If this setting is enabled, the solver uses max(solref[0], 2*timestep) in place of solref[0]
    separately for each active constraint.
+:at:`sensor`: :at-val:`[disable, enable], "enable"`
+   This flag disables all computations related to sensors. When disabled, sensor values will remain constant, either
+   zeros if disabled at the start of simulation, or, if disabled at runtime, whatever value was last computed.
 :at:`override`: :at-val:`[disable, enable], "disable"`
    This flag enables to :ref:`Contact override <COverride>` mechanism explained above.
 :at:`energy`: :at-val:`[disable, enable], "disable"`

doc/changelog.rst

@@ -40,6 +40,7 @@ General
   the free camera at model load time.
 - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes.
 - ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``.
+- Added a disable flag for sensors.
 
 Deleted/deprecated features
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^

include/mujoco/mjmodel.h

@@ -58,8 +58,9 @@ typedef enum mjtDisableBit_ {     // disable default feature bitflags
   mjDSBL_FILTERPARENT = 1<<9,     // remove collisions with parent body
   mjDSBL_ACTUATION    = 1<<10,    // apply actuation forces
   mjDSBL_REFSAFE      = 1<<11,    // integrator safety: make ref[0]>=2*timestep
+  mjDSBL_SENSOR       = 1<<12,    // sensors
 
-  mjNDISABLE          = 12        // number of disable flags
+  mjNDISABLE          = 13        // number of disable flags
 } mjtDisableBit;
 
 

introspect/enums.py

@@ -39,7 +39,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjDSBL_FILTERPARENT', 512),
              ('mjDSBL_ACTUATION', 1024),
              ('mjDSBL_REFSAFE', 2048),
-             ('mjNDISABLE', 12),
+             ('mjDSBL_SENSOR', 4096),
+             ('mjNDISABLE', 13),
          ]),
      )),
     ('mjtEnableBit',

src/engine/engine_sensor.c

@@ -193,6 +193,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
   int ne = d->ne, nf = d->nf, nefc = d->nefc;
   mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;
 
+  // disabled sensors: return
+  if (mjDISABLED(mjDSBL_SENSOR)) {
+    return;
+  }
+
   // process sensors matching stage
   for (int i=0; i<m->nsensor; i++) {
     if (m->sensor_needstage[i]==mjSTAGE_POS) {
@@ -349,6 +354,11 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
   int ne = d->ne, nf = d->nf, nefc = d->nefc;
   mjtNum xvel[6];
 
+  // disabled sensors: return
+  if (mjDISABLED(mjDSBL_SENSOR)) {
+    return;
+  }
+
   // process sensors matching stage
   int subtreeVel = 0;
   for (int i=0; i<m->nsensor; i++) {
@@ -502,6 +512,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
   mjtNum tmp[6], conforce[6], conray[3];
   mjContact* con;
 
+  // disabled sensors: return
+  if (mjDISABLED(mjDSBL_SENSOR)) {
+    return;
+  }
+
   // process sensors matching stage
   int rnePost = 0;
   for (int i=0; i<m->nsensor; i++) {

src/engine/engine_support.c

@@ -51,7 +51,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
   "Warmstart",
   "Filterparent",
   "Actuation",
-  "Refsafe"
+  "Refsafe",
+  "Sensor"
 };
 
 

src/xml/xml_native_reader.cc

@@ -63,9 +63,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         "integrator", "collision", "cone", "jacobian",
         "solver", "iterations", "noslip_iterations", "mpr_iterations"},
     {"<"},
-        {"flag", "?", "17", "constraint", "equality", "frictionloss", "limit", "contact",
+        {"flag", "?", "18", "constraint", "equality", "frictionloss", "limit", "contact",
             "passive", "gravity", "clampctrl", "warmstart",
-            "filterparent", "actuation", "refsafe",
+            "filterparent", "actuation", "refsafe", "sensor",
             "override", "energy", "fwdinv", "sensornoise", "multiccd"},
     {">"},
 
@@ -887,6 +887,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
     READDSBL("filterparent", mjDSBL_FILTERPARENT)
     READDSBL("actuation",    mjDSBL_ACTUATION)
     READDSBL("refsafe",      mjDSBL_REFSAFE)
+    READDSBL("sensor",       mjDSBL_SENSOR)
 #undef READDSBL
 
 #define READENBL(NAME, MASK) \

src/xml/xml_native_writer.cc

@@ -752,6 +752,7 @@ void mjXWriter::Option(XMLElement* root) {
     WRITEDSBL("filterparent",   mjDSBL_FILTERPARENT)
     WRITEDSBL("actuation",      mjDSBL_ACTUATION)
     WRITEDSBL("refsafe",        mjDSBL_REFSAFE)
+    WRITEDSBL("sensor",         mjDSBL_SENSOR)
 #undef WRITEDSBL
 
 #define WRITEENBL(NAME, MASK) \

test/engine/engine_sensor_test.cc

@@ -32,21 +32,65 @@
 namespace mujoco {
 namespace {
 
+const mjtNum tol = 1e-14;  // nearness tolerance for floating point numbers
+
 // returns as a vector the measured values from sensor with index `id`
-static std::vector<mjtNum> GetSensor(const mjModel* model, const mjData* data, int id) {
+static std::vector<mjtNum> GetSensor(const mjModel* model,
-  return std::vector<mjtNum>(data->sensordata + model->sensor_adr[id],
+                                     const mjData* data,
-                             data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
+                                     int id) {
+  return std::vector<mjtNum>(
+      data->sensordata + model->sensor_adr[id],
+      data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
 }
 
-
-// --------------------- test relative frame sensors  --------------------------
-
 using ::testing::Pointwise;
 using ::testing::DoubleNear;
 using ::testing::StrEq;
-using RelativeFrameSensorTest = MujocoTest;
 
-const mjtNum tol = 1e-14;  // nearness tolerance for floating point numbers
+using SensorTest = MujocoTest;
+
+// --------------------- test sensor disableflag  -----------------------------
+
+// hand-picked positions and orientations for simple expected values
+TEST_F(SensorTest, DisableSensors) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <sensor>
+      <clock/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml, 0, 0);
+  mjData* data = mj_makeData(model);
+
+  // before calling anything, check that sensors are initialised to 0
+  EXPECT_EQ(data->sensordata[0], 0.0);
+
+  // call mj_step, mj_step1, expect clock to be incremented by timestep
+  mj_step(model, data);
+  mj_step1(model, data);
+  EXPECT_EQ(data->sensordata[0], model->opt.timestep);
+
+  // disable sensors, call mj_step, mj_step1, expect clock to not increment
+  model->opt.disableflags |= mjDSBL_SENSOR;
+  mj_step(model, data);
+  mj_step1(model, data);
+  EXPECT_EQ(data->time, 2*model->opt.timestep);
+  EXPECT_EQ(data->sensordata[0], model->opt.timestep);
+
+  // re-enable sensors, call mj_step, mj_step1, expect clock to match time
+  model->opt.disableflags = 0;
+  mj_step(model, data);
+  mj_step1(model, data);
+  EXPECT_EQ(data->time, data->sensordata[0]);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+// --------------------- test relative frame sensors  --------------------------
+
+using RelativeFrameSensorTest = MujocoTest;
 
 // hand-picked positions and orientations for simple expected values
 TEST_F(RelativeFrameSensorTest, ReferencePosMat) {
@@ -161,7 +205,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
   std::vector expected_values(data->sensordata, data->sensordata+nsensordata/2);
 
   // set qpos to arbitrary values, call mj_forward
-  for (int i=0; i<7; i++) {
+  for (int i=0; i < 7; i++) {
     data->qpos[i] = i+1;
   }
   mj_forward(model, data);

test/xml/xml_native_writer_test.cc

@@ -52,6 +52,20 @@ using ::testing::NotNull;
 
 using XMLWriterTest = MujocoTest;
 
+TEST_F(XMLWriterTest, SavesDisableSensor) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag sensor="disable"/>
+    </option>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  std::string saved_xml = SaveAndReadXml(model);
+  EXPECT_THAT(saved_xml, HasSubstr("sensor=\"disable\""));
+  mj_deleteModel(model);
+}
+
 TEST_F(XMLWriterTest, KeepsEmptyClasses) {
   static constexpr char xml[] = R"(
   <mujoco>