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

// --- Simple UI state for sliders ---
struct SliderDef {
  int jnt;           // joint index this slider group belongs to
  int comp;          // component within joint: -1 for hinge/slide, 0..2 Euler for ball, 0..5 (xyz,rpy) for free
  double vmin;       // minimum slider value (units: m or rad)
  double vmax;       // maximum slider value
  double value;      // current value
  std::string label; // label (not rendered as text in this minimal UI)
};

static std::vector<SliderDef> g_sliders;   // all sliders

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

// Quaternion helpers (ZYX yaw-pitch-roll convention)
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 order: 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 (x-axis rotation)
  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-axis)
  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-axis)
  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);
}

// Build sliders from model and current data
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"});
    }
  }
}

// Apply slider values to qpos and forward kinematics
static void apply_sliders_to_qpos() {
  if (!m || !d) return;
  // Start from current qpos
  // For each joint, write values
  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) {
      // Find the three sliders for this joint
      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);
}

// No custom overlay; ImGui will handle all UI rendering and interactions

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

  // Record current cursor position (window coordinates)
  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;
  // If no mouse button is pressed, just update last position
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // If ImGui wants the mouse, don't move camera
  if (ImGui::GetIO().WantCaptureMouse) {
    lastx = xpos; lasty = ypos;
    return;
  }

  // Compute motion in window coordinates, normalize by framebuffer height (stable across 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) {
    // 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;
  // Scroll to zoom (gentle factor)
  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) {
      // Reset simulation state and real-time timers so the sim doesn't jump ahead
      mj_resetData(m, d);
      sim_start_time = (d ? d->time : 0.0);
      realtime_start = glfwGetTime();
  // Rebuild and resync sliders from the reset state
  build_sliders();
  apply_sliders_to_qpos();
    }
    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)

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

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

  // Create window/context
  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

  // Set callbacks
  glfwSetMouseButtonCallback(window, mouse_button_callback);
  glfwSetCursorPosCallback(window, cursor_pos_callback);
  glfwSetScrollCallback(window, scroll_callback);
  glfwSetKeyCallback(window, key_callback);

  // Init MuJoCo visualization
  mjv_defaultCamera(&cam);
  // Set camera look-at point so the initial view is centered on the model
  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

  // Initialize real-time sync timers (start both at the same wall-clock time)
  sim_start_time = d->time;
  realtime_start = glfwGetTime();

  // Build initial sliders from model
  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();

  // Place camera to show entire model
  cam.type = mjCAMERA_FREE;
  mj_forward(m, d);

  // Main loop
  while (!glfwWindowShouldClose(window)) {
  // Simulation disabled: directly use slider-set qpos and keep mj_forward up to date
  // In case something external changed d->qpos, we could re-apply every frame; cheap enough
  apply_sliders_to_qpos();

    // Get framebuffer size and set viewport
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    mjrRect viewport = {0, 0, width, height};

    // Update scene and render
    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();

    // Apply slider changes and render UI on top
    apply_sliders_to_qpos();
    ImGui::Render();
    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());

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

  // Cleanup
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  mj_deleteData(d);
  mj_deleteModel(m);
  // ImGui shutdown
  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)

# Prefer GLVND on systems that support it. If you need the legacy loader, set
# OpenGL_GL_PREFERENCE to "LEGACY" on the cmake command line or in the 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