diff --git a/install.m b/install.m index 51c4309..880b7e4 100644 --- a/install.m +++ b/install.m @@ -4,7 +4,7 @@ function install() %% Run this script once and follow the instructions % Make sure you run this script from this folder. -MJ_VER = '2.3.2'; +MJ_VER = '2.3.5'; GLFW_VER = '3.3.7'; urlsList = fileread("tools/links.json"); blockPath = './blocks/'; diff --git a/src/mj.hpp b/src/mj.hpp index 894dea4..42692c4 100644 --- a/src/mj.hpp +++ b/src/mj.hpp @@ -78,8 +78,7 @@ class MujocoModelInstance // disable copy constructor MujocoModelInstance(const MujocoModelInstance &mi); public: - std::mutex dMutex; // mutex for model data access - binarySemp cameraSync; // semp for syncing main thread and render camera thread + std::mutex dMutex; // mutex for model data access // cameras in the model instance std::vector> offscreenCam; @@ -108,6 +107,8 @@ class MujocoModelInstance double lastRenderTime = 0; double cameraRenderInterval = 0.020; std::atomic isCameraDataNew = false; + binarySemp cameraSync; // semp for syncing main thread and render camera thread + std::atomic shouldCameraRenderNow = false; void step(std::vector u); std::vector getSensor(unsigned index); diff --git a/src/mj_sfun.cpp b/src/mj_sfun.cpp index f4ee2d0..0e58bef 100644 --- a/src/mj_sfun.cpp +++ b/src/mj_sfun.cpp @@ -442,17 +442,7 @@ static void mdlUpdate(SimStruct *S, int_T tid) auto &miTemp = sd.mi[miIndex]; - if(miTemp->offscreenCam.size() != 0) - { - if( (miTemp->get_d()->time - miTemp->lastRenderTime) > miTemp->cameraRenderInterval) - { - // maintain camera and physics in sync at required camera sample time - miTemp->cameraSync.acquire(); // blocking till offscreen buffer is rendered - miTemp->lastRenderTime = miTemp->get_d()->time; - } - } - - // set control inputs + // Step the simulation by one discrete time step. Outputs (sensors and camera) get reflected in the next step miTemp->step(uVec); } @@ -525,7 +515,6 @@ void renderingThreadFcn() while(1) { // Visualization window(s) - int activeCount = 0; for(int index=0; indexlastRenderClockTime; @@ -533,7 +522,6 @@ void renderingThreadFcn() { if(sd.mg[index]->loopInThread() == 0) { - activeCount++; sd.mg[index]->lastRenderClockTime = std::chrono::steady_clock::now(); } } @@ -544,23 +532,25 @@ void renderingThreadFcn() { auto &miTemp = sd.mi[miIndex]; - if(miTemp->cameraSync.check_availability() == false) + if(miTemp->shouldCameraRenderNow == true) { // if rendering is already done and not consumed, dont do again for(int camIndex = 0; camIndexoffscreenCam.size(); camIndex++) { - if(miTemp->offscreenCam[camIndex]->loopInThread() == 0) + auto status = miTemp->offscreenCam[camIndex]->loopInThread(); + if(status == 0) { - activeCount++; - miTemp->isCameraDataNew = true; + miTemp->lastRenderTime = miTemp->get_d()->time; + miTemp->isCameraDataNew = true; // Used to indicate that a new data is available for copying into blk output } } + miTemp->shouldCameraRenderNow = false; miTemp->cameraSync.release(); + } } // If there is nothing to render, donot keep spinning while loop if(sd.signalThreadExit == true) break; - // if(activeCount == 0) break; } // Release visualization resources @@ -584,16 +574,17 @@ void renderingThreadFcn() static void mdlOutputs(SimStruct *S, int_T tid) { int miIndex = ssGetIWorkValue(S, MI_IW_IDX); + auto &miTemp = sd.mi[miIndex]; // Copy sensors to output real_T *y = ssGetOutputPortRealSignal(S, SENSOR_PORT_INDEX); int_T ny = ssGetOutputPortWidth(S, SENSOR_PORT_INDEX); int_T index = 0; - auto nSensors = sd.mi[miIndex]->si.count; + auto nSensors = miTemp->si.count; for(int_T i=0; i yVec = sd.mi[miIndex]->getSensor(i); + vector yVec = miTemp->getSensor(i); for(auto elem: yVec) { y[index] = elem; @@ -603,15 +594,29 @@ static void mdlOutputs(SimStruct *S, int_T tid) } y[index] = static_cast(nSensors); // last element is a dummy to handle empty sensor case + // Render camera based on the current states. mdlupdate will be called after mdloutputs and update moves the time tk to tk+1 + if(miTemp->offscreenCam.size() != 0) + { + double elapsedTimeSinceRender = miTemp->get_d()->time - miTemp->lastRenderTime; + if( elapsedTimeSinceRender > (miTemp->cameraRenderInterval-0.00001) ) + { + // maintain camera and physics in sync at required camera sample time + miTemp->shouldCameraRenderNow = true; + miTemp->cameraSync.acquire(); // blocking till offscreen buffer is rendered + + // ssPrintf("sim time=%lf & render time=%lf\n", miTemp->get_d()->time, miTemp->lastRenderTime); + } + } + // Copy camera to output uint8_T *rgbOut = (uint8_T *) ssGetOutputPortSignal(S, RGB_PORT_INDEX); real32_T *depthOut = (real32_T *) ssGetOutputPortSignal(S, DEPTH_PORT_INDEX); - if(sd.mi[miIndex]->isCameraDataNew) + if(miTemp->isCameraDataNew) { //avoid unnecessary memcpy. copy only when there is new data. Rest of the time steps, old data will be output - sd.mi[miIndex]->getCameraRGB((uint8_t *) rgbOut); - sd.mi[miIndex]->getCameraDepth((float *) depthOut); - sd.mi[miIndex]->isCameraDataNew = false; + miTemp->getCameraRGB((uint8_t *) rgbOut); + miTemp->getCameraDepth((float *) depthOut); + miTemp->isCameraDataNew = false; } } diff --git a/tools/links.json b/tools/links.json index 12e3a18..609e094 100644 --- a/tools/links.json +++ b/tools/links.json @@ -30,6 +30,18 @@ "version": "2.3.2", "downloadLink": "https://github.com/deepmind/mujoco/releases/download/2.3.2/mujoco-2.3.2-linux-x86_64.tar.gz" }, + { + "name": "mujoco", + "arch": "win64", + "version": "2.3.5", + "downloadLink": "https://github.com/deepmind/mujoco/releases/download/2.3.5/mujoco-2.3.5-windows-x86_64.zip" + }, + { + "name": "mujoco", + "arch": "glnxa64", + "version": "2.3.5", + "downloadLink": "https://github.com/deepmind/mujoco/releases/download/2.3.5/mujoco-2.3.5-linux-x86_64.tar.gz" + }, { "name": "glfw", "arch": "win64",