# LLM-MCP Genesis：用大模型驱动的机器人仿真

通过 [Model Context Protocol](https://modelcontextprotocol.io) 把 [Genesis](https://github.com/Genesis-Embodied-AI/Genesis) 物理仿真器暴露给 LLM Agent，让模型能够以多轮对话的方式连续操控同一个仿真场景——搭建场景、规划抓取、执行倒水任务，并观察结果后再决定下一步。

任务域：**机械臂抓取杯子并倒水**（刚体 + SPH 液体耦合）。

---

## 演示

<!-- ══════════════════════════════════════════════════════════════════════
     维护说明 —— 如何替换下面的主演示视频

     GitHub 无法播放仓库内的 mp4（写相对路径只会渲染成下载链接）。
     内嵌播放器只能用 user-attachments 托管的 URL：

       1. 打开 https://github.com/1douya1/genesis-mcp-robotics/issues/new
       2. 把 mp4 拖进正文编辑框，等进度条走完
       3. 编辑框会生成一行 https://github.com/user-attachments/assets/...
       4. 把该 URL 填进下面 <video> 的 src
       5. 关掉 issue 页面即可，不需要真的提交（附件已存在 CDN 上）

     单文件上限 100MB。width 属性控制播放器宽度；若某天 GitHub 不再
     渲染 <video> 标签，退回到「URL 单独成行」的写法也能出播放器，
     只是宽度不可控。

     调整时长/体积（当前源片 1200x842，131 秒）：
       ffmpeg -i in.mp4 -vf "setpts=PTS/2" -an -c:v libx264 -crf 25 \
              -preset slow -pix_fmt yuv420p -movflags +faststart out.mp4
     ══════════════════════════════════════════════════════════════════════ -->

一次完整的对话驱动仿真：Agent 从空场景开始，逐步搭建环境、定位杯子、规划抓取、执行倾倒，每一步都读取上一步的返回值再决定下一步——全程在同一个常驻的物理世界里完成，没有任何一次重启。

<div align="center">
  <video src="https://github.com/user-attachments/assets/3968b589-b4ac-471a-b0d6-8a4a476b03e7" width="820" controls></video>
</div>

### 案例

<table>
<tr>
<td width="50%" align="center">
<img src="docs/media/gif/llm_pouring_flow.gif" width="100%"><br>
<b>LLM 驱动的完整倒水流程</b><br>
<sub>Agent 连续调用工具：建场景 → 抓取 → 倾倒</sub>
</td>
<td width="50%" align="center">
<img src="docs/media/gif/auto_grasp_pipeline.gif" width="100%"><br>
<b>全自动抓取管线</b><br>
<sub>IK 求解 + 路径规划 + 夹爪闭合</sub>
</td>
</tr>
<tr>
<td width="50%" align="center">
<img src="docs/media/gif/early_version.gif" width="100%"><br>
<b>早期版本（对照）</b><br>
<sub>姿态约束加入前，夹爪易脱手</sub>
</td>
<td width="50%" align="center">
<img src="docs/media/gif/uf850_grasp.gif" width="100%"><br>
<b>UF850 抓取</b><br>
<sub>可运行，但稳定性欠佳 —— 见<a href="#已知问题">已知问题</a></sub>
</td>
</tr>
</table>

<sub>以上为 GIF 预览（已加速、降帧、缩放）。原始 mp4 见 <a href="docs/media/">docs/media/</a>；完整实验录像归档保留在本地，未纳入版本控制。</sub>

---

## 核心设计：持久化会话

常规 MCP 工具是无状态的——每次调用都要重建仿真、重开渲染窗口，LLM 无法做连续决策。

本项目的 [`GenesisSession`](mcp_server/simple_genesis_mcp.py) 让 `gs.init()` 全局只执行一次、场景与查看器窗口常驻，实体和相机注册在会话字典里。于是 LLM 可以：

```
create_simulation_scene()          → 建场景（此时先不 build）
add_cup_with_liquid()              → 往同一场景加实体
create_complete_scene_with_sphere()→ 补齐后统一 build
execute_robot_action(...)          → 抓取，观察返回值
debug_robot_grasp_status()         → 发现没夹稳
execute_pour_sequence(...)         → 调整参数后倒水
```

全程同一个窗口、同一个物理世界。场景的 `build()` 被延迟到实体添加完毕（Genesis 要求 build 后不能再加实体），由 `build_scene_if_needed()` 统一处理。

---

## 目录结构

```
mcp_server/          MCP 服务器（项目核心）
  simple_genesis_mcp.py    最终版，1682 行，11 个 tool + 2 个 resource
  start_server_simple.sh   启动脚本
  debug_mcp.py             环境诊断服务器（排查 "Not connected"）
  test_mcp.py              最小化连通性测试
  legacy/                  演化历史，见下文

experiments/         仿真实验脚本（MCP 工具的原型来源）
  01_water_sim/            液体仿真：SPH vs MPM 求解器对比
  02_grasping_franka/      Franka Panda 抓取
  03_grasping_uf850/       UFactory UF850 抓取
  04_ik_study/             IK 与坐标系研究
  05_pouring/              倒水任务
  scenes/                  场景搭建

assets/
  objects/                 杯子等网格资产（obj/mtl/blend）
  robots/uf850/            UF850 URDF + xarm_description 网格

docs/                替换配置、故障排除、演示视频
media/               完整视频归档（已 gitignore，本地保留）
```

---

## MCP 接口

**Tools**

| 分类 | 工具 | 说明 |
|---|---|---|
| 场景 | `create_simulation_scene(show_viewer)` | 建场景 + 地面 + Franka，暂不 build |
| | `add_cup_with_liquid()` | 加杯子与 SPH 液体 |
| | `add_test_sphere(...)` | 加测试球体（液体的轻量替代） |
| | `create_complete_scene_with_sphere(show_viewer)` | 一步建完整场景并 build |
| 执行 | `execute_robot_action(...)` | IK 求解 + 运动执行，含路径规划降级 |
| | `execute_pour_sequence(...)` | 完整倒水序列（接近→抓取→抬起→移动→倾倒） |
| 调试 | `debug_robot_grasp_status()` | 夹爪开合、接触力、物体位姿 |
| | `test_wrist_rotation(...)` | 单独验证腕部旋转 |
| | `test_extreme_pour(...)` | 极限倾角测试 |
| 管理 | `get_simulation_status()` | 会话统计与实体清单 |
| | `reset_simulation()` | 重置场景 |

**Resources**：`genesis://status/session`、`genesis://help/persistent`

**两个关键的运动辅助函数**（工具背后的实现）：
- `_move_with_fallback()` — 优先用 Genesis 的路径规划器，失败则降级为关节空间直接插值，避免规划器报错导致整个工具调用失败
- `_move_with_fixed_orientation()` — 倒水时锁定腕部以外的姿态，防止 IK 解算把杯口转歪

---

## 快速开始

> ⚠️ **本项目锁定 `genesis-world==0.2.1`。** 请勿直接升级到 Genesis 1.x，先读下面的[兼容性说明](#兼容性说明)。

```bash
# 1. 环境（Genesis 需要 GPU）
conda create -n genesis310 python=3.10
conda activate genesis310
pip install -r requirements.txt

# 2. 直接跑服务器
cd mcp_server && python simple_genesis_mcp.py

# 3. 或接入 MCP 客户端（Cursor / Claude Desktop）
```

客户端配置（`~/.cursor/mcp.json`）：

```json
{
  "mcpServers": {
    "genesis-robotics": {
      "command": "/绝对路径/llm-mcp-genesis/mcp_server/start_server_simple.sh",
      "args": [],
      "env": { "CONDA_DEFAULT_ENV": "genesis310" }
    }
  }
}
```

完整配置说明见 [docs/mcp_setup.md](docs/mcp_setup.md)，连接问题见 [docs/troubleshooting.md](docs/troubleshooting.md)。

资产路径默认按仓库相对定位，也可用 `GENESIS_ASSET_DIR` 环境变量覆盖。

---

## 实验记录

### 01 液体仿真
对比 SPH 与 MPM 两种求解器模拟杯中水。`Water_cup_Success.py` 是收敛的配置：`dt=2e-3`、`substeps=100`、`particle_size=0.002`。子步数不足时液体会穿透杯壁。

### 02 Franka 抓取
`Opus_try.py` 里的 `RobotControlWrapper` 把控制频率（20ms）与仿真步长（2ms）解耦，避免每个仿真步都重设目标导致的抖动。`grasp-optimization.py` 是抓取位姿的搜索实现，后来演化成了 MCP 里的抓取逻辑。

### 03 UF850 抓取 ⚠️
从 `xarm_ros2` 把 UFactory 850 的 xacro 展开成 `uf850_final.urdf` 导入 Genesis。**能跑通，但仿真效果较差**——关节响应和抓取稳定性都不理想，尚未定位是 URDF 的惯量/摩擦参数问题还是控制增益问题。保留作为后续修复的起点。

### 04 IK 研究
`Robotics_Test.py` 梳理 Genesis 中 position/quaternion 的坐标约定；`Test_IK_Multi.py` 用多链接 IK 约束夹爪保持水平，这是抓取稳定性的关键改进。

### 05 倒水
渐进式验证：`Pouring_No_Liquids.py`（空杯先验证轨迹）→ `Pouring_Sphere.py`（小球代替液体，快得多）→ `Pouring_Liquids_IK.py`（真 SPH 液体）。

---

## 服务器演化历史（`mcp_server/legacy/`）

| 文件 | 行数 | 说明 |
|---|---|---|
| `callback.py` | 857 | 最早原型，6 个 tool |
| `Pouring_Test.py` | 929 | 倒水分支 |
| `pour_liquid_tool.py` | 304 | 倒水工具片段 |
| `genesis_mcp_backup_V2.py` | 2062 | 功能最全，13 个 tool（含独立的 `control_gripper`/`get_gripper_status`） |
| `simple_genesis_mcp_V1.py` | 1846 | 精简中间版 |

保留作为设计过程的记录。注意 V1 和 V2 中 `execute_pour_sequence` 被重复定义了两次（后者覆盖前者），该问题在最终版中已修复——这也是判断哪个版本是最新的依据。

在此之前还有一版基于 `dustland/genesis-mcp` 的分层重构（Pydantic 模型 + service 层），设计上更规范，但没有完成，已弃用。

---

## 兼容性说明

**本项目开发于 `genesis-world==0.2.1`（2025年5月），并锁定在该版本。**

上游此后改名为 [`Genesis-Embodied-AI/genesis-world`](https://github.com/Genesis-Embodied-AI/genesis-world)，底层编译器从 Taichi 换成自研的 Quadrants，并已发布到 1.x。截至 1.2.3 与本项目的差异核对如下。

### 会直接报错

**相机录制 API 的参数位置变了**（影响 `experiments/` 下 21 个调用点；MCP 服务器本身不录制视频，不受影响）：

```python
# 0.2.1（本项目的写法）
cam.start_recording()
cam.stop_recording(save_to_filename='out.mp4', fps=60)

# 1.x
cam.start_recording(save_to_filename='out.mp4', fps=60)
cam.stop_recording()          # 不再接受参数
```

### 不报错但行为改变

**`set_quat` 的默认值变了**（影响 10 个实验脚本 + `mcp_server/simple_genesis_mcp.py` 的 3 处）：

```python
# 0.2.1:  set_quat(quat, envs_idx=None, *, zero_velocity=True,  unsafe=False)
# 1.x:    set_quat(quat, envs_idx=None, *, zero_velocity=False, relative=True, skip_forward=False)
```

`zero_velocity` 默认由 True 变 False；新增的 `relative=True` 使 quat 改为在用户坐标系下解释，morph 的位姿偏移与惯量对齐会叠加上去。本项目的杯子都是以 `gs.morphs.Mesh(..., euler=(-np.pi/2, 0, 0))` 加载后再 `set_quat` 纠正朝向的，因此升级后杯子朝向需要重新校准。恢复旧语义需显式传 `relative=False, zero_velocity=True`。

**`plan_path` 默认值变了**：`resolution` 0.01→0.05、`timeout` 5.0→None、`num_waypoints` 100→300，`ignore_joint_limit` 参数被移除。本项目的调用均使用关键字参数，不会报错，但规划出的路径会不同。

**物理行为的累积变化**（不需改代码，但仿真结果会变）：

| 版本 | 变化 | 对本项目的影响 |
|---|---|---|
| v0.4.4 | 自由关节惯量轴对齐、MJCF 默认 armature 修正、Rigid 默认密度调整 | 抓取参数需重调 |
| v1.0.0 | 非凸多点接触碰撞检测 | **利好**——杯子用的正是 `convexify=False`，杯口结构不再被凸包吞掉 |
| v1.2.0 | 运动树改为深度优先解析 | 代码中 `motors_dof = np.arange(7)` 等硬编码 DOF 索引需复核，UF850 的并联夹爪连杆尤其要确认 |
| v1.2.1 / v1.2.2 | 复合连杆质心对齐、非凸碰撞检测修正 | 液体与接触参数需重调 |

### 未变化（已核对）

`gs.init(backend=gs.gpu)`、`gs.euler_to_quat`、`quat_to_xyz` / `quat_to_R`、`SimOptions(dt, substeps, requires_grad)`、`SPHOptions(lower_bound, upper_bound, particle_size)`、`materials.Rigid(rho, friction, needs_coup, coup_friction, coup_restitution)`、`SPH.Liquid(rho, gamma, sampler='pbs')`、`morphs.Mesh(convexify, decimate)`、`inverse_kinematics(link=, pos=, quat=)`、`Rasterizer` / `RayTracer` —— 均仍可用。

IK 调用全部使用关键字参数，因此 1.x 在 `quat` 与 `init_qpos` 之间新增的 `local_point` 参数不影响本项目。Python 3.10 仍在支持范围内（1.x 要求 `>=3.10,<3.14`）。

### 关于 LuisaRender

LuisaRender 是 Genesis 的光线追踪渲染后端，对应 `gs.renderers.RayTracer`。从源码构建 Genesis 时它需要本地编译（约 2.6G 产物）。本项目绝大多数场景使用 `Rasterizer`，仅一处用到 `RayTracer`，因此**通过 pip 安装即可，无需自行编译 LuisaRender**。1.x 另有自研渲染器 Nyx（独立包 `gs-nyx`），LuisaRender 降为三条渲染路径之一。

### 升级路线

若要迁移到 1.x，建议顺序：① 替换 21 处 `stop_recording` → ② 给 `set_quat` 补显式参数并重新校准杯子朝向 → ③ 复核 UF850 的 DOF 索引 → ④ 重调液体 `substeps` 与抓取参数。前两步是机械修改，后两步是本项目参数敏感性的主要工作量。

---

## 已知问题

- UF850 仿真质量差（见上）
- 液体仿真对 `substeps` 极度敏感，参数迁移到新场景时通常需要重调
- 抓取成功率依赖杯子网格的 `convexify` 设置，凸包简化会丢失杯口结构

## License

实验性研究代码。Genesis 采用 Apache-2.0，`xarm_description` 网格来自 [xArm-Developer/xarm_ros2](https://github.com/xArm-Developer/xarm_ros2)（BSD-3-Clause）。