diff --git a/simulate/main.cc b/simulate/main.cc index 40e9cf0a..23d7d547 100644 --- a/simulate/main.cc +++ b/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() { - // the creation of this static member will immediately - // initialize the glfw ui - static mj::Simulate* simulate = new mj::Simulate(); - return *simulate; +mjModel* LoadModel(const char* file, mj::Simulate& simulate) { + // this copy is needed so that the mju::strlen call below compiles + char filename[mj::Simulate::kMaxFilenameLength]; + mju::strcpy_arr(filename, file); + + // 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 lock(GetMutex()); + const std::lock_guard 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 scale = GetInstance().ctrlnoisestd * mju_sqrt(1-rate*rate); + mjtNum rate = mju_exp(-m->opt.timestep / simulate.ctrlnoiserate); + mjtNum scale = simulate.ctrlnoisestd * mju_sqrt(1-rate*rate); for (int i=0; inu; 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)); - if( d->time*GetInstance().slow_down syncmisalign*GetInstance().slow_down || GetInstance().speed_changed) { + mjtNum offset = mju_abs((d->time*simulate.slow_down-simsync)-(tmstart-cpusync)); + if( d->time*simulate.slow_down syncmisalign*simulate.slow_down || simulate.speed_changed) { // re-sync cpusync = tmstart; - simsync = d->time*GetInstance().slow_down; - GetInstance().speed_changed = false; + simsync = d->time*simulate.slow_down; + 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_applyPerturbForce(m, d, &GetInstance().pert); + mjv_applyPerturbPose(m, d, &simulate.pert, 0); // move mocap bodies only + 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) && - (glfwGetTime()-tmstart) < refreshfactor/GetInstance().vmode.refreshRate) { + while ((d->time*simulate.slow_down-simsync) < (glfwGetTime()-cpusync) && + (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_applyPerturbForce(m, d, &GetInstance().pert); + mjv_applyPerturbPose(m, d, &simulate.pert, 0); // move mocap bodies only + 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_downtime*simulate.slow_down1) { - mju::strcpy_arr(GetInstance().filename, argv[1]); - GetInstance().loadrequest = 2; + m = LoadModel(argv[1], simulate); + if (m) { + d = mj_makeData(m); + simulate.load(argv[1], m, d, true); + mj_forward(m, d); + } } - // start simulation thread - std::thread simthread(simulate_thread); - - // run event loop - while (!glfwWindowShouldClose(GetInstance().window) && !GetInstance().exitrequest) { - { // start exclusive access (block simulation thread) - const std::lock_guard 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(); + // init GLFW + if (!glfwInit()) { + mju_error("could not initialize GLFW"); } - // stop simulation thread - GetInstance().exitrequest = 1; - simthread.join(); + // start simulation thread (this creates the UI) + simulate.startthread(); + + 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) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 41f8c675..1c33a7f0 100644 --- a/simulate/simulate.cc +++ b/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]); - simulate->loadrequest = 1; + mju::strcpy_arr(simulate->dropfilename, paths[0]); + 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) - if (!glfwInit()) { - mju_error("could not initialize GLFW"); - } - mjcb_time = timer; +} // TODO constructor is now empty... - // multisampling - glfwWindowHint(GLFW_SAMPLES, 4); - glfwWindowHint(GLFW_VISIBLE, 1); +//------------------------ start the render thread ----------------------------- +void Simulate::startthread(void) { + this->renderthreadhandle = std::thread(&Simulate::renderthread, this); +} - // get videomode and save - this->vmode = *glfwGetVideoMode(glfwGetPrimaryMonitor()); +//------------------------ stop the render thread ------------------------------ +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 - 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); +//-------------------- Tell the render thread to load a file ------------------- +void Simulate::load(const char* file, + mjModel* mnew, + mjData* dnew, + bool delete_old_m_d) { + this->mnew = mnew; + this->dnew = dnew; + this->delete_old_m_d = delete_old_m_d; + mju::strcpy_arr(this->filename, file); + this->loadrequest = 2; } //------------------------ load mjb or xml model ------------------------------- void Simulate::loadmodel(void) { - // clear request - this->loadrequest = 0; - - // make sure filename is not empty - 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"); + if (this->delete_old_m_d) { + // delete old model if requested + if (this->d) { + mj_deleteData(d); } - } else { - mnew = mj_loadXML(this->filename, nullptr, this->loadError, Simulate::kMaxFilenameLength); - // 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 (this->m) { + mj_deleteModel(m); } } - if (!mnew) { - std::printf("%s\n", this->loadError); - return; - } - // compiler warning: print and pause - if (this->loadError[0]) { - // 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); + this->m = this->mnew; + this->d = this->dnew; // 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 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 diff --git a/simulate/simulate.h b/simulate/simulate.h index 66c04ac3..3b96e4b8 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -15,6 +15,9 @@ #ifndef MUJOCO_SIMULATE_H_ #define MUJOCO_SIMULATE_H_ +#include +#include + #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; - mjData* d; + mjModel* mnew = nullptr; + 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;