Mujoco + GLFW : Exemple avec des curseurs d'espace articulaire utilisant Dear imgui

git submodule add https://github.com/ocornut/imgui.git

#include <mujoco/mujoco.h>
#include <GLFW/glfw3.h>
#include <GL/gl.h>
#include "imgui/imgui.h"
#include "imgui/backends/imgui_impl_glfw.h"
#include "imgui/backends/imgui_impl_opengl2.h"
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <time.h>
#include <vector>
#include <string>
#include <algorithm>

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

// --- État UI simple pour les curseurs ---
struct SliderDef {
  int jnt;           // index d'articulation auquel ce groupe de curseurs appartient
  int comp;          // composante dans l'articulation : -1 pour pivot/glissière, 0..2 Euler pour rotule, 0..5 (xyz,rpy) pour libre
  double vmin;       // valeur minimale du curseur (unités : m ou rad)
  double vmax;       // valeur maximale du curseur
  double value;      // valeur actuelle
  std::string label; // étiquette (non affichée comme texte dans cette UI minimale)
};

static std::vector<SliderDef> g_sliders;   // tous les curseurs

// Utilitaires : clamp
template <typename T>
static inline T clamp(T v, T lo, T hi) { return std::max(lo, std::min(v, hi)); }

// Aides pour les quaternions (convention ZYX lacet-tangage-roulis)
static void quat_normalize(double q[4]) {
  double n = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
  if (n <= 0) { q[0]=1; q[1]=q[2]=q[3]=0; return; }
  q[0]/=n; q[1]/=n; q[2]/=n; q[3]/=n;
}

static void euler_rpy_to_quat(double roll, double pitch, double yaw, double q[4]) {
  double cr = std::cos(roll*0.5),  sr = std::sin(roll*0.5);
  double cp = std::cos(pitch*0.5), sp = std::sin(pitch*0.5);
  double cy = std::cos(yaw*0.5),   sy = std::sin(yaw*0.5);
  // Ordre ZYX : q = Rz(yaw)*Ry(pitch)*Rx(roll)
  q[0] = cr*cp*cy + sr*sp*sy; // w
  q[1] = sr*cp*cy - cr*sp*sy; // x
  q[2] = cr*sp*cy + sr*cp*sy; // y
  q[3] = cr*cp*sy - sr*sp*cy; // z
  quat_normalize(q);
}

static void quat_to_euler_rpy(const double qin[4], double& roll, double& pitch, double& yaw) {
  double q[4] = {qin[0], qin[1], qin[2], qin[3]};
  quat_normalize(q);
  // roulis (rotation axe x)
  double sinr_cosp = 2.0*(q[0]*q[1] + q[2]*q[3]);
  double cosr_cosp = 1.0 - 2.0*(q[1]*q[1] + q[2]*q[2]);
  roll = std::atan2(sinr_cosp, cosr_cosp);
  // tangage (axe y)
  double sinp = 2.0*(q[0]*q[2] - q[3]*q[1]);
  if (std::abs(sinp) >= 1.0) pitch = std::copysign(M_PI/2.0, sinp);
  else pitch = std::asin(sinp);
  // lacet (axe z)
  double siny_cosp = 2.0*(q[0]*q[3] + q[1]*q[2]);
  double cosy_cosp = 1.0 - 2.0*(q[2]*q[2] + q[3]*q[3]);
  yaw = std::atan2(siny_cosp, cosy_cosp);
}

// Construire les curseurs à partir du modèle et des données actuelles
static void build_sliders() {
  g_sliders.clear();
  if (!m || !d) return;
  for (int j=0; j<m->njnt; ++j) {
    int type = m->jnt_type[j];
    int qadr = m->jnt_qposadr[j];
    const char* name = (m->names && m->name_jntadr) ? m->names + m->name_jntadr[j] : "joint";
    if (type == mjJNT_HINGE) {
      double vmin = -M_PI, vmax = M_PI;
      if (m->jnt_limited[j]) { vmin = m->jnt_range[2*j]; vmax = m->jnt_range[2*j+1]; }
      g_sliders.push_back({j, -1, vmin, vmax, d->qpos[qadr], std::string(name)});
    } else if (type == mjJNT_SLIDE) {
      double vmin = -1.0, vmax = 1.0;
      if (m->jnt_limited[j]) { vmin = m->jnt_range[2*j]; vmax = m->jnt_range[2*j+1]; }
      g_sliders.push_back({j, -1, vmin, vmax, d->qpos[qadr], std::string(name)});
    } else if (type == mjJNT_BALL) {
      double q[4] = { d->qpos[qadr+0], d->qpos[qadr+1], d->qpos[qadr+2], d->qpos[qadr+3] };
      double r,p,y; quat_to_euler_rpy(q, r,p,y);
      g_sliders.push_back({j, 0, -M_PI, M_PI, r, std::string(name)+".roll"});
      g_sliders.push_back({j, 1, -M_PI, M_PI, p, std::string(name)+".pitch"});
      g_sliders.push_back({j, 2, -M_PI, M_PI, y, std::string(name)+".yaw"});
    } else if (type == mjJNT_FREE) {
      // position xyz
      g_sliders.push_back({j, 0, -2.0, 2.0, d->qpos[qadr+0], std::string(name)+".x"});
      g_sliders.push_back({j, 1, -2.0, 2.0, d->qpos[qadr+1], std::string(name)+".y"});
      g_sliders.push_back({j, 2, -2.0, 2.0, d->qpos[qadr+2], std::string(name)+".z"});
      double q[4] = { d->qpos[qadr+3], d->qpos[qadr+4], d->qpos[qadr+5], d->qpos[qadr+6] };
      double r,p,y; quat_to_euler_rpy(q, r,p,y);
      g_sliders.push_back({j, 3, -M_PI, M_PI, r, std::string(name)+".roll"});
      g_sliders.push_back({j, 4, -M_PI, M_PI, p, std::string(name)+".pitch"});
      g_sliders.push_back({j, 5, -M_PI, M_PI, y, std::string(name)+".yaw"});
    }
  }
}

// Appliquer les valeurs des curseurs à qpos et cinématique directe
static void apply_sliders_to_qpos() {
  if (!m || !d) return;
  // Partir du qpos actuel
  // Pour chaque articulation, écrire les valeurs
  for (size_t i=0; i<g_sliders.size(); ++i) {
    const SliderDef& s = g_sliders[i];
    int type = m->jnt_type[s.jnt];
    int qadr = m->jnt_qposadr[s.jnt];
    if (type == mjJNT_HINGE || type == mjJNT_SLIDE) {
      d->qpos[qadr] = s.value;
    } else if (type == mjJNT_BALL) {
      // Trouver les trois curseurs pour cette articulation
      double r=0,p=0,y=0;
      for (const auto& t : g_sliders) if (t.jnt==s.jnt) {
        if (t.comp==0) r=t.value; else if (t.comp==1) p=t.value; else if (t.comp==2) y=t.value;
      }
      double q[4]; euler_rpy_to_quat(r,p,y,q);
      d->qpos[qadr+0]=q[0]; d->qpos[qadr+1]=q[1]; d->qpos[qadr+2]=q[2]; d->qpos[qadr+3]=q[3];
    } else if (type == mjJNT_FREE) {
      double pos[3]; double r=0,p=0,y=0;
      for (const auto& t : g_sliders) if (t.jnt==s.jnt) {
        if (t.comp==0) pos[0]=t.value; else if (t.comp==1) pos[1]=t.value; else if (t.comp==2) pos[2]=t.value;
        else if (t.comp==3) r=t.value; else if (t.comp==4) p=t.value; else if (t.comp==5) y=t.value;
      }
      double q[4]; euler_rpy_to_quat(r,p,y,q);
      d->qpos[qadr+0]=pos[0]; d->qpos[qadr+1]=pos[1]; d->qpos[qadr+2]=pos[2];
      d->qpos[qadr+3]=q[0];  d->qpos[qadr+4]=q[1];  d->qpos[qadr+5]=q[2];  d->qpos[qadr+6]=q[3];
    }
  }
  mj_forward(m, d);
}

// Pas d'overlay personnalisé ; ImGui gérera tout le rendu et les interactions de l'UI

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

  // Si ImGui veut la souris, ne pas déplacer la caméra
  if (ImGui::GetIO().WantCaptureMouse) {
    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();
  // Reconstruire et resynchroniser les curseurs depuis l'état réinitialisé
  build_sliders();
  apply_sliders_to_qpos();
    }
    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();

  // Construire les curseurs initiaux à partir du modèle
  build_sliders();
  apply_sliders_to_qpos();

  // --- Configuration ImGui ---
  IMGUI_CHECKVERSION();
  ImGui::CreateContext();
  ImGuiIO& io = ImGui::GetIO(); (void)io;
  ImGui::StyleColorsDark();
  ImGui_ImplGlfw_InitForOpenGL(window, true);
  ImGui_ImplOpenGL2_Init();

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

  // Boucle principale
  while (!glfwWindowShouldClose(window)) {
  // Simulation désactivée : utiliser directement le qpos défini par les curseurs et maintenir mj_forward à jour
  // Si quelque chose d'externe a modifié d->qpos, on pourrait réappliquer à chaque image ; assez peu coûteux
  apply_sliders_to_qpos();

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

    // UI ImGui
    ImGui_ImplOpenGL2_NewFrame();
    ImGui_ImplGlfw_NewFrame();
    ImGui::NewFrame();

    ImGui::SetNextWindowPos(ImVec2(10, 10), 0);
    ImGui::SetNextWindowSize(ImVec2(350, std::min(700, height-20)), 0);
    if (ImGui::Begin("Joints", nullptr, 0)) {
      for (size_t i=0; i<g_sliders.size(); ++i) {
        SliderDef& s = g_sliders[i];
        float v = (float)s.value;
        float vmin = (float)s.vmin;
        float vmax = (float)s.vmax;
        std::string lab = s.label;
        if (ImGui::SliderFloat(lab.c_str(), &v, vmin, vmax)) {
          s.value = v;
        }
      }
    }
    ImGui::End();

    // Appliquer les changements des curseurs et rendre l'UI par-dessus
    apply_sliders_to_qpos();
    ImGui::Render();
    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());

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

  // Nettoyage
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  mj_deleteData(d);
  mj_deleteModel(m);
  // Arrêt ImGui
  ImGui_ImplOpenGL2_Shutdown();
  ImGui_ImplGlfw_Shutdown();
  ImGui::DestroyContext();
  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)

# Préférer GLVND sur les systèmes qui le supportent. Si vous avez besoin du chargeur legacy, définissez
# OpenGL_GL_PREFERENCE à "LEGACY" sur la ligne de commande cmake ou dans le cache.
set(OpenGL_GL_PREFERENCE "GLVND" CACHE STRING "Préférer GLVND ou LEGACY pour le chargeur OpenGL")

find_package(OpenGL REQUIRED)

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} ${CMAKE_CURRENT_SOURCE_DIR}/imgui ${CMAKE_CURRENT_SOURCE_DIR}/imgui/backends)

set(IMGUI_SOURCES
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/imgui.cpp
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/imgui_draw.cpp
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/imgui_tables.cpp
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/imgui_widgets.cpp
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/backends/imgui_impl_glfw.cpp
	${CMAKE_CURRENT_SOURCE_DIR}/imgui/backends/imgui_impl_opengl2.cpp
)

target_sources(mujocotest PRIVATE ${IMGUI_SOURCES})
target_link_libraries(mujocotest PRIVATE ${MUJOCO_LIBRARIES} ${GLFW_LIBRARIES} OpenGL::GL)

mkdir build
cmake .
make
./mujocotest ~/mujoco_menagerie/franka_fr3/fr3.xml