Refactor simulate to split UI from application

Fix name of glew library

Move simulate to new directory and refactor to use C++ conventions

Wrap simulate in mujoco namespace

fix namespaces, convert struct to class, use std::lock_guard

use GetMutex to better handle destruction ordering

rename mjSimulate to Simulate

nit: space before public and typo

move startup print

use nullptr instead of NULL

use GetInstance() to get simulate object

convert init to default constructor

sample/Makefile

@@ -11,6 +11,3 @@ all:
 	$(CXX) $(COMMON) derivative.cc         -lmujoco -fopenmp -o ../bin/derivative
 	$(CXX) $(COMMON) basic.cc              -lmujoco -lglfw   -o ../bin/basic
 	$(CXX) $(COMMON) record.cc             -lmujoco -lglfw   -o ../bin/record
-	$(CC) -c -O2 -I../include uitools.c
-	$(CXX) $(COMMON) uitools.o simulate.cc -lmujoco -lglfw   -o ../bin/simulate
-	rm *.o

sample/Makefile.macos

@@ -17,7 +17,3 @@ all:
 	clang++ $(ALLFLAGS)    derivative.cc -framework mujoco        -o derivative
 	clang++ $(ALLFLAGS)    basic.cc      -framework mujoco -lglfw -o basic
 	clang++ $(ALLFLAGS)    record.cc     -framework mujoco -lglfw -o record
-	clang   -c $(CFLAGS)   uitools.c
-	clang++ -c $(CXXFLAGS) macos_save.mm
-	clang++ $(ALLFLAGS)    simulate.cc uitools.o macos_save.o -framework mujoco -framework Cocoa -lglfw -o simulate
-	rm *.o

sample/Makefile.windows

@@ -15,5 +15,4 @@ all:
 	cl $(COMMON) derivative.cc /openmp                     ../lib/mujoco.lib
 	cl $(COMMON) basic.cc              ../lib/glfw3dll.lib ../lib/mujoco.lib
 	cl $(COMMON) record.cc             ../lib/glfw3dll.lib ../lib/mujoco.lib
-	cl $(COMMON) simulate.cc uitools.c ../lib/glfw3dll.lib ../lib/mujoco.lib
 	del *.obj

simulate/Makefile

@@ -0,0 +1,12 @@
+# This Makefile assumes that you have GLFW libraries and headers installed on,
+# which is commonly available through your distro's package manager.
+# On Debian and Ubuntu, GLFW can be installed via `apt install libglfw3-dev`.
+
+COMMON=-O2 -I../include -L../lib -std=c++11 -pthread -Wl,-rpath,'$$ORIGIN'/../lib
+
+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) main.cc -lmjsimulate -lmujoco -lGL -lglfw -o ../bin/simulate

simulate/array_safety.h

@@ -0,0 +1,96 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MUJOCO_SAMPLE_ARRAY_SAFETY_H_
+#define MUJOCO_SAMPLE_ARRAY_SAFETY_H_
+
+#include <algorithm>
+#include <cstdarg>
+#include <cstddef>
+#include <cstdio>
+#include <cstring>
+
+// Provides safe alternatives to the sizeof() operator and standard library functions for handling
+// null-terminated (C-style) strings in raw char arrays.
+//
+// These functions make use of compile-time array sizes to limit read and write operations to within
+// the array bounds. They are designed to trigger a compile error if the array size cannot be
+// determined at compile time (e.g. when an array has decayed into a pointer).
+//
+// They do not perform runtime bound checks.
+
+namespace mujoco {
+namespace sample_util {
+
+// returns sizeof(arr)
+// use instead of sizeof() to avoid unintended array-to-pointer decay
+template <typename T, int N>
+static constexpr std::size_t sizeof_arr(const T(&arr)[N]) {
+  return sizeof(arr);
+}
+
+// like std::strcmp but it will not read beyond the bound of either lhs or rhs
+template <std::size_t N1, std::size_t N2>
+static inline int strcmp_arr(const char (&lhs)[N1], const char (&rhs)[N2]) {
+  return std::strncmp(lhs, rhs, std::min(N1, N2));
+}
+
+// like std::strlen but it will not read beyond the bound of str
+// if str is not null-terminated, returns sizeof(str)
+template <std::size_t N>
+static inline std::size_t strlen_arr(const char (&str)[N]) {
+  for (std::size_t i = 0; i < N; ++i) {
+    if (str[i] == '\0') {
+      return i;
+    }
+  }
+  return N;
+}
+
+// like std::sprintf but will not write beyond the bound of dest
+// dest is guaranteed to be null-terminated
+template <std::size_t N>
+static inline int sprintf_arr(char (&dest)[N], const char* format, ...) {
+  std::va_list vargs;
+  va_start(vargs, format);
+  int retval = std::vsnprintf(dest, N, format, vargs);
+  va_end(vargs);
+  return retval;
+}
+
+// like std::strcat but will not write beyond the bound of dest
+// dest is guaranteed to be null-terminated
+template <std::size_t N>
+static inline char* strcat_arr(char (&dest)[N], const char* src) {
+  return std::strncat(dest, src, sizeof_arr(dest) - strlen_arr(dest) - 1);
+}
+
+// like std::strcpy but won't write beyond the bound of dest
+// dest is guaranteed to be null-terminated
+template <std::size_t N>
+static inline char* strcpy_arr(char (&dest)[N], const char* src) {
+  {
+    std::size_t i = 0;
+    for (; src[i] && i < N - 1; ++i) {
+      dest[i] = src[i];
+    }
+    dest[i] = '\0';
+  }
+  return &dest[0];
+}
+
+}  // namespace sample_util
+}  // namespace mujoco
+
+#endif  // MUJOCO_SAMPLE_ARRAY_SAFETY_H_

