diff --git a/simulate/main.cc b/simulate/main.cc index 035c03ad..30139f8f 100644 --- a/simulate/main.cc +++ b/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; inu; 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; diff --git a/simulate/simulate.cc b/simulate/simulate.cc index ecf931a1..a86c7343 100644 --- a/simulate/simulate.cc +++ b/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; inu; 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) { diff --git a/simulate/simulate.h b/simulate/simulate.h index 7d9e2657..5c385a74 100644 --- a/simulate/simulate.h +++ b/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}