MuJoCo + GLFW 示例：使用 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>

// --- 为 GLFW 回调保留的全局变量，保持精简 ---
static mjModel* m = nullptr;
static mjData*  d = nullptr;

static mjvCamera cam;       // 抽象相机
static mjvOption opt;       // 可视化选项
static mjvScene  scn;       // 抽象场景
static mjrContext con;      // 自定义 GPU 上下文

static bool  button_left = false, button_middle = false, button_right = false;
static double lastx = 0, lasty = 0;
// 实时同步：记录仿真和挂钟时间的起始时刻
static double sim_start_time = 0.0;
static double realtime_start = 0.0;

// --- 滑块的简单 UI 状态 ---
struct SliderDef {
  int jnt;           // 该滑块组所属的关节索引
  int comp;          // 关节内的分量：铰链/滑动为 -1，球关节为 0..2（欧拉角），自由关节为 0..5（xyz, rpy）
  double vmin;       // 滑块最小值（单位：m 或 rad）
  double vmax;       // 滑块最大值
  double value;      // 当前值
  std::string label; // 标签（在此最小化 UI 中不作为文本渲染）
};

static std::vector<SliderDef> g_sliders;   // 所有滑块

// 工具函数：限幅
template <typename T>
static inline T clamp(T v, T lo, T hi) { return std::max(lo, std::min(v, hi)); }

// 四元数辅助函数（ZYX 偏航-俯仰-翻滚约定）
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 顺序：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);
  // 翻滚（绕 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);
  // 俯仰（绕 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);
  // 偏航（绕 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);
}

// 根据模型和当前数据构建滑块
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) {
      // 位置 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"});
    }
  }
}

// 将滑块值应用到 qpos 并执行正向运动学
static void apply_sliders_to_qpos() {
  if (!m || !d) return;
  // 从当前 qpos 开始
  // 对每个关节写入值
  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) {
      // 查找该关节的三个滑块
      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);
}

// 无自定义叠加层；ImGui 将处理所有 UI 渲染和交互

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

// --- 鼠标按键：记录哪些按键被按下 ---
static void mouse_button_callback(GLFWwindow* window, int button, int act, int /*mods*/) {
  // 简单、稳定的按键状态处理
  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);

  // 记录当前光标位置（窗口坐标）
  glfwGetCursorPos(window, &lastx, &lasty);
}

// --- 鼠标移动：将运动转换为相机操作 ---
static void cursor_pos_callback(GLFWwindow* window, double xpos, double ypos) {
  if (!m) return;
  // 如果没有鼠标按键被按下，仅更新最后位置
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // 如果 ImGui 需要鼠标，则不移动相机
  if (ImGui::GetIO().WantCaptureMouse) {
    lastx = xpos; lasty = ypos;
    return;
  }

  // 计算窗口坐标中的运动量，按帧缓冲高度归一化（在不同 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) {
    // 左键拖动：旋转
    mjv_moveCamera(m, mjMOUSE_ROTATE_H, dx, dy, &scn, &cam);
  } else if (button_right) {
    // 右键拖动：水平平移
    mjv_moveCamera(m, mjMOUSE_MOVE_H, dx, dy, &scn, &cam);
  } else if (button_middle) {
    // 中键拖动：缩放
    mjv_moveCamera(m, mjMOUSE_ZOOM, dx, dy, &scn, &cam);
  }
}

// --- 滚轮缩放 ---
static void scroll_callback(GLFWwindow* window, double /*xoffset*/, double yoffset) {
  if (!m) return;
  // 滚轮缩放（温和系数）
  mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
}

// --- 按键：'R' 重置 ---
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) {
      // 重置仿真状态和实时计时器，避免仿真跳到前面
      mj_resetData(m, d);
      sim_start_time = (d ? d->time : 0.0);
      realtime_start = glfwGetTime();
  // 从重置状态重新构建并同步滑块
  build_sliders();
  apply_sliders_to_qpos();
    }
    if (key == GLFW_KEY_ESCAPE) glfwSetWindowShouldClose(window, GLFW_TRUE);
  }
}

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

  // （开源 MuJoCo >= 2.2 无需许可证激活步骤）

  // 加载模型
  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
  glfwSetErrorCallback(glfw_error_callback);
  if (!glfwInit()) {
    std::fprintf(stderr, "Could not initialize GLFW\n");
    return 1;
  }

  // 创建窗口/上下文
  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

  // 设置回调
  glfwSetMouseButtonCallback(window, mouse_button_callback);
  glfwSetCursorPosCallback(window, cursor_pos_callback);
  glfwSetScrollCallback(window, scroll_callback);
  glfwSetKeyCallback(window, key_callback);

  // 初始化 MuJoCo 可视化
  mjv_defaultCamera(&cam);
  // 设置相机注视点，使初始视图居中于模型
  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);               // 分配场景
  mjr_makeContext(m, &con, mjFONTSCALE_100);  // 分配 GPU 上下文

  // 初始化实时同步计时器（两者在同一挂钟时间开始）
  sim_start_time = d->time;
  realtime_start = glfwGetTime();

  // 从模型构建初始滑块
  build_sliders();
  apply_sliders_to_qpos();

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

  // 放置相机以显示整个模型
  cam.type = mjCAMERA_FREE;
  mj_forward(m, d);

  // 主循环
  while (!glfwWindowShouldClose(window)) {
  // 仿真已禁用：直接使用滑块设置的 qpos 并保持 mj_forward 更新
  // 如果有外部因素改变了 d->qpos，可以每帧重新应用；开销很小
  apply_sliders_to_qpos();

    // 获取帧缓冲大小并设置视口
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    mjrRect viewport = {0, 0, width, height};

    // 更新场景并渲染
    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();

    // 应用滑块更改并在上层渲染 UI
    apply_sliders_to_qpos();
    ImGui::Render();
    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());

    // 交换缓冲并轮询事件
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

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

# 在支持 GLVND 的系统上优先使用 GLVND。如果需要传统加载器，请在
# cmake 命令行或缓存中将 OpenGL_GL_PREFERENCE 设置为 "LEGACY"。
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