simulate/main.cc

@@ -0,0 +1,221 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <cstdio>
+#include <cstring>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include <mjxmacro.h>
+#include "uitools.h"
+#include "simulate.h"
+
+#include "array_safety.h"
+
+namespace {
+namespace mj = ::mujoco;
+namespace mju = ::mujoco::sample_util;
+
+// constants
+const double syncmisalign = 0.1;    // maximum time mis-alignment before re-sync
+const double refreshfactor = 0.7;   // fraction of refresh available for simulation
+
+// model and data
+mjModel* m = nullptr;
+mjData* d = nullptr;
+
+// control noise variables
+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;
+}
+
+// simulate in background thread (while rendering in main thread)
+void simulate_thread(void) {
+  // cpu-sim syncronization point
+  double cpusync = 0;
+  mjtNum simsync = 0;
+
+  // run until asked to exit
+  while (!GetInstance().exitrequest) {
+    // 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) {
+      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());
+
+      // run only if model is present
+      if (m) {
+        // running
+        if (GetInstance().run) {
+          // record cpu time at start of iteration
+          double tmstart = glfwGetTime();
+
+          // inject noise
+          if (GetInstance().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);
+
+            for (int i=0; i<m->nu; i++) {
+              // update noise
+              ctrlnoise[i] = rate * ctrlnoise[i] + scale * mju_standardNormal(nullptr);
+              // apply noise
+              d->ctrl[i] = ctrlnoise[i];
+            }
+          }
+
+          // 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) {
+            // re-sync
+            cpusync = tmstart;
+            simsync = d->time*GetInstance().slow_down;
+            GetInstance().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);
+
+            // run single step, let next iteration deal with timing
+            mj_step(m, d);
+          }
+
+          // 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) {
+              // 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);
+
+              // run mj_step
+              mjtNum prevtm = d->time*GetInstance().slow_down;
+              mj_step(m, d);
+
+              // break on reset
+              if (d->time*GetInstance().slow_down<prevtm) {
+                break;
+              }
+            }
+          }
+        }
+
+        // paused
+        else {
+          // apply pose perturbation
+          mjv_applyPerturbPose(m, d, &GetInstance().pert, 1);      // move mocap and dynamic bodies
+
+          // run mj_forward, to update rendering and joint sliders
+          mj_forward(m, d);
+        }
+      }
+    } // end exclusive access
+  }
+}
+} // end unnamed namespace
+
+//---------------------------------- main -------------------------------------------------
+
+// run event loop
+int main(int argc, const char** argv) {
+  // print version, check compatibility
+  std::printf("MuJoCo version %s\n", mj_versionString());
+  if (mjVERSION_HEADER!=mj_version()) {
+    mju_error("Headers and library have different versions");
+  }
+
+  // request loadmodel if file given (otherwise drag-and-drop)
+  if (argc>1) {
+    mju::strcpy_arr(GetInstance().filename, argv[1]);
+    GetInstance().loadrequest = 2;
+  }
+
+  // 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();
+  }
+
+  // stop simulation thread
+  GetInstance().exitrequest = 1;
+  simthread.join();
+
+  // 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)
+  glfwTerminate();
+#endif
+
+  return 0;
+}

simulate/simulate.h

