Simulate creates and maintains its own thread

simulate/main.cc

@@ -42,50 +42,101 @@ mjtNum* ctrlnoise = nullptr;
 
 //---------------------------------- simulation --------------------------------------
 
-// sim thread synchronization
-std::mutex& GetMutex() {
-  static std::mutex* mtx = new std::mutex();
-  return *mtx;
-}
 
-mj::Simulate& GetInstance() {
+mjModel* LoadModel(const char* file, mj::Simulate& simulate) {
-  // the creation of this static member will immediately
+  // this copy is needed so that the mju::strlen call below compiles
-  // initialize the glfw ui
+  char filename[mj::Simulate::kMaxFilenameLength];
-  static mj::Simulate* simulate = new mj::Simulate();
+  mju::strcpy_arr(filename, file);
-  return *simulate;
+
+  // make sure filename is not empty
+  if (!filename[0]) {
+    return nullptr;
+  }
+
+  // load and compile
+  char loadError[mj::Simulate::kMaxFilenameLength] = "";
+  mjModel* mnew = 0;
+  if (mju::strlen_arr(filename)>4 &&
+      !std::strncmp(filename+mju::strlen_arr(filename)-4, ".mjb",
+                    mju::sizeof_arr(filename)-mju::strlen_arr(filename)+4)) {
+    mnew = mj_loadModel(filename, nullptr);
+    if (!mnew) {
+      mju::strcpy_arr(loadError, "could not load binary model");
+    }
+  } else {
+    mnew = mj_loadXML(filename, nullptr, loadError, mj::Simulate::kMaxFilenameLength);
+    // remove trailing newline character from loadError
+    if (loadError[0]) {
+      int error_length = mju::strlen_arr(loadError);
+      if (loadError[error_length-1] == '\n') {
+        loadError[error_length-1] = '\0';
+      }
+    }
+  }
+
+  mju::strcpy_arr(simulate.loadError, loadError);
+
+  if (!mnew) {
+    std::printf("%s\n", loadError);
+    return nullptr;
+  }
+
+  // compiler warning: print and pause
+  if (loadError[0]) {
+    // mj_forward() below will print the warning message
+    std::printf("Model compiled, but simulation warning (paused):\n  %s\n", loadError);
+    simulate.run = 0;
+  }
+
+  return mnew;
 }
 
 // simulate in background thread (while rendering in main thread)
-void simulate_thread(void) {
+void SimulateLoop(mj::Simulate& simulate) {
   // cpu-sim syncronization point
   double cpusync = 0;
   mjtNum simsync = 0;
 
   // run until asked to exit
-  while (!GetInstance().exitrequest) {
+  while (!simulate.exitrequest) {
+
+    if (simulate.droploadrequest) {
+      mjModel* mnew = LoadModel(simulate.dropfilename, simulate);
+      if (mnew) {
+        mjData* dnew = mj_makeData(mnew);
+        simulate.load(simulate.dropfilename, mnew, dnew, true);
+
+        simulate.droploadrequest = 0;
+
+        m = mnew;
+        d = dnew;
+        mj_forward(m, d);
+      }
+    }
+
     // sleep for 1 ms or yield, to let main thread run
     //  yield results in busy wait - which has better timing but kills battery life
-    if (GetInstance().run && GetInstance().busywait) {
+    if (simulate.run && simulate.busywait) {
       std::this_thread::yield();
     } else {
       std::this_thread::sleep_for(std::chrono::milliseconds(1));
     }
 
     { // start exclusive access
-      const std::lock_guard<std::mutex> lock(GetMutex());
+      const std::lock_guard<std::mutex> lock(simulate.mtx);
 
       // run only if model is present
       if (m) {
         // running
-        if (GetInstance().run) {
+        if (simulate.run) {
           // record cpu time at start of iteration
           double tmstart = glfwGetTime();
 
           // inject noise
-          if (GetInstance().ctrlnoisestd) {
+          if (simulate.ctrlnoisestd) {
             // convert rate and scale to discrete time given current timestep
-            mjtNum rate = mju_exp(-m->opt.timestep / GetInstance().ctrlnoiserate);
+            mjtNum rate = mju_exp(-m->opt.timestep / simulate.ctrlnoiserate);
-            mjtNum scale = GetInstance().ctrlnoisestd * mju_sqrt(1-rate*rate);
+            mjtNum scale = simulate.ctrlnoisestd * mju_sqrt(1-rate*rate);
 
             for (int i=0; i<m->nu; i++) {
               // update noise
@@ -96,18 +147,18 @@ void simulate_thread(void) {
           }
 
           // out-of-sync (for any reason)
-          mjtNum offset = mju_abs((d->time*GetInstance().slow_down-simsync)-(tmstart-cpusync));
+          mjtNum offset = mju_abs((d->time*simulate.slow_down-simsync)-(tmstart-cpusync));
-          if( d->time*GetInstance().slow_down<simsync || tmstart<cpusync || cpusync==0 ||
+          if( d->time*simulate.slow_down<simsync || tmstart<cpusync || cpusync==0 ||
-              offset > syncmisalign*GetInstance().slow_down || GetInstance().speed_changed) {
+              offset > syncmisalign*simulate.slow_down || simulate.speed_changed) {
             // re-sync
             cpusync = tmstart;
-            simsync = d->time*GetInstance().slow_down;
+            simsync = d->time*simulate.slow_down;
-            GetInstance().speed_changed = false;
+            simulate.speed_changed = false;
 
             // clear old perturbations, apply new
             mju_zero(d->xfrc_applied, 6*m->nbody);
-            mjv_applyPerturbPose(m, d, &GetInstance().pert, 0);  // move mocap bodies only
+            mjv_applyPerturbPose(m, d, &simulate.pert, 0);  // move mocap bodies only
-            mjv_applyPerturbForce(m, d, &GetInstance().pert);
+            mjv_applyPerturbForce(m, d, &simulate.pert);
 
             // run single step, let next iteration deal with timing
             mj_step(m, d);
@@ -116,19 +167,19 @@ void simulate_thread(void) {
           // in-sync
           else {
             // step while simtime lags behind cputime, and within safefactor
-            while ((d->time*GetInstance().slow_down-simsync) < (glfwGetTime()-cpusync) &&
+            while ((d->time*simulate.slow_down-simsync) < (glfwGetTime()-cpusync) &&
-                   (glfwGetTime()-tmstart) < refreshfactor/GetInstance().vmode.refreshRate) {
+                   (glfwGetTime()-tmstart) < refreshfactor/simulate.vmode.refreshRate) {
               // clear old perturbations, apply new
               mju_zero(d->xfrc_applied, 6*m->nbody);
-              mjv_applyPerturbPose(m, d, &GetInstance().pert, 0);  // move mocap bodies only
+              mjv_applyPerturbPose(m, d, &simulate.pert, 0);  // move mocap bodies only
-              mjv_applyPerturbForce(m, d, &GetInstance().pert);
+              mjv_applyPerturbForce(m, d, &simulate.pert);
 
               // run mj_step
-              mjtNum prevtm = d->time*GetInstance().slow_down;
+              mjtNum prevtm = d->time*simulate.slow_down;
               mj_step(m, d);
 
               // break on reset
-              if (d->time*GetInstance().slow_down<prevtm) {
+              if (d->time*simulate.slow_down<prevtm) {
                 break;
               }
             }
@@ -138,7 +189,7 @@ void simulate_thread(void) {
         // paused
         else {
           // apply pose perturbation
-          mjv_applyPerturbPose(m, d, &GetInstance().pert, 1);      // move mocap and dynamic bodies
+          mjv_applyPerturbPose(m, d, &simulate.pert, 1);      // move mocap and dynamic bodies
 
           // run mj_forward, to update rendering and joint sliders
           mj_forward(m, d);
@@ -159,58 +210,36 @@ int main(int argc, const char** argv) {
     mju_error("Headers and library have different versions");
   }
 
+  // simulate object for encapsulates UI
+  mj::Simulate simulate;
+
   // request loadmodel if file given (otherwise drag-and-drop)
   if (argc>1) {
-    mju::strcpy_arr(GetInstance().filename, argv[1]);
+    m = LoadModel(argv[1], simulate);
-    GetInstance().loadrequest = 2;
+    if (m) {
+      d = mj_makeData(m);
+      simulate.load(argv[1], m, d, true);
+      mj_forward(m, d);
+    }
   }
 
-  // start simulation thread
+  // init GLFW
-  std::thread simthread(simulate_thread);
+  if (!glfwInit()) {
-
+    mju_error("could not initialize GLFW");
-  // run event loop
-  while (!glfwWindowShouldClose(GetInstance().window) && !GetInstance().exitrequest) {
-    { // start exclusive access (block simulation thread)
-      const std::lock_guard<std::mutex> lock(GetMutex());
-
-      // load model (not on first pass, to show "loading" label)
-      if (GetInstance().loadrequest==1) {
-        {
-          GetInstance().loadmodel();
-          m = GetInstance().m;
-          d = GetInstance().d;
-
-          // allocate ctrlnoise
-          free(ctrlnoise);
-          ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
-          mju_zero(ctrlnoise, m->nu);
-        }
-      } else if (GetInstance().loadrequest>1) {
-        GetInstance().loadrequest = 1;
-      }
-
-      // handle events (calls all callbacks)
-      glfwPollEvents();
-
-      // prepare to render
-      GetInstance().prepare();
-    } // end exclusive access (allow simulation thread to run)
-
-    // render while simulation is running
-    GetInstance().render();
   }
 
-  // stop simulation thread
+  // start simulation thread (this creates the UI)
-  GetInstance().exitrequest = 1;
+  simulate.startthread();
-  simthread.join();
+
+  SimulateLoop(simulate);
+
+  // If simulate loop exited its time to stop the UI
+  simulate.stopthread();
 
   // delete everything we allocated
-  GetInstance().clearcallback();
   free(ctrlnoise);
   mj_deleteData(d);
   mj_deleteModel(m);
-  mjv_freeScene(&GetInstance().scn);
-  mjr_freeContext(&GetInstance().con);
 
   // terminate GLFW (crashes with Linux NVidia drivers)
 #if defined(__APPLE__) || defined(_WIN32)

simulate/simulate.cc

@@ -1515,8 +1515,8 @@ void uiRender(mjuiState* state) {
 void drop(mj::Simulate* simulate, int count, const char** paths) {
   // make sure list is non-empty
   if (count>0) {
-    mju::strcpy_arr(simulate->filename, paths[0]);
+    mju::strcpy_arr(simulate->dropfilename, paths[0]);
-    simulate->loadrequest = 1;
+    simulate->droploadrequest = 1;
   }
 }
 
@@ -1534,132 +1534,47 @@ namespace mju = ::mujoco::sample_util;
 
 // create object and initialize the simulate ui
 Simulate::Simulate(void) {
-  // init GLFW, set timer callback (milliseconds)
+} // TODO constructor is now empty...
-  if (!glfwInit()) {
-    mju_error("could not initialize GLFW");
-  }
-  mjcb_time = timer;
 
-  // multisampling
+//------------------------ start the render thread -----------------------------
-  glfwWindowHint(GLFW_SAMPLES, 4);
+void Simulate::startthread(void) {
-  glfwWindowHint(GLFW_VISIBLE, 1);
+  this->renderthreadhandle = std::thread(&Simulate::renderthread, this);
+}
 
-  // get videomode and save
+//------------------------ stop the render thread ------------------------------
-  this->vmode = *glfwGetVideoMode(glfwGetPrimaryMonitor());
+void Simulate::stopthread(void) {
+    // stop simulation thread
+  this->exitrequest = 1;
+  this->renderthreadhandle.join();
+}
 
-  // create window
-  this->window = glfwCreateWindow((2*this->vmode.width)/3, (2*this->vmode.height)/3,
-                            "Simulate", nullptr, nullptr);
-  if (!this->window) {
-    glfwTerminate();
-    mju_error("could not create window");
-  }
 
-  // save window position and size
+//-------------------- Tell the render thread to load a file -------------------
-  glfwGetWindowPos(this->window, this->windowpos, this->windowpos+1);
+void Simulate::load(const char* file,
-  glfwGetWindowSize(this->window, this->windowsize, this->windowsize+1);
+                    mjModel* mnew,
-
+                    mjData* dnew,
-  // make context current, set v-sync
+                    bool delete_old_m_d) {
-  glfwMakeContextCurrent(this->window);
+  this->mnew = mnew;
-  glfwSwapInterval(this->vsync);
+  this->dnew = dnew;
-
+  this->delete_old_m_d = delete_old_m_d;
-  // init abstract visualization
+  mju::strcpy_arr(this->filename, file);
-  mjv_defaultCamera(&this->cam);
+  this->loadrequest = 2;
-  mjv_defaultOption(&this->vopt);
-  profilerinit(this);
-  sensorinit(this);
-
-  // make empty scene
-  mjv_defaultScene(&this->scn);
-  mjv_makeScene(nullptr, &this->scn, maxgeom);
-
-  // select default font
-  int fontscale = uiFontScale(this->window);
-  this->font = fontscale/50 - 1;
-
-  // make empty context
-  mjr_defaultContext(&this->con);
-  mjr_makeContext(nullptr, &this->con, fontscale);
-
-  // init state and uis
-  std::memset(&this->uistate, 0, sizeof(mjuiState));
-  std::memset(&this->ui0, 0, sizeof(mjUI));
-  std::memset(&this->ui1, 0, sizeof(mjUI));
-  this->ui0.spacing = mjui_themeSpacing(this->spacing);
-  this->ui0.color = mjui_themeColor(this->color);
-  this->ui0.predicate = uiPredicate;
-  this->ui0.rectid = 1;
-  this->ui0.auxid = 0;
-  this->ui1.spacing = mjui_themeSpacing(this->spacing);
-  this->ui1.color = mjui_themeColor(this->color);
-  this->ui1.predicate = uiPredicate;
-  this->ui1.rectid = 2;
-  this->ui1.auxid = 1;
-
-  // set GLFW callbacks
-  this->uistate.userdata = (void*)(this);
-  uiSetCallback(this->window, &this->uistate, uiEvent, uiLayout, uiRender, uiDrop);
-
-  // populate uis with standard sections
-  this->ui0.userdata = (void*)(this);
-  this->ui1.userdata = (void*)(this);
-  mjui_add(&this->ui0, defFile);
-  mjui_add(&this->ui0, this->defOption);
-  mjui_add(&this->ui0, this->defSimulation);
-  mjui_add(&this->ui0, this->defWatch);
-  uiModify(this->window, &this->ui0, &this->uistate, &this->con);
-  uiModify(this->window, &this->ui1, &this->uistate, &this->con);
 }
 
 //------------------------ load mjb or xml model -------------------------------
 void Simulate::loadmodel(void) {
-  // clear request
+  if (this->delete_old_m_d) {
-  this->loadrequest = 0;
+    // delete old model if requested
-
+    if (this->d) {
-  // make sure filename is not empty
+      mj_deleteData(d);
-  if (!this->filename[0]) {
-    return;
-  }
-
-  // load and compile
-  this->loadError[0] = '\0';
-  mjModel* mnew = 0;
-  if (mju::strlen_arr(this->filename)>4 &&
-      !std::strncmp(this->filename+mju::strlen_arr(this->filename)-4, ".mjb",
-                    mju::sizeof_arr(this->filename)-mju::strlen_arr(this->filename)+4)) {
-    mnew = mj_loadModel(this->filename, nullptr);
-    if (!mnew) {
-      mju::strcpy_arr(this->loadError, "could not load binary model");
     }
-  } else {
+    if (this->m) {
-    mnew = mj_loadXML(this->filename, nullptr, this->loadError, Simulate::kMaxFilenameLength);
+      mj_deleteModel(m);
-    // remove trailing newline character from loadError
-    if (this->loadError[0]) {
-      int error_length = mju::strlen_arr(this->loadError);
-      if (this->loadError[error_length-1] == '\n') {
-        this->loadError[error_length-1] = '\0';
-      }
     }
   }
-  if (!mnew) {
-    std::printf("%s\n", this->loadError);
-    return;
-  }
 
-  // compiler warning: print and pause
+  this->m = this->mnew;
-  if (this->loadError[0]) {
+  this->d = this->dnew;
-    // mj_forward() below will print the warning message
-    std::printf("Model compiled, but simulation warning (paused):\n  %s\n", this->loadError);
-    this->run = 0;
-  }
-
-  // delete old model, assign new
-  mj_deleteData(this->d);
-  mj_deleteModel(this->m);
-  this->m = nullptr;
-  this->m = mj_copyModel(this->m, mnew);
-  this->d = mj_makeData(this->m);
-  mj_forward(this->m, this->d);
 
   // re-create scene and context
   mjv_makeScene(this->m, &this->scn, maxgeom);
@@ -1698,6 +1613,9 @@ void Simulate::loadmodel(void) {
   uiModify(this->window, &this->ui0, &this->uistate, &this->con);
   uiModify(this->window, &this->ui1, &this->uistate, &this->con);
   updatesettings(this);
+
+  // clear request
+  this->loadrequest = 0;
 }
 
 
@@ -1861,4 +1779,108 @@ void Simulate::clearcallback(void) {
   uiClearCallback(this->window);
 }
 
+void Simulate::renderthread(void) {
+  // Set timer callback (milliseconds)
+  mjcb_time = timer;
+
+  // multisampling
+  glfwWindowHint(GLFW_SAMPLES, 4);
+  glfwWindowHint(GLFW_VISIBLE, 1);
+
+  // get videomode and save
+  this->vmode = *glfwGetVideoMode(glfwGetPrimaryMonitor());
+
+  // create window
+  this->window = glfwCreateWindow((2*this->vmode.width)/3, (2*this->vmode.height)/3,
+                            "Simulate", nullptr, nullptr);
+  if (!this->window) {
+    glfwTerminate();
+    mju_error("could not create window");
+  }
+
+  // save window position and size
+  glfwGetWindowPos(this->window, this->windowpos, this->windowpos+1);
+  glfwGetWindowSize(this->window, this->windowsize, this->windowsize+1);
+
+  // make context current, set v-sync
+  glfwMakeContextCurrent(this->window);
+  glfwSwapInterval(this->vsync);
+
+  // init abstract visualization
+  mjv_defaultCamera(&this->cam);
+  mjv_defaultOption(&this->vopt);
+  profilerinit(this);
+  sensorinit(this);
+
+  // make empty scene
+  mjv_defaultScene(&this->scn);
+  mjv_makeScene(nullptr, &this->scn, maxgeom);
+
+  // select default font
+  int fontscale = uiFontScale(this->window);
+  this->font = fontscale/50 - 1;
+
+  // make empty context
+  mjr_defaultContext(&this->con);
+  mjr_makeContext(nullptr, &this->con, fontscale);
+
+  // init state and uis
+  std::memset(&this->uistate, 0, sizeof(mjuiState));
+  std::memset(&this->ui0, 0, sizeof(mjUI));
+  std::memset(&this->ui1, 0, sizeof(mjUI));
+  this->ui0.spacing = mjui_themeSpacing(this->spacing);
+  this->ui0.color = mjui_themeColor(this->color);
+  this->ui0.predicate = uiPredicate;
+  this->ui0.rectid = 1;
+  this->ui0.auxid = 0;
+  this->ui1.spacing = mjui_themeSpacing(this->spacing);
+  this->ui1.color = mjui_themeColor(this->color);
+  this->ui1.predicate = uiPredicate;
+  this->ui1.rectid = 2;
+  this->ui1.auxid = 1;
+
+  // set GLFW callbacks
+  this->uistate.userdata = (void*)(this);
+  uiSetCallback(this->window, &this->uistate, uiEvent, uiLayout, uiRender, uiDrop);
+
+  // populate uis with standard sections
+  this->ui0.userdata = (void*)(this);
+  this->ui1.userdata = (void*)(this);
+  mjui_add(&this->ui0, defFile);
+  mjui_add(&this->ui0, this->defOption);
+  mjui_add(&this->ui0, this->defSimulation);
+  mjui_add(&this->ui0, this->defWatch);
+  uiModify(this->window, &this->ui0, &this->uistate, &this->con);
+  uiModify(this->window, &this->ui1, &this->uistate, &this->con);
+
+  // run event loop
+  while (!glfwWindowShouldClose(this->window) && !this->exitrequest) {
+    { // start exclusive access (block simulation thread)
+      const std::lock_guard<std::mutex> lock(this->mtx);
+
+      // load model (not on first pass, to show "loading" label)
+      if (this->loadrequest==1) {
+        {
+          this->loadmodel();
+        }
+      } else if (this->loadrequest>1) {
+        this->loadrequest = 1;
+      }
+
+      // handle events (calls all callbacks)
+      glfwPollEvents();
+
+      // prepare to render
+      this->prepare();
+    } // end exclusive access (allow simulation thread to run)
+
+    // render while simulation is running
+    this->render();
+  }
+
+  this->clearcallback();
+  mjv_freeScene(&this->scn);
+  mjr_freeContext(&this->con);
+}
+
 }  // namespace mujoco

simulate/simulate.h

@@ -15,6 +15,9 @@
 #ifndef MUJOCO_SIMULATE_H_
 #define MUJOCO_SIMULATE_H_
 
+#include <mutex>
+#include <thread>
+
 #include "uitools.h"
 
 namespace mujoco {
@@ -27,7 +30,18 @@ class Simulate {
   // create object and initialize the simulate ui
   Simulate(void);
 
-  // load mjb or xml model
+  // Start the Simulate UI thread
+  void startthread(void);
+
+  // Stop the Simulate UI thread
+  void stopthread(void);
+
+  // Request that the Simulate UI thread render a new model
+  // optionally delete the old model and data when done
+  void load(const char* file, mjModel* m, mjData* d, bool delete_old_m_d);
+
+  // functions below are used by the renderthread
+  // load mjb or xml model that has been requested by load()
   void loadmodel(void);
 
   // prepare to render
@@ -39,12 +53,23 @@ class Simulate {
   // clear callbacks registered in external structures
   void clearcallback(void);
 
+  // thread to render the UI
+  void renderthread(void);
+
   // constants
   static constexpr int kMaxFilenameLength = 1000;
 
+  // the UI rendering thread
+  std::thread renderthreadhandle;
+
   // model and data to be visualized
-  mjModel* m;
+  mjModel* mnew = nullptr;
-  mjData* d;
+  mjData* dnew = nullptr;
+  bool delete_old_m_d = false;
+
+  mjModel* m = nullptr;
+  mjData* d = nullptr;
+  std::mutex mtx;
 
   // file
   int exitrequest = 0;
@@ -66,9 +91,11 @@ class Simulate {
   // simulation
   int run = 1;
   int key = 0;
+  int droploadrequest = 0;
   int loadrequest = 0;
   // strings
   char loadError[kMaxFilenameLength] = "";
+  char dropfilename[kMaxFilenameLength] = "";
   char filename[kMaxFilenameLength] = "";
   char previous_filename[kMaxFilenameLength] = "";
   int slow_down = 1;