Mujoco + GLFW-Beispiel mit Gelenkraum-Schiebereglern mittels 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>

// --- Globals, klein gehalten für GLFW-Callbacks ---
static mjModel* m = nullptr;
static mjData*  d = nullptr;

static mjvCamera cam;       // abstrakte Kamera
static mjvOption opt;       // Visualisierungsoptionen
static mjvScene  scn;       // abstrakte Szene
static mjrContext con;      // benutzerdefinierter GPU-Kontext

static bool  button_left = false, button_middle = false, button_right = false;
static double lastx = 0, lasty = 0;
// Echtzeit-Synchronisation: Startzeiten von Simulation und Wanduhr aufzeichnen
static double sim_start_time = 0.0;
static double realtime_start = 0.0;

// --- Einfacher UI-Zustand für Schieberegler ---
struct SliderDef {
  int jnt;           // Gelenkindex, zu dem diese Schiebereglergruppe gehört
  int comp;          // Komponente innerhalb des Gelenks: -1 für hinge/slide, 0..2 Euler für ball, 0..5 (xyz,rpy) für free
  double vmin;       // minimaler Schiebereglerwert (Einheiten: m oder rad)
  double vmax;       // maximaler Schiebereglerwert
  double value;      // aktueller Wert
  std::string label; // Beschriftung (in dieser minimalen UI nicht als Text gerendert)
};

static std::vector<SliderDef> g_sliders;   // alle Schieberegler

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

// Quaternion-Hilfsfunktionen (ZYX yaw-pitch-roll-Konvention)
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);
  // ZYX-Reihenfolge: 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);
  // roll (Rotation um x-Achse)
  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);
  // pitch (y-Achse)
  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);
  // yaw (z-Achse)
  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);
}

// Schieberegler aus Modell und aktuellen Daten aufbauen
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"});
    }
  }
}

// Schiebereglerwerte auf qpos anwenden und Vorwärtskinematik berechnen
static void apply_sliders_to_qpos() {
  if (!m || !d) return;
  // Vom aktuellen qpos ausgehen
  // Für jedes Gelenk die Werte schreiben
  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) {
      // Die drei Schieberegler für dieses Gelenk finden
      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);
}

// Kein benutzerdefiniertes Overlay; ImGui übernimmt das gesamte UI-Rendering und die Interaktionen

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

// --- Maustaste: merken, welche Tasten gedrückt sind ---
static void mouse_button_callback(GLFWwindow* window, int button, int act, int /*mods*/) {
  // Einfache, stabile Tastenzustandsbehandlung
  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);
}

// --- Mausbewegung: Bewegung in Kamera-Aktion umwandeln ---
static void cursor_pos_callback(GLFWwindow* window, double xpos, double ypos) {
  if (!m) return;
  // Wenn keine Maustaste gedrückt ist, nur die letzte Position aktualisieren
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // Wenn ImGui die Maus haben will, Kamera nicht bewegen
  if (ImGui::GetIO().WantCaptureMouse) {
    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) {
    // Links-Ziehen: rotieren
    mjv_moveCamera(m, mjMOUSE_ROTATE_H, dx, dy, &scn, &cam);
  } else if (button_right) {
    // Rechts-Ziehen: horizontal verschieben
    mjv_moveCamera(m, mjMOUSE_MOVE_H, dx, dy, &scn, &cam);
  } else if (button_middle) {
    // Mittel-Ziehen: zoomen
    mjv_moveCamera(m, mjMOUSE_ZOOM, dx, dy, &scn, &cam);
  }
}

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

// --- Taste: 'R' zum Zurücksetzen ---
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();
  // Schieberegler aus dem zurückgesetzten Zustand neu aufbauen und synchronisieren
  build_sliders();
  apply_sliders_to_qpos();
    }
    if (key == GLFW_KEY_ESCAPE) glfwSetWindowShouldClose(window, GLFW_TRUE);
  }
}

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

  // (Open-Source-MuJoCo >= 2.2 hat keinen Lizenzaktivierungsschritt)

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

  // 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-Blickpunkt setzen, damit 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);               // Szene allozieren
  mjr_makeContext(m, &con, mjFONTSCALE_100);  // GPU-Kontext allozieren

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

  // Anfangs-Schieberegler aus dem Modell aufbauen
  build_sliders();
  apply_sliders_to_qpos();

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

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

  // Hauptschleife
  while (!glfwWindowShouldClose(window)) {
  // Simulation deaktiviert: direkt den von Schiebereglern gesetzten qpos verwenden und mj_forward aktuell halten
  // Falls etwas externes d->qpos geändert hat, könnten wir jeden Frame neu anwenden; günstig genug
  apply_sliders_to_qpos();

    // Framebuffer-Größe abfragen 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);

    // ImGui-UI
    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();

    // Schieberegler-Änderungen anwenden und UI obendrauf rendern
    apply_sliders_to_qpos();
    ImGui::Render();
    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());

    // Puffern tauschen und Events abfragen
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

  // Aufräumen
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  mj_deleteData(d);
  mj_deleteModel(m);
  // ImGui herunterfahren
  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)

# GLVND auf Systemen bevorzugen, die es unterstützen. Wenn du den Legacy-Loader brauchst, setze
# OpenGL_GL_PREFERENCE auf "LEGACY" in der cmake-Kommandozeile oder im Cache.
set(OpenGL_GL_PREFERENCE "GLVND" CACHE STRING "Prefer GLVND or LEGACY for OpenGL loader")

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