Merge pull request #2 from mathworks-robotics/dev

Dev

Author: vmanoj1996

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/';

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<std::shared_ptr<MujocoGUI>> offscreenCam;
@@ -108,6 +107,8 @@ class MujocoModelInstance
     double lastRenderTime = 0;
     double cameraRenderInterval = 0.020;
     std::atomic<bool> isCameraDataNew = false;
+    binarySemp cameraSync; // semp for syncing main thread and render camera thread
+    std::atomic<bool> shouldCameraRenderNow = false;
 
     void step(std::vector<double> u);
     std::vector<double> getSensor(unsigned index);

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,15 +515,13 @@ void renderingThreadFcn()
     while(1)
     {
         // Visualization window(s)
-        int activeCount = 0;
         for(int index=0; index<sd.mg.size(); index++)
         {
             auto duration = std::chrono::steady_clock::now() - sd.mg[index]->lastRenderClockTime;
             if (duration>sd.mg[index]->renderInterval)
             {
                 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; camIndex<miTemp->offscreenCam.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<nSensors; i++)
     {
-        vector<double> yVec = sd.mi[miIndex]->getSensor(i);
+        vector<double> 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<double>(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;
     }
 }
 

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",