Minimaler C++ Mujoco-Szene-Viewer mit Maus-Navigation & Simulation

// minimal_viewer.cpp
// Build: see notes below

#include <mujoco/mujoco.h>
#include <GLFW/glfw3.h>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <time.h>

// --- Globals kept tiny for GLFW callbacks ---
static mjModel* m = nullptr;
static mjData*  d = nullptr;

static mjvCamera cam;       // abstract camera
static mjvOption opt;       // visualization options
static mjvScene  scn;       // abstract scene
static mjrContext con;      // custom GPU context

static bool  button_left = false, button_middle = false, button_right = false;
static double lastx = 0, lasty = 0;
// Real-time sync: record simulation and wall-clock start times
static double sim_start_time = 0.0;
static double realtime_start = 0.0;

// --- GLFW error callback ---
static void glfw_error_callback(int error, const char* description) {
  std::fprintf(stderr, "GLFW error %d: %s\n", error, description);
}

// --- Mouse button: remember which buttons are down ---
static void mouse_button_callback(GLFWwindow* window, int button, int act, int /*mods*/) {
  // Simple, stable button state handling
  if (button == GLFW_MOUSE_BUTTON_LEFT)   button_left   = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_left);
  if (button == GLFW_MOUSE_BUTTON_MIDDLE) button_middle = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_middle);
  if (button == GLFW_MOUSE_BUTTON_RIGHT)  button_right  = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_right);

  // Aktuelle Cursor-Position aufzeichnen (Fensterkoordinaten)
  glfwGetCursorPos(window, &lastx, &lasty);
}

// --- Mouse move: convert motion to camera action ---
static void cursor_pos_callback(GLFWwindow* window, double xpos, double ypos) {
  if (!m) return;
  // Wenn keine Maustaste gedrückt ist, nur letzte Position aktualisieren
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // Bewegung in Fensterkoordinaten berechnen, durch Framebuffer-Höhe normalisieren (stabil über DPI hinweg)
  int fbw = 0, fbh = 1;
  glfwGetFramebufferSize(window, &fbw, &fbh);
  if (fbh <= 0) fbh = 1;

  double dx = (xpos - lastx) / (double)fbh;
  double dy = (ypos - lasty) / (double)fbh;
  lastx = xpos;
  lasty = ypos;

  if (button_left) {
    // Left-drag: rotate
    mjv_moveCamera(m, mjMOUSE_ROTATE_H, dx, dy, &scn, &cam);
  } else if (button_right) {
    // Right-drag: pan horizontally
    mjv_moveCamera(m, mjMOUSE_MOVE_H, dx, dy, &scn, &cam);
  } else if (button_middle) {
    // Middle-drag: zoom
    mjv_moveCamera(m, mjMOUSE_ZOOM, dx, dy, &scn, &cam);
  }
}

// --- Scroll wheel zoom ---
static void scroll_callback(GLFWwindow* window, double /*xoffset*/, double yoffset) {
  if (!m) return;
  // Scrollen zum Zoomen (sanfter Faktor)
  mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
}

// --- Key: 'R' to reset ---
static void key_callback(GLFWwindow* window, int key, int scancode, int act, int mods) {
  if (act == GLFW_PRESS || act == GLFW_REPEAT) {
    if (key == GLFW_KEY_R) {
      // Simulationszustand und Echtzeit-Timer zurücksetzen, damit die Simulation nicht vorspringt
      mj_resetData(m, d);
      sim_start_time = (d ? d->time : 0.0);
      realtime_start = glfwGetTime();
    }
    if (key == GLFW_KEY_ESCAPE) glfwSetWindowShouldClose(window, GLFW_TRUE);
  }
}