@@ -0,0 +1,167 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MUJOCO_SIMULATE_H_
+#define MUJOCO_SIMULATE_H_
+
+#include "uitools.h"
+
+namespace mujoco {
+
+//-------------------------------- global -----------------------------------------------
+
+// Simulate states not contained in MuJoCo structures
+class Simulate {
+ public:
+  // create object and initialize the simulate ui
+  Simulate(void);
+
+  // load mjb or xml model
+  void loadmodel(void);
+
+  // prepare to render
+  void prepare(void);
+
+  // render the ui to the window
+  void render(void);
+
+  // clear callbacks registered in external structures
+  void clearcallback(void);
+
+  // constants
+  static constexpr int kMaxFilenameLength = 1000;
+
+  // model and data to be visualized
+  mjModel* m;
+  mjData* d;
+
+  // file
+  int exitrequest = 0;
+
+  // option
+  int spacing = 0;
+  int color = 0;
+  int font = 0;
+  int ui0_enable = 1;
+  int ui1_enable = 1;
+  int help = 0;
+  int info = 0;
+  int profiler = 0;
+  int sensor = 0;
+  int fullscreen = 0;
+  int vsync = 1;
+  int busywait = 0;
+
+  // simulation
+  int run = 1;
+  int key = 0;
+  int loadrequest = 0;
+  // strings
+  char loadError[kMaxFilenameLength] = "";
+  char filename[kMaxFilenameLength] = "";
+  char previous_filename[kMaxFilenameLength] = "";
+  int slow_down = 1;
+  bool speed_changed = true;
+  double ctrlnoisestd = 0.0;
+  double ctrlnoiserate = 0.0;
+
+  // watch
+  char field[mjMAXUITEXT] = "qpos";
+  int index = 0;
+
+  // physics: need sync
+  int disable[mjNDISABLE];
+  int enable[mjNENABLE];
+
+  // rendering: need sync
+  int camera = 0;
+
+  // abstract visualization
+  mjvScene scn;
+  mjvCamera cam;
+  mjvOption vopt;
+  mjvPerturb pert;
+  mjvFigure figconstraint;
+  mjvFigure figcost;
+  mjvFigure figtimer;
+  mjvFigure figsize;
+  mjvFigure figsensor;
+
+  // OpenGL rendering and UI
+  GLFWvidmode vmode;
+  int windowpos[2];
+  int windowsize[2];
+  mjrContext con;
+  GLFWwindow* window;
+  mjuiState uistate;
+  mjUI ui0, ui1;
+
+  // Constant arrays needed for the option section of UI and the UI interface
+  // TODO setting the size here is not ideal
+  const mjuiDef defOption[14] = {
+    {mjITEM_SECTION,   "Option",        1, nullptr,              "AO"},
+    {mjITEM_SELECT,    "Spacing",       1, &this->spacing,       "Tight\nWide"},
+    {mjITEM_SELECT,    "Color",         1, &this->color,         "Default\nOrange\nWhite\nBlack"},
+    {mjITEM_SELECT,    "Font",          1, &this->font,          "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
+    {mjITEM_CHECKINT,  "Left UI (Tab)", 1, &this->ui0_enable,    " #258"},
+    {mjITEM_CHECKINT,  "Right UI",      1, &this->ui1_enable,    "S#258"},
+    {mjITEM_CHECKINT,  "Help",          2, &this->help,          " #290"},
+    {mjITEM_CHECKINT,  "Info",          2, &this->info,          " #291"},
+    {mjITEM_CHECKINT,  "Profiler",      2, &this->profiler,      " #292"},
+    {mjITEM_CHECKINT,  "Sensor",        2, &this->sensor,        " #293"},
+  #ifdef __APPLE__
+    {mjITEM_CHECKINT,  "Fullscreen",    0, &this->fullscreen,    " #294"},
+  #else
+    {mjITEM_CHECKINT,  "Fullscreen",    1, &this->fullscreen,    " #294"},
+  #endif
+    {mjITEM_CHECKINT,  "Vertical Sync", 1, &this->vsync,         ""},
+    {mjITEM_CHECKINT,  "Busy Wait",     1, &this->busywait,      ""},
+    {mjITEM_END}
+  };
+
+
+  // simulation section of UI
+  const mjuiDef defSimulation[12] = {
+    {mjITEM_SECTION,   "Simulation",    1, nullptr,              "AS"},
+    {mjITEM_RADIO,     "",              2, &this->run,           "Pause\nRun"},
+    {mjITEM_BUTTON,    "Reset",         2, nullptr,              " #259"},
+    {mjITEM_BUTTON,    "Reload",        2, nullptr,              "CL"},
+    {mjITEM_BUTTON,    "Align",         2, nullptr,              "CA"},
+    {mjITEM_BUTTON,    "Copy pose",     2, nullptr,              "CC"},
+    {mjITEM_SLIDERINT, "Key",           3, &this->key,           "0 0"},
+    {mjITEM_BUTTON,    "Load key",      3},
+    {mjITEM_BUTTON,    "Save key",      3},
+    {mjITEM_SLIDERNUM, "Noise scale",   2, &this->ctrlnoisestd,  "0 2"},
+    {mjITEM_SLIDERNUM, "Noise rate",    2, &this->ctrlnoiserate, "0 2"},
+    {mjITEM_END}
+  };
+
+
+  // watch section of UI
+  const mjuiDef defWatch[5] = {
+    {mjITEM_SECTION,   "Watch",         0, nullptr,              "AW"},
+    {mjITEM_EDITTXT,   "Field",         2, this->field,          "qpos"},
+    {mjITEM_EDITINT,   "Index",         2, &this->index,         "1"},
+    {mjITEM_STATIC,    "Value",         2, nullptr,              " "},
+    {mjITEM_END}
+  };
+
+  // info strings
+  char info_title[Simulate::kMaxFilenameLength];
+  char info_content[Simulate::kMaxFilenameLength];
+};
+
+}  // namespace mujoco
+
+#endif

simulate/uitools.c

@@ -226,6 +226,17 @@ static void uiResize(GLFWwindow* wnd, int width, int height) {
 }
 
 
+static void uiRender(GLFWwindow* wnd) {
+  uiUserPointer* ptr = (uiUserPointer*)glfwGetWindowUserPointer(wnd);
+  mjuiState* state = ptr->state;
+  ptr->uiRender(state);
+}
+
+static void uiDrop(GLFWwindow* wnd, int count, const char** paths) {
+  uiUserPointer* ptr = (uiUserPointer*)glfwGetWindowUserPointer(wnd);
+  mjuiState* state = ptr->state;
+  ptr->uiDrop(state, count, paths);
+}
 
 //----------------------------------- Public API ----------------------------------------
 
@@ -262,12 +273,15 @@ int uiFontScale(GLFWwindow* wnd) {
 
 // Set internal and user-supplied UI callbacks in GLFW window.
 void uiSetCallback(GLFWwindow* wnd, mjuiState* state,
-                   uiEventFn uiEvent, uiLayoutFn uiLayout) {
+                   uiEventFn uiEvent, uiLayoutFn uiLayout,
+                   uiRenderFn uiUserRender, uiDropFn uiUserDrop) {
   // make container with user-supplied objects and set window pointer
   uiUserPointer* ptr = (uiUserPointer*) mju_malloc(sizeof(uiUserPointer));
   ptr->state = state;
   ptr->uiEvent = uiEvent;
   ptr->uiLayout = uiLayout;
+  ptr->uiRender = uiUserRender;
+  ptr->uiDrop = uiUserDrop;
   glfwSetWindowUserPointer(wnd, ptr);
 
   // compute framebuffer-to-window pixel ratio
@@ -282,6 +296,8 @@ void uiSetCallback(GLFWwindow* wnd, mjuiState* state,
   glfwSetMouseButtonCallback(wnd, uiMouseButton);
   glfwSetScrollCallback(wnd, uiScroll);
   glfwSetWindowSizeCallback(wnd, uiResize);
+  glfwSetWindowRefreshCallback(wnd, uiRender);
+  glfwSetDropCallback(wnd, uiDrop);
 }
 
 
@@ -300,6 +316,8 @@ void uiClearCallback(GLFWwindow* wnd) {
   glfwSetMouseButtonCallback(wnd, NULL);
   glfwSetScrollCallback(wnd, NULL);
   glfwSetWindowSizeCallback(wnd, NULL);
+  glfwSetWindowRefreshCallback(wnd, NULL);
+  glfwSetDropCallback(wnd, NULL);
 }
 
 

simulate/uitools.h

@@ -19,6 +19,7 @@
 #include <GLFW/glfw3.h>
 #include <mujoco/mujoco.h>
 
+
 // this is a C-API
 #if defined(__cplusplus)
 extern "C" {
@@ -28,19 +29,24 @@ extern "C" {
 // User-supplied callback function types.
 typedef void (*uiEventFn)(mjuiState* state);
 typedef void (*uiLayoutFn)(mjuiState* state);
+typedef void (*uiRenderFn)(mjuiState* state);
+typedef void (*uiDropFn)  (mjuiState* state, int count, const char** paths);
 
 // Container for GLFW window pointer.
 struct _uiUserPointer {
   mjuiState* state;
   uiEventFn uiEvent;
   uiLayoutFn uiLayout;
+  uiRenderFn uiRender;
+  uiDropFn   uiDrop;
   double buffer2window;
 };
 typedef struct _uiUserPointer uiUserPointer;
 
 // Set internal and user-supplied UI callbacks in GLFW window.
 void uiSetCallback(GLFWwindow* wnd, mjuiState* state,
-                   uiEventFn uiEvent, uiLayoutFn uiLayout);
+                   uiEventFn uiEvent, uiLayoutFn uiLayout,
+                   uiRenderFn uiUserRender, uiDropFn uiUserDrop);
 
 // Clear UI callbacks in GLFW window.
 void uiClearCallback(GLFWwindow* wnd);