From 033ca81db62d923560d0ebec49e491e14ea4c45f Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 17 May 2022 09:37:52 -0400 Subject: [PATCH] Simulate: fix reload button causing segfault, make interface changes needed for Python bindings --- simulate/Makefile | 5 ++-- simulate/main.cc | 61 ++++++++++++++++++++++++++++++++------------ simulate/simulate.cc | 24 +++++++++++++++-- simulate/simulate.h | 7 +++++ 4 files changed, 76 insertions(+), 21 deletions(-) diff --git a/simulate/Makefile b/simulate/Makefile index 82c89a68..ecb4dbe6 100644 --- a/simulate/Makefile +++ b/simulate/Makefile @@ -7,6 +7,7 @@ COMMON=-O2 -I../include -L../lib -std=c++11 -pthread -Wl,-rpath,'$$ORIGIN'/../li all: $(CC) -c -O2 -fPIC -I../include uitools.c $(CXX) -c -O2 -fPIC -I../include simulate.cc - $(CXX) -shared -o libmjsimulate.so simulate.o uitools.o - mv libmjsimulate.so ../lib/libmjsimulate.so + $(CXX) $(COMMON) -shared -Wl,-no-as-needed -Wl,-soname,libmjsimulate.so.2.1.5 -lmujoco -o libmjsimulate.so.2.1.5 simulate.o uitools.o + mv libmjsimulate.so.2.1.5 ../lib/libmjsimulate.so.2.1.5 + ln -sf ../lib/libmjsimulate.so.2.1.5 ../lib/libmjsimulate.so $(CXX) $(COMMON) -Wl,-no-as-needed main.cc -lmjsimulate -lmujoco -lGL -lglfw -o ../bin/simulate diff --git a/simulate/main.cc b/simulate/main.cc index 23d7d547..6e61188b 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -102,15 +102,37 @@ void SimulateLoop(mj::Simulate& simulate) { if (simulate.droploadrequest) { mjModel* mnew = LoadModel(simulate.dropfilename, simulate); + simulate.droploadrequest = 0; 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); + + // allocate ctrlnoise + free(ctrlnoise); + ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu); + mju_zero(ctrlnoise, m->nu); + } + } + + if (simulate.uiloadrequest) { + mjModel* mnew = LoadModel(simulate.filename, simulate); + simulate.uiloadrequest = 0; + if (mnew) { + mjData* dnew = mj_makeData(mnew); + simulate.load(simulate.filename, mnew, dnew, true); m = mnew; d = dnew; mj_forward(m, d); + + // allocate ctrlnoise + free(ctrlnoise); + ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu); + mju_zero(ctrlnoise, m->nu); } } @@ -157,8 +179,8 @@ void SimulateLoop(mj::Simulate& simulate) { // clear old perturbations, apply new mju_zero(d->xfrc_applied, 6*m->nbody); - mjv_applyPerturbPose(m, d, &simulate.pert, 0); // move mocap bodies only - mjv_applyPerturbForce(m, d, &simulate.pert); + simulate.applyposepertubations(0); // move mocap bodies only + simulate.applyforceperturbations(); // run single step, let next iteration deal with timing mj_step(m, d); @@ -171,8 +193,8 @@ void SimulateLoop(mj::Simulate& simulate) { (glfwGetTime()-tmstart) < refreshfactor/simulate.vmode.refreshRate) { // clear old perturbations, apply new mju_zero(d->xfrc_applied, 6*m->nbody); - mjv_applyPerturbPose(m, d, &simulate.pert, 0); // move mocap bodies only - mjv_applyPerturbForce(m, d, &simulate.pert); + simulate.applyposepertubations(0); // move mocap bodies only + simulate.applyforceperturbations(); // run mj_step mjtNum prevtm = d->time*simulate.slow_down; @@ -189,7 +211,7 @@ void SimulateLoop(mj::Simulate& simulate) { // paused else { // apply pose perturbation - mjv_applyPerturbPose(m, d, &simulate.pert, 1); // move mocap and dynamic bodies + simulate.applyposepertubations(1); // move mocap and dynamic bodies // run mj_forward, to update rendering and joint sliders mj_forward(m, d); @@ -210,19 +232,9 @@ int main(int argc, const char** argv) { mju_error("Headers and library have different versions"); } - // simulate object for encapsulates UI + // simulate object encapsulates the UI mj::Simulate simulate; - // request loadmodel if file given (otherwise drag-and-drop) - if (argc>1) { - m = LoadModel(argv[1], simulate); - if (m) { - d = mj_makeData(m); - simulate.load(argv[1], m, d, true); - mj_forward(m, d); - } - } - // init GLFW if (!glfwInit()) { mju_error("could not initialize GLFW"); @@ -231,6 +243,21 @@ int main(int argc, const char** argv) { // start simulation thread (this creates the UI) simulate.startthread(); + // request loadmodel if file given (otherwise drag-and-drop) + if (argc>1) { + m = LoadModel(argv[1], simulate); + if (m) { + d = mj_makeData(m); + simulate.load(argv[1], m, d, true); + mj_forward(m, d); + + // allocate ctrlnoise + free(ctrlnoise); + ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu); + mju_zero(ctrlnoise, m->nu); + } + } + SimulateLoop(simulate); // If simulate loop exited its time to stop the UI diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 1c33a7f0..77fcecf3 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1150,7 +1150,7 @@ void uiEvent(mjuiState* state) { break; case 2: // Reload - simulate->loadrequest = 1; + simulate->uiloadrequest = 1; break; case 3: // Align @@ -1548,8 +1548,21 @@ void Simulate::stopthread(void) { this->renderthreadhandle.join(); } +//------------------------ apply pose perturbations ---------------------------- +void Simulate::applyposepertubations(int flg_paused) { + if (this->m != nullptr) { + mjv_applyPerturbPose(this->m, this->d, &this->pert, flg_paused); // move mocap bodies only + } +} -//-------------------- Tell the render thread to load a file ------------------- +//------------------------ apply force perturbations --------------------------- +void Simulate::applyforceperturbations(void) { + if (this->m != nullptr) { + mjv_applyPerturbForce(this->m, this->d, &this->pert); + } +} + +//-------------------- Tell the render thread to load a file and wait ---------- void Simulate::load(const char* file, mjModel* mnew, mjData* dnew, @@ -1559,6 +1572,13 @@ void Simulate::load(const char* file, this->delete_old_m_d = delete_old_m_d; mju::strcpy_arr(this->filename, file); this->loadrequest = 2; + + // Wait for the render thread to be done loading + // so that we know the old model and data's memory can + // be free'd by the other thread (sometimes python) + while (this->loadrequest > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } } //------------------------ load mjb or xml model ------------------------------- diff --git a/simulate/simulate.h b/simulate/simulate.h index 3b96e4b8..b02c77a9 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -36,6 +36,12 @@ class Simulate { // Stop the Simulate UI thread void stopthread(void); + // Apply UI pose perturbations to model and data + void applyposepertubations(int flg_paused); + + // Apply UI force perturbations to model and data + void applyforceperturbations(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); @@ -91,6 +97,7 @@ class Simulate { // simulation int run = 1; int key = 0; + int uiloadrequest = 0; int droploadrequest = 0; int loadrequest = 0; // strings