int main(int argc, char** argv) {
  // Usage
  if (argc < 2) {
    std::printf("Usage: %s model.xml\n", argv[0]);
    return 1;
  }

  // (Open-source MuJoCo >= 2.2 has no license activation step)

  // Modell laden
  char error[1024] = {0};
  m = mj_loadXML(argv[1], nullptr, error, sizeof(error));
  if (!m) {
    std::fprintf(stderr, "Could not load model: %s\n", error);
    return 1;
  }
  d = mj_makeData(m);

  // GLFW initialisieren
  glfwSetErrorCallback(glfw_error_callback);
  if (!glfwInit()) {
    std::fprintf(stderr, "Could not initialize GLFW\n");
    return 1;
  }

  // Fenster/Kontext erstellen
  glfwWindowHint(GLFW_DOUBLEBUFFER, 1);
  GLFWwindow* window = glfwCreateWindow(1200, 900, "MuJoCo Minimal Viewer", nullptr, nullptr);
  if (!window) {
    std::fprintf(stderr, "Could not create GLFW window\n");
    glfwTerminate();
    return 1;
  }
  glfwMakeContextCurrent(window);
  glfwSwapInterval(0); // disable vsync

  // Callbacks setzen
  glfwSetMouseButtonCallback(window, mouse_button_callback);
  glfwSetCursorPosCallback(window, cursor_pos_callback);
  glfwSetScrollCallback(window, scroll_callback);
  glfwSetKeyCallback(window, key_callback);

  // MuJoCo-Visualisierung initialisieren
  mjv_defaultCamera(&cam);
  // Kamera-Look-at-Punkt setzen, sodass die Anfangsansicht auf das Modell zentriert ist
  cam.lookat[0] = 0.0;
  cam.lookat[1] = 0.0;
  cam.lookat[2] = 0.5;
  mjv_defaultOption(&opt);
  mjv_defaultScene(&scn);
  mjr_defaultContext(&con);

  mjv_makeScene(m, &scn, 2000);               // allocate scene
  mjr_makeContext(m, &con, mjFONTSCALE_100);  // allocate GPU context

  // Echtzeit-Sync-Timer initialisieren (beide zur gleichen Wanduhrzeit starten)
  sim_start_time = d->time;
  realtime_start = glfwGetTime();

  // Kamera platzieren, um das gesamte Modell anzuzeigen
  cam.type = mjCAMERA_FREE;
  mj_forward(m, d);

  // Hauptschleife
  while (!glfwWindowShouldClose(window)) {
    // Simulation einen Schritt weiterführen (einfaches echtzeitähnliches Stepping)
    mj_step(m, d);

    // --- Real-time synchronization: ensure sim does not run faster than wall-clock ---
    double sim_elapsed = d->time - sim_start_time;                 // simulation seconds since start/reset
    double real_elapsed = glfwGetTime() - realtime_start;          // wall-clock seconds since start/reset
    double sleep_time = sim_elapsed - real_elapsed;                // positive => sim is ahead
    if (sleep_time > 0.0) {
      // Für die notwendige Zeit schlafen (nanosleep ist sicher ohne Threading-Linker-Flags)
      struct timespec req;
      req.tv_sec = (time_t)std::floor(sleep_time);
      req.tv_nsec = (long)((sleep_time - req.tv_sec) * 1e9);
      if (req.tv_sec < 0) req.tv_sec = 0;
      if (req.tv_nsec < 0) req.tv_nsec = 0;
      nanosleep(&req, nullptr);
    }

    // Framebuffer-Größe abrufen und Viewport setzen
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    mjrRect viewport = {0, 0, width, height};

    // Szene aktualisieren und rendern
    mjv_updateScene(m, d, &opt, nullptr, &cam, mjCAT_ALL, &scn);
    mjr_render(viewport, &scn, &con);

    // Swap & poll
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

  // Cleanup
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  mj_deleteData(d);
  mj_deleteModel(m);
  glfwTerminate();
  return 0;
}

cmake_minimum_required(VERSION 3.10)
project(mujocotest)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

find_package(PkgConfig REQUIRED)
pkg_search_module(MUJOCO mujoco)
pkg_search_module(GLFW REQUIRED glfw3)

if(NOT MUJOCO_FOUND)
	set(MUJOCO_INCLUDE_DIRS /opt/mujoco/include)
	set(MUJOCO_LIBRARIES /opt/mujoco/lib/libmujoco.so)
endif()

add_executable(mujocotest main.cpp)
target_include_directories(mujocotest PRIVATE ${MUJOCO_INCLUDE_DIRS} ${GLFW_INCLUDE_DIRS})
target_link_libraries(mujocotest PRIVATE ${MUJOCO_LIBRARIES} ${GLFW_LIBRARIES})

cmake .
make -j8

./mujocotest ~/mujoco_menagerie/franka_fr3/fr3.xml