Mujoco C++ : Visionneuse de scène minimale avec navigation à la souris et simulation

// minimal_viewer.cpp
// Compilation : voir les notes ci-dessous

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

// --- Globales réduites pour les callbacks GLFW ---
static mjModel* m = nullptr;
static mjData*  d = nullptr;

static mjvCamera cam;       // caméra abstraite
static mjvOption opt;       // options de visualisation
static mjvScene  scn;       // scène abstraite
static mjrContext con;      // contexte GPU personnalisé

static bool  button_left = false, button_middle = false, button_right = false;
static double lastx = 0, lasty = 0;
// Synchronisation temps réel : enregistrer les temps de départ de simulation et d'horloge
static double sim_start_time = 0.0;
static double realtime_start = 0.0;

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

// --- Bouton de la souris : mémoriser quels boutons sont enfoncés ---
static void mouse_button_callback(GLFWwindow* window, int button, int act, int /*mods*/) {
  // Gestion simple et stable de l'état des boutons
  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);

  // Enregistrer la position actuelle du curseur (coordonnées fenêtre)
  glfwGetCursorPos(window, &lastx, &lasty);
}

// --- Déplacement de la souris : convertir le mouvement en action de caméra ---
static void cursor_pos_callback(GLFWwindow* window, double xpos, double ypos) {
  if (!m) return;
  // Si aucun bouton de la souris n'est enfoncé, juste mettre à jour la dernière position
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // Calculer le mouvement en coordonnées fenêtre, normaliser par la hauteur du framebuffer (stable selon la DPI)
  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) {
    // Glisser-gauche : rotation
    mjv_moveCamera(m, mjMOUSE_ROTATE_H, dx, dy, &scn, &cam);
  } else if (button_right) {
    // Glisser-droit : déplacement horizontal
    mjv_moveCamera(m, mjMOUSE_MOVE_H, dx, dy, &scn, &cam);
  } else if (button_middle) {
    // Glisser-milieu : zoom
    mjv_moveCamera(m, mjMOUSE_ZOOM, dx, dy, &scn, &cam);
  }
}

// --- Molette de défilement : zoom ---
static void scroll_callback(GLFWwindow* window, double /*xoffset*/, double yoffset) {
  if (!m) return;
  // Défilement pour zoomer (facteur doux)
  mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
}

// --- Touche : 'R' pour réinitialiser ---
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) {
      // Réinitialiser l'état de simulation et les minuteurs temps réel pour que la sim n'avance pas trop
      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) {
  // Utilisation
  if (argc < 2) {
    std::printf("Utilisation : %s modele.xml\n", argv[0]);
    return 1;
  }

  // (MuJoCo open-source >= 2.2 n'a pas d'étape d'activation de licence)

  // Charger le modèle
  char error[1024] = {0};
  m = mj_loadXML(argv[1], nullptr, error, sizeof(error));
  if (!m) {
    std::fprintf(stderr, "Impossible de charger le modèle : %s\n", error);
    return 1;
  }
  d = mj_makeData(m);

  // Initialiser GLFW
  glfwSetErrorCallback(glfw_error_callback);
  if (!glfwInit()) {
    std::fprintf(stderr, "Impossible d'initialiser GLFW\n");
    return 1;
  }

  // Créer la fenêtre/le contexte
  glfwWindowHint(GLFW_DOUBLEBUFFER, 1);
  GLFWwindow* window = glfwCreateWindow(1200, 900, "MuJoCo Minimal Viewer", nullptr, nullptr);
  if (!window) {
    std::fprintf(stderr, "Impossible de créer la fenêtre GLFW\n");
    glfwTerminate();
    return 1;
  }
  glfwMakeContextCurrent(window);
  glfwSwapInterval(0); // désactiver vsync

  // Définir les callbacks
  glfwSetMouseButtonCallback(window, mouse_button_callback);
  glfwSetCursorPosCallback(window, cursor_pos_callback);
  glfwSetScrollCallback(window, scroll_callback);
  glfwSetKeyCallback(window, key_callback);

  // Initialiser la visualisation MuJoCo
  mjv_defaultCamera(&cam);
  // Définir le point de visée de la caméra pour que la vue initiale soit centrée sur le modèle
  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);               // allouer la scène
  mjr_makeContext(m, &con, mjFONTSCALE_100);  // allouer le contexte GPU

  // Initialiser les minuteurs de synchronisation temps réel (démarrer les deux à la même heure d'horloge)
  sim_start_time = d->time;
  realtime_start = glfwGetTime();

  // Placer la caméra pour afficher tout le modèle
  cam.type = mjCAMERA_FREE;
  mj_forward(m, d);

  // Boucle principale
  while (!glfwWindowShouldClose(window)) {
    // Faire avancer la simulation (pas simple approximativement temps réel)
    mj_step(m, d);

    // --- Synchronisation temps réel : s'assurer que la sim ne va pas plus vite que l'horloge ---
    double sim_elapsed = d->time - sim_start_time;                 // secondes de simulation depuis le départ/la réinitialisation
    double real_elapsed = glfwGetTime() - realtime_start;          // secondes d'horloge depuis le départ/la réinitialisation
    double sleep_time = sim_elapsed - real_elapsed;                // positif => la sim est en avance
    if (sleep_time > 0.0) {
      // Dormir le temps nécessaire (nanosleep est sûr sans options de linker de threading)
      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);
    }

    // Obtenir la taille du framebuffer et définir le viewport
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    mjrRect viewport = {0, 0, width, height};

    // Mettre à jour la scène et rendre
    mjv_updateScene(m, d, &opt, nullptr, &cam, mjCAT_ALL, &scn);
    mjr_render(viewport, &scn, &con);

    // Échanger et sonder
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

  // Nettoyage
  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