chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Simulate creates and maintains its own thread

Browse Source
This commit is contained in:
Levi Burner
2022-04-14 14:36:27 -04:00
parent 9f4a7bf4fc
commit 447877abef
3 changed files with 270 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulate/main.cc
+102 -73
View File
@@ -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<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 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; 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));
if( d->time*GetInstance().slow_down<simsync || tmstart<cpusync || cpusync==0 ||
offset > 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<simsync || tmstart<cpusync || cpusync==0 ||
offset > 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_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]);
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<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();
// 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)
simulate/simulate.cc
+138 -116
View File
@@ -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<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
+30 -3
View File
@@ -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;
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;