From 834e8dd50601eba690a9ae82cd214a1b35e2da81 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 1 Sep 2022 09:09:19 -0700 Subject: [PATCH] Improvements to time synchronisation in `simulate`. - Improved time-sync logic readability in `PhysicsLoop`. - Fixed unattainable condition for breaking from stepping loop if data was reset. - Added measurement of actual real-time tracking. - Moved slowdown overlay to top left. - Report slowdown as % of real-time, rather than fraction. - Report actual slowdown if different than requested (very small timestep, PhysicsLoop cannot keep up) by more than 10%. - Replaced slowdown increments of 2 with decimal-rounded increments of 10^(1/10). This is a finer-grained scale (approximately 3x as fine) and well suited for decimal percentage representation. - Maximum slowdown increased to 1000. - Added `Simulate.refreshrate` with a default of 60Hz, in case videomode refreshrate is 0 (can occur e.g. when forwarding over X11). - Remove realtime reporting from regular info overlay. - Various cleanups. PiperOrigin-RevId: 471547481 Change-Id: Idd2514a4defb1dc431617f3a515cc747c4fd8971 --- doc/changelog.rst | 3 ++ simulate/main.cc | 66 ++++++++++++++++++++++++++++---------------- simulate/simulate.cc | 45 +++++++++++++++++++++--------- simulate/simulate.h | 42 ++++++++++++++++++++-------- 4 files changed, 107 insertions(+), 49 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 77d7706b..90e4d246 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -48,9 +48,12 @@ 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``. +- Improvements to time synchronisation in `simulate`, in particular report actual real-time factor if different from + requested factor. - Added a disable flag for sensors. - :ref:`mju_mulQuat` and :ref:`mju_mulQuatAxis` support in place computation. For example |br| ``mju_mulQuat(a, a, b);`` sets the quaternion ``a`` equal to the product of ``a`` and ``b``. + Deleted/deprecated features ^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/simulate/main.cc b/simulate/main.cc index 44dedcbb..fda110e8 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -22,7 +22,6 @@ #include #include -#include #include "glfw_dispatch.h" #include "simulate.h" #include "array_safety.h" @@ -34,9 +33,9 @@ namespace mju = ::mujoco::sample_util; using ::mujoco::Glfw; // constants -const double syncmisalign = 0.1; // maximum time mis-alignment before re-sync -const double refreshfactor = 0.7; // fraction of refresh available for simulation -const int kErrorLength = 1024; +const double syncMisalign = 0.1; // maximum mis-alignment before re-sync (simulation seconds) +const double simRefreshFraction = 0.7; // fraction of refresh available for simulation +const int kErrorLength = 1024; // load error string length // model and data mjModel* m = nullptr; @@ -101,8 +100,8 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim) { // simulate in background thread (while rendering in main thread) void PhysicsLoop(mj::Simulate& sim) { // cpu-sim syncronization point - double cpusync = 0; - mjtNum simsync = 0; + double syncCPU = 0; + mjtNum syncSim = 0; // run until asked to exit while (!sim.exitrequest.load()) { @@ -154,6 +153,7 @@ void PhysicsLoop(mj::Simulate& sim) { } { + // lock the sim mutex const std::lock_guard lock(sim.mtx); // run only if model is present @@ -161,30 +161,39 @@ void PhysicsLoop(mj::Simulate& sim) { // running if (sim.run) { // record cpu time at start of iteration - double tmstart = Glfw().glfwGetTime(); + double startCPU = Glfw().glfwGetTime(); + + // elapsed CPU and simulation time since last sync + double elapsedCPU = startCPU - syncCPU; + double elapsedSim = d->time - syncSim; // inject noise if (sim.ctrlnoisestd) { - // convert rate and scale to discrete time given current timestep + // convert rate and scale to discrete time (Ornstein–Uhlenbeck) mjtNum rate = mju_exp(-m->opt.timestep / sim.ctrlnoiserate); mjtNum scale = sim.ctrlnoisestd * mju_sqrt(1-rate*rate); for (int i=0; inu; i++) { // update noise ctrlnoise[i] = rate * ctrlnoise[i] + scale * mju_standardNormal(nullptr); + // apply noise d->ctrl[i] = ctrlnoise[i]; } } - // out-of-sync (for any reason) - mjtNum offset = mju_abs((d->time*sim.slow_down-simsync)-(tmstart-cpusync)); - if (d->time*sim.slow_down syncmisalign*sim.slow_down || sim.speed_changed) { + // requested slow-down factor + double slowdown = 100 / sim.percentRealTime[sim.realTimeIndex]; + + // misalignment condition: distance from target sim time is bigger than syncmisalign + bool misaligned = mju_abs(elapsedCPU/slowdown - elapsedSim) > syncMisalign; + + // out-of-sync (for any reason): reset sync times, step + if (elapsedSim < 0 || elapsedCPU < 0 || syncCPU == 0 || misaligned || sim.speedChanged) { // re-sync - cpusync = tmstart; - simsync = d->time*sim.slow_down; - sim.speed_changed = false; + syncCPU = startCPU; + syncSim = d->time; + sim.speedChanged = false; // clear old perturbations, apply new mju_zero(d->xfrc_applied, 6*m->nbody); @@ -195,22 +204,31 @@ void PhysicsLoop(mj::Simulate& sim) { mj_step(m, d); } - // in-sync + // in-sync: step until ahead of cpu else { - // step while simtime lags behind cputime, and within safefactor - while ((d->time*sim.slow_down-simsync) < (Glfw().glfwGetTime()-cpusync) && - (Glfw().glfwGetTime()-tmstart) < refreshfactor/sim.vmode.refreshRate) { + bool measured = false; + mjtNum prevSim = d->time; + double refreshTime = simRefreshFraction/sim.refreshRate; + + // step while sim lags behind cpu and within refreshTime + while ((d->time - syncSim)*slowdown < (Glfw().glfwGetTime()-syncCPU) && + (Glfw().glfwGetTime()-startCPU) < refreshTime) { + // measure slowdown before first step + if (!measured && elapsedSim) { + sim.measuredSlowdown = elapsedCPU / elapsedSim; + measured = true; + } + // clear old perturbations, apply new mju_zero(d->xfrc_applied, 6*m->nbody); sim.applyposepertubations(0); // move mocap bodies only sim.applyforceperturbations(); - // run mj_step - mjtNum prevtm = d->time*sim.slow_down; + // call mj_step mj_step(m, d); - // break on reset - if (d->time*sim.slow_downtime < prevSim) { break; } } @@ -226,7 +244,7 @@ void PhysicsLoop(mj::Simulate& sim) { mj_forward(m, d); } } - } // std::lock_guard + } // release std::lock_guard } } } // namespace diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 9db1da17..bb826c05 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -54,7 +54,6 @@ using ::mujoco::Glfw; //------------------------------------------- global ----------------------------------------------- const int maxgeom = 5000; // preallocated geom array in mjvScene -const int max_slow_down = 128; // maximum slow-down quotient const double zoom_increment = 0.02; // ratio of one click-wheel zoom increment to vertical extent // section ids @@ -476,11 +475,10 @@ void infotext(mj::Simulate* sim, solerr = mju_log10(mju_max(mjMINVAL, solerr)); // prepare info text - const std::string realtime_nominator = sim->slow_down == 1 ? "" : "1/"; mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nstack\nconbuf\nefcbuf"); mju::sprintf_arr(content, - "%-9.3f %s%d x\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.3f\n%.3f\n%.3f", - d->time, realtime_nominator.c_str(), sim->slow_down, + "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.3f\n%.3f\n%.3f", + d->time, d->nefc, d->ncon, sim->run ? d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) : @@ -1382,16 +1380,19 @@ void uiEvent(mjuiState* state) { break; case '-': // slow down - if (sim->slow_down < max_slow_down && !state->shift) { - sim->slow_down *= 2; - sim->speed_changed = true; + { + int numclicks = sizeof(sim->percentRealTime) / sizeof(sim->percentRealTime[0]); + if (sim->realTimeIndex < numclicks-1 && !state->shift) { + sim->realTimeIndex++; + sim->speedChanged = true; + } } break; case '=': // speed up - if (sim->slow_down > 1 && !state->shift) { - sim->slow_down /= 2; - sim->speed_changed = true; + if (sim->realTimeIndex > 0 && !state->shift) { + sim->realTimeIndex--; + sim->speedChanged = true; } break; } @@ -1759,9 +1760,24 @@ void Simulate::render() { } // show realtime label - if (this->run && this->slow_down != 1) { - std::string realtime_label = "1/" + std::to_string(this->slow_down) + " x"; - mjr_overlay(mjFONT_BIG, mjGRID_TOPRIGHT, smallrect, realtime_label.c_str(), nullptr, &this->con); + if (this->run) { + // get desired and actual percent-of-real-time + float desiredRealtime = this->percentRealTime[this->realTimeIndex]; + float actualRealtime = 100 / this->measuredSlowdown; + + // check if real-time tracking is misaligned by more than than 10% + bool misalignment = mju_abs(actualRealtime - desiredRealtime) > 0.1 * desiredRealtime; + + // display realtime overlay if not 100% or there is misalignment + if (desiredRealtime != 100.0 || misalignment) { + char overlay[30]; + if (misalignment) { + std::snprintf(overlay, sizeof(overlay), "%g%% (%-.1f%%)", desiredRealtime, actualRealtime); + } else { + std::snprintf(overlay, sizeof(overlay), "%g%%", desiredRealtime); + } + mjr_overlay(mjFONT_BIG, mjGRID_TOPLEFT, smallrect, overlay, nullptr, &this->con); + } } // show ui 0 @@ -1847,6 +1863,9 @@ void Simulate::renderloop() { // get videomode and save this->vmode = *Glfw().glfwGetVideoMode(Glfw().glfwGetPrimaryMonitor()); + // use videomode refreshrate if nonzero + if (this->vmode.refreshRate) this->refreshRate = this->vmode.refreshRate; + // create window this->window = Glfw().glfwCreateWindow((2*this->vmode.width)/3, (2*this->vmode.height)/3, "Simulate", nullptr, nullptr); diff --git a/simulate/simulate.h b/simulate/simulate.h index ec8fca34..1c9577e9 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -86,9 +86,7 @@ class MJSIMULATEAPI Simulate { std::mutex mtx; std::condition_variable cond_loadrequest; - std::atomic_bool exitrequest = false; - - // option + // options int spacing = 0; int color = 0; int font = 0; @@ -102,24 +100,43 @@ class MJSIMULATEAPI Simulate { int vsync = 1; int busywait = 0; - // simulation - int run = 1; // keyframe index int key = 0; - std::atomic_int uiloadrequest = 0; + + // simulation + int run = 1; + + // atomics for cross-thread messages + std::atomic_bool exitrequest = false; std::atomic_bool droploadrequest = false; - // 2: render thread asked to update its model - // 1: showing "loading" label, about to load - // 0: model loaded or no load requested. - int loadrequest = 0; std::atomic_bool screenshotrequest = false; + std::atomic_int uiloadrequest = 0; + + // loadrequest + // 2: render thread asked to update its model + // 1: showing "loading" label, about to load + // 0: model loaded or no load requested. + int loadrequest = 0; + // strings char loadError[kMaxFilenameLength] = ""; char dropfilename[kMaxFilenameLength] = ""; char filename[kMaxFilenameLength] = ""; char previous_filename[kMaxFilenameLength] = ""; - int slow_down = 1; - bool speed_changed = true; + + // time synchronization + int realTimeIndex = 0; + bool speedChanged = true; + float measuredSlowdown = 1.0; + // logarithmically spaced realtime slow-down coefficients (percent) + static constexpr float percentRealTime[] = { + 100, 80, 66, 50, 40, 33, 25, 20, 16, 13, + 10, 8, 6.6, 5.0, 4, 3.3, 2.5, 2, 1.6, 1.3, + 1, .8, .66, .5, .4, .33, .25, .2, .16, .13, + .1 + }; + + // control noise double ctrlnoisestd = 0.0; double ctrlnoiserate = 0.0; @@ -147,6 +164,7 @@ class MJSIMULATEAPI Simulate { // OpenGL rendering and UI GLFWvidmode vmode = {}; + int refreshRate = 60; int windowpos[2] = {0}; int windowsize[2] = {0}; mjrContext con = {};