Remove native sensor noise sampling.

PiperOrigin-RevId: 620075316
Change-Id: I6a0a1d63834e7c8bfd290f89d86fea791d794739

src/engine/engine_sensor.c

@@ -32,78 +32,9 @@
 #include "engine/engine_util_spatial.h"
 
 
+
 //-------------------------------- utility ---------------------------------------------------------
 
-// add sensor noise after each stage
-static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
-  int adr, dim;
-  mjtNum rnd[4], noise, quat[4], res[4];
-
-  // process sensors matching stage and having positive noise
-  for (int i=0; i < m->nsensor; i++) {
-    if (m->sensor_needstage[i] == stage && m->sensor_noise[i] > 0) {
-      // get sensor info
-      adr = m->sensor_adr[i];
-      dim = m->sensor_dim[i];
-      noise = m->sensor_noise[i];
-
-      // real or positive: add noise directly, with clamp for positive
-      if (m->sensor_datatype[i] == mjDATATYPE_REAL ||
-          m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
-        for (int j=0; j < dim; j++) {
-          // get random numbers; use only the first one
-          rnd[0] = mju_standardNormal(rnd+1);
-
-          // positive
-          if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
-            // add noise only if positive, keep it positive
-            if (d->sensordata[adr+j] > 0) {
-              d->sensordata[adr+j] = mju_max(0, d->sensordata[adr+j]+rnd[0]*noise);
-            }
-          }
-
-          // real
-          else {
-            d->sensordata[adr+j] += rnd[0]*noise;
-          }
-        }
-      }
-
-      // axis or quat: rotate around random axis by random angle
-      else {
-        // get four random numbers
-        rnd[0] = mju_standardNormal(rnd+1);
-        rnd[2] = mju_standardNormal(rnd+3);
-
-        // scale angle, normalize axis, make quaternion
-        rnd[0] *= noise;
-        mju_normalize3(rnd+1);
-        mju_axisAngle2Quat(quat, rnd+1, rnd[0]);
-
-        // axis
-        if (m->sensor_datatype[i] == mjDATATYPE_AXIS) {
-          // apply quaternion rotation to axis, assign
-          mju_rotVecQuat(res, d->sensordata+adr, quat);
-          mju_copy3(d->sensordata+adr, res);
-        }
-
-        // quaternion
-        else if (m->sensor_datatype[i] == mjDATATYPE_QUATERNION) {
-          // apply quaternion rotation to quaternion, assign
-          mju_mulQuat(d->sensordata+adr, d->sensordata+adr, quat);
-        }
-
-        // unknown datatype
-        else {
-          mjERROR("unknown datatype in sensor %d", i);
-        }
-      }
-    }
-  }
-}
-
-
-
 // apply cutoff after each stage
 static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
   // process sensors matching stage and having positive cutoff
@@ -442,11 +373,6 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
     mjcb_sensor(m, d, mjSTAGE_POS);
   }
 
-  // add noise if enabled
-  if (mjENABLED(mjENBL_SENSORNOISE)) {
-    add_noise(m, d, mjSTAGE_POS);
-  }
-
   // compute plugin sensor values
   if (m->nplugin) {
     const int nslot = mjp_pluginCount();
@@ -623,11 +549,6 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
     mjcb_sensor(m, d, mjSTAGE_VEL);
   }
 
-  // add noise if enabled
-  if (mjENABLED(mjENBL_SENSORNOISE)) {
-    add_noise(m, d, mjSTAGE_VEL);
-  }
-
   // trigger computation of plugins
   if (m->nplugin) {
     const int nslot = mjp_pluginCount();
@@ -839,11 +760,6 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
     mjcb_sensor(m, d, mjSTAGE_ACC);
   }
 
-  // add noise if enabled
-  if (mjENABLED(mjENBL_SENSORNOISE)) {
-    add_noise(m, d, mjSTAGE_ACC);
-  }
-
   // trigger computation of plugins
   if (m->nplugin) {
     const int nslot = mjp_pluginCount();

src/engine/engine_support.c

@@ -67,7 +67,6 @@ const char* mjENABLESTRING[mjNENABLE] = {
   "Energy",
   "Fwdinv",
   "InvDiscrete",
-  "Sensornoise",
   "MultiCCD",
   "Island"
 };

src/xml/xml_native_reader.cc

@@ -101,10 +101,10 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
         "solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
         "sdf_iterations", "sdf_initpoints", "actuatorgroupdisable"},
     {"<"},
-        {"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact",
+        {"flag", "?", "21", "constraint", "equality", "frictionloss", "limit", "contact",
             "passive", "gravity", "clampctrl", "warmstart",
             "filterparent", "actuation", "refsafe", "sensor", "midphase", "eulerdamp",
-            "override", "energy", "fwdinv", "invdiscrete", "sensornoise", "multiccd", "island"},
+            "override", "energy", "fwdinv", "invdiscrete", "multiccd", "island"},
     {">"},
 
     {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey",
@@ -1091,7 +1091,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
     READENBL("energy",      mjENBL_ENERGY)
     READENBL("fwdinv",      mjENBL_FWDINV)
     READENBL("invdiscrete", mjENBL_INVDISCRETE)
-    READENBL("sensornoise", mjENBL_SENSORNOISE)
     READENBL("multiccd",    mjENBL_MULTICCD)
     READENBL("island",      mjENBL_ISLAND)
 #undef READENBL

src/xml/xml_native_writer.cc

@@ -938,7 +938,6 @@ void mjXWriter::Option(XMLElement* root) {
     WRITEENBL("energy",         mjENBL_ENERGY)
     WRITEENBL("fwdinv",         mjENBL_FWDINV)
     WRITEENBL("invdiscrete",    mjENBL_INVDISCRETE)
-    WRITEENBL("sensornoise",    mjENBL_SENSORNOISE)
     WRITEENBL("multiccd",       mjENBL_MULTICCD)
     WRITEENBL("island",         mjENBL_ISLAND)
 #undef WRITEENBL