Fix bug in simulate noise injection logic.

Noise injection was occurring at every iteration of the physics thread, rather than before every call to `mj_step`, leading to time-step-dependent noise rate.

- Moved the logic from main.cc to a `Simulate::` method, called before `mj_step`.
- Reduced the maximum time-scale slider value back to 4 seconds (recent increase was due to this bug).

PiperOrigin-RevId: 643350296
Change-Id: I96832905e6614f92ac725298b16fcabe64ea1df1

simulate/main.cc

@@ -329,34 +329,6 @@ void PhysicsLoop(mj::Simulate& sim) {
           const auto elapsedCPU = startCPU - syncCPU;
           double elapsedSim = d->time - syncSim;
 
-          // inject noise
-          if (sim.ctrl_noise_std > 0) {
-            // convert rate and scale to discrete time (Ornstein–Uhlenbeck)
-            mjtNum rate = mju_exp(-m->opt.timestep / sim.ctrl_noise_rate);
-            mjtNum scale = sim.ctrl_noise_std * mju_sqrt(1-rate*rate);
-
-            for (int i=0; i<m->nu; i++) {
-              mjtNum bottom = 0, top = 0, midpoint = 0, halfrange = 1;
-              if (m->actuator_ctrllimited[i]) {
-                bottom = m->actuator_ctrlrange[2*i];
-                top = m->actuator_ctrlrange[2*i+1];
-                midpoint =  0.5 * (top + bottom);  // target of exponential decay
-                halfrange = 0.5 * (top - bottom);  // scales noise
-              }
-
-              // exponential convergence to midpoint at ctrl_noise_rate
-              d->ctrl[i] = rate * d->ctrl[i] + (1-rate) * midpoint;
-
-              // add noise
-              d->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr);
-
-              // clip to range
-              if (m->actuator_ctrllimited[i]) {
-                d->ctrl[i] = mju_clip(d->ctrl[i], bottom, top);
-              }
-            }
-          }
-
           // requested slow-down factor
           double slowdown = 100 / sim.percentRealTime[sim.real_time_index];
 
@@ -394,6 +366,9 @@ void PhysicsLoop(mj::Simulate& sim) {
                 measured = true;
               }
 
+              // inject noise
+              sim.InjectNoise();
+
               // call mj_step
               mj_step(m, d);
               stepped = true;

simulate/simulate.cc

@@ -2741,6 +2741,39 @@ void Simulate::AddToHistory() {
   mj_getState(m_, d_, state, mjSTATE_INTEGRATION);
 }
 
+// inject Brownian noise
+void Simulate::InjectNoise() {
+  // no noise, return
+  if (ctrl_noise_std <= 0) {
+    return;
+  }
+
+  // convert rate and scale to discrete time (Ornstein–Uhlenbeck)
+  mjtNum rate = mju_exp(-m_->opt.timestep / ctrl_noise_rate);
+  mjtNum scale = ctrl_noise_std * mju_sqrt(1-rate*rate);
+
+  for (int i=0; i<m_->nu; i++) {
+    mjtNum bottom = 0, top = 0, midpoint = 0, halfrange = 1;
+    if (m_->actuator_ctrllimited[i]) {
+      bottom = m_->actuator_ctrlrange[2*i];
+      top = m_->actuator_ctrlrange[2*i+1];
+      midpoint =  0.5 * (top + bottom);  // target of exponential decay
+      halfrange = 0.5 * (top - bottom);  // scales noise
+    }
+
+    // exponential convergence to midpoint at ctrl_noise_rate
+    d_->ctrl[i] = rate * d_->ctrl[i] + (1-rate) * midpoint;
+
+    // add noise
+    d_->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr);
+
+    // clip to range if limited
+    if (m_->actuator_ctrllimited[i]) {
+      d_->ctrl[i] = mju_clip(d_->ctrl[i], bottom, top);
+    }
+  }
+}
+
 void Simulate::UpdateHField(int hfieldid) {
   MutexLock lock(this->mtx);
   if (!m_ || hfieldid < 0 || hfieldid >= m_->nhfield) {

simulate/simulate.h

@@ -86,6 +86,9 @@ class Simulate {
   // add state to history buffer
   void AddToHistory();
 
+  // inject control noise
+  void InjectNoise();
+
   // constants
   static constexpr int kMaxFilenameLength = 1000;
 
@@ -294,7 +297,7 @@ class Simulate {
     {mjITEM_BUTTON,    "Load key",      3},
     {mjITEM_BUTTON,    "Save key",      3},
     {mjITEM_SLIDERNUM, "Noise scale",   5, &this->ctrl_noise_std,  "0 1"},
-    {mjITEM_SLIDERNUM, "Noise rate",    5, &this->ctrl_noise_rate, "0 20"},
+    {mjITEM_SLIDERNUM, "Noise rate",    5, &this->ctrl_noise_rate, "0 4"},
     {mjITEM_SEPARATOR, "History",       1},
     {mjITEM_SLIDERINT, "",              5, &this->scrub_index,     "0 0"},
     {mjITEM_END}