diff --git a/simulate/main.cc b/simulate/main.cc index 209be7e8..c2d72d95 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -381,7 +381,7 @@ void PhysicsLoop(mj::Simulate& sim) { sim.speed_changed = false; // inject noise - sim.InjectNoise(); + sim.InjectNoise(sim.key); // run single step, let next iteration deal with timing mj_step(m, d); @@ -412,7 +412,7 @@ void PhysicsLoop(mj::Simulate& sim) { } // inject noise - sim.InjectNoise(); + sim.InjectNoise(sim.key); // call mj_step mj_step(m, d); diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 555ab68c..3fb21976 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -2917,7 +2917,7 @@ void Simulate::AddToHistory() { } // inject Brownian noise -void Simulate::InjectNoise() { +void Simulate::InjectNoise(int key) { // no noise, return if (ctrl_noise_std <= 0) { return; @@ -2936,6 +2936,11 @@ void Simulate::InjectNoise() { halfrange = 0.5 * (top - bottom); // scales noise } + // overwrite midpoint with keyframe, if given + if (key >= 0) { + midpoint = m_->key_ctrl[key*m_->nu+i]; + } + // exponential convergence to midpoint at ctrl_noise_rate d_->ctrl[i] = rate * d_->ctrl[i] + (1-rate) * midpoint; diff --git a/simulate/simulate.h b/simulate/simulate.h index c1fd960e..567a1226 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -89,7 +89,7 @@ class Simulate { void AddToHistory(); // inject control noise - void InjectNoise(); + void InjectNoise(int key); // constants static constexpr int kMaxFilenameLength = 1000; @@ -199,7 +199,7 @@ class Simulate { int busywait = 0; // keyframe index - int key = 0; + int key = -1; // index of history-scrubber slider int scrub_index = 0;