# MindBot Project

基于 Isaac Lab 的移动操作机器人（MindRobot）仿真学习框架，支持强化学习（RL）、模仿学习（IL）和 XR 遥操作。

## 环境要求

- **Isaac Sim**: 5.1.0
- **Python**: 3.11
- **GPU**: NVIDIA RTX PRO 6000 96GB（开发机），推荐开启 DLSS
- **依赖**: isaaclab, isaaclab_assets, isaaclab_mimic, isaaclab_rl, isaaclab_tasks
- **Conda 环境**：
  - `env_isaaclab` — 主开发环境
  - `xr` — XR 遥操作 + 仿真推流环境，包含 isaaclab/isaacsim + PyAV

**IMPORTANT**: 所有 Isaac Lab 脚本必须通过 `~/IsaacLab/isaaclab.sh -p` 启动，直接用 `python` 会报 `KeyError: 'EXP_PATH'`（该变量只有 `isaaclab.sh` 会在启动时设置）。

```bash
# 安装包
~/IsaacLab/isaaclab.sh -p -m pip install -e source/mindbot
```

## 关键环境变量

```bash
export MINDBOT_ASSETS_DIR=/home/tangger/LYT/maic_usd_assets_moudle  # USD 资源路径（默认值）
```

## 常用命令

```bash
# 列出所有注册环境
python scripts/list_envs.py

# 测试环境（零动作 / 随机动作）
python scripts/zero_agent.py --task Template-Mindbot-v0 --num_envs 4
python scripts/random_agent.py --task Template-Mindbot-v0 --num_envs 4

# RL 训练（RSL-RL PPO）
python scripts/rsl_rl/train.py --task Template-Mindbot-v0 --num_envs 4096

# RL 推理
python scripts/rsl_rl/play.py --task Template-Mindbot-v0

# 遥操作（Spacemouse / 键盘）
python scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-MindRobot-LeftArm-IK-Rel-v0

# 遥操作（XR 双臂 + 头部控制）
python scripts/environments/teleoperation/teleop_xr_agent.py --task Isaac-MindRobot-2i-DualArm-IK-Abs-v0

# 遥操作（XR 双臂，无头部）
python scripts/environments/teleoperation/teleop_xr_agent.py --task Isaac-MindRobot-DualArm-IK-Abs-v0

# 遥操作 + 仿真画面推送到 PICO VR（需要 --enable_cameras）
python scripts/environments/teleoperation/teleop_xr_agent.py \
  --task Isaac-MindRobot-2i-DualArm-IK-Abs-v0 \
  --stream-to <PICO_IP> --enable_cameras

# 推流测试（不需要 Isaac Lab，用 xr 环境直接运行）
conda run -n xr python scripts/test_sim_streamer.py --host <PICO_IP>

# 代码格式化
pre-commit run --all-files
```

## 项目架构

```
source/mindbot/mindbot/
├── robot/mindbot.py              # MindRobot 关节/执行器配置（MINDBOT_CFG）
├── utils/
│   ├── assets.py                 # USD 资源路径（MINDBOT_ASSETS_DIR）
│   └── sim_video_streamer.py     # 仿真→VR H264推流模块（PyAV编码 + TCP）
└── tasks/manager_based/
    ├── rl/                       # 强化学习任务
    │   ├── mindbot/              # Template-Mindbot-v0（抓取）
    │   ├── centrifuge/           # Template-centrifuge-lidup-v0
    │   ├── pull/                 # Template-Pull-v0
    │   └── pullUltrasoundLidUp/  # Pull-Ultrasound-Lid-Up-v0
    └── il/                       # 模仿学习任务
        ├── demo/                 # Demo 任务（H1、Franka）
        └── open_drybox/          # 双臂遥操作任务

scripts/environments/teleoperation/
├── teleop_xr_agent.py            # PICO XR 遥操作（双臂/单臂，支持仿真推流）
├── teleop_se3_agent.py           # Spacemouse/键盘 遥操作
└── test_sim_streamer.py          # 推流独立测试（红蓝测试图案）

deps/
├── XRoboToolkit-Teleop-Sample-Python/  # XR SDK Python 封装
├── XRoboToolkit-RobotVision-PC/        # 物理ZED相机采集+H264推流（C++）
└── zed-isaac-sim/                      # ZED 仿真扩展（Isaac Sim OGN）
```

## 注册任务列表

| 任务 ID | 类型 | Action维度 | 说明 |
|---|---|---|---|
| `Template-Mindbot-v0` | RL | - | MindRobot 抓取 |
| `Template-centrifuge-lidup-v0` | RL | - | 离心机旋转 |
| `Template-Pull-v0` | RL | - | 物体拉取 |
| `Pull-Ultrasound-Lid-Up-v0` | RL | - | 超声机盖拉取 |
| `Isaac-MindRobot-LeftArm-IK-Rel-v0` | IL | 12D | 左臂 IK 相对控制 |
| `Isaac-MindRobot-LeftArm-IK-Abs-v0` | IL | 12D | 左臂 IK 绝对控制 |
| `Isaac-MindRobot-DualArm-IK-Abs-v0` | IL | 20D | 双臂 IK（无头部） |
| `Isaac-MindRobot-2i-DualArm-IK-Abs-v0` | IL | 22D | 双臂 IK + 头部（带VR推流相机） |

## XR 遥操作

### teleop_xr_agent.py 功能

- **双臂绝对 IK 控制**：左右 PICO 手柄分别控制左右机械臂
- **Grip 按钮**：按住移动手臂，松开锁定关节（缓存 root-frame IK 命令）
- **左摇杆**：底盘前后/转向（直接速度控制，绕过摩擦限制）
- **左手 Y 键**：重置环境（右手 B 键不触发重置）
- **头部追踪**：HMD 偏航/俯仰映射到机器人头部关节（仅 2i 任务）
- **`--stream-to`**：将仿真立体相机画面实时推送到 PICO VR 显示

### 仿真→VR 推流架构

```
Isaac Lab env.step()
  → vr_left_eye / vr_right_eye (CameraCfg, 1280x720)
  → .cpu().numpy() (GPU→CPU)
  → SimVideoStreamer (后台线程: np.concatenate SBS → PyAV H264 → TCP)
  → PICO Unity Client (TCP Server :12345, H264解码 + VR双眼显示)
```

- TCP 协议：4字节大端长度头 + H264 NALU（与 RobotVisionConsole 相同）
- Unity Client 零修改，选择 SIM-Stereo 或 ZEDMINI 配置均可
- PICO 端 `video_source.yml` 通过 adb push 热更新：
  ```bash
  adb push Assets/StreamingAssets/video_source.yml \
    /sdcard/Android/data/com.xrobotoolkit.client/files/video_source.yml
  adb shell am force-stop com.xrobotoolkit.client
  ```

### 性能优化

- **decimation=4**（25Hz 控制频率，平衡 FPS 和响应性）
- 推流时可关闭调试 viewport 节省 GPU（`--stream-to` 默认关，`--debug-viewports` 开）
- Isaac Sim 开启 **DLSS + Frame Generation** 提升渲染帧率
- 编码在后台线程异步执行，不阻塞主仿真循环

### ZED 2i 物理相机的 IPD 问题

ZED 2i 基线 120mm >> 人眼 IPD 65mm，直接在 VR 中显示会导致所有距离视差过大，无法融合3D。
- Shader `stereoOffset` 只是 toe-in 矫正，只对一个固定距离有效
- PC 端深度矫正（DIBR）理论正确但 artifact 严重
- **根本解决**：在仿真中用 65mm 基线虚拟相机，或使用 ZED Mini（63mm ≈ 人眼 IPD）

## 新增任务的规范

### RL 任务结构
```
tasks/manager_based/rl/<task_name>/
├── __init__.py              # gym.register(id="<TaskId>-v0", ...)
├── <task_name>_env_cfg.py   # ManagerBasedRLEnvCfg 子类
├── agents/
│   └── rsl_rl_ppo_cfg.py   # RslRlOnPolicyRunnerCfg 子类
└── mdp/
    ├── rewards.py
    ├── terminations.py
    └── __init__.py
```

### IL 任务结构
```
tasks/manager_based/il/<task_name>/
├── __init__.py              # gym.register with robomimic_bc_cfg_entry_point
├── <task_name>_env_cfg.py   # ManagerBasedRLEnvCfg 子类（使用 DifferentialIKController）
└── agents/robomimic/
    └── bc_rnn_low_dim.json  # RoboMimic BC 配置
```

新任务的 `__init__.py` 必须在 `mindbot/tasks/manager_based/__init__.py` 中 import 才能注册到 Gym。

## 代码风格

- 文件头使用 SPDX 许可证注释：`# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.`
- 关节角度配置用弧度，注释中可标注对应度数
- 执行器刚度/阻尼参数调整需在注释中说明物理含义

## MindRobot 关节组

| 关节组 | 关节名 |
|---|---|
| 左臂（6 自由度） | l_joint1 ~ l_joint6 |
| 右臂（6 自由度） | r_joint1 ~ r_joint6 |
| 左夹爪 | left_hand_joint_left/right |
| 右夹爪 | right_hand_joint_left/right |
| 躯干升降 | PrismaticJoint |
| 头部 | head_revoluteJoint, head_pitch_Joint |
| 底盘轮子 | front/back revolute joints |

## 遥操作数据收集

- Isaac Lab 内遥操作脚本使用 `xr` 环境（包含 isaaclab + XR SDK）
- XR 设备接入依赖 `deps/XRoboToolkit-Teleop-Sample-Python/`
- 示教数据格式兼容 LeRobot / RoboMimic
- 多相机配置：cam_head, cam_chest, cam_left_hand, cam_right_hand, cam_top, cam_side
- VR 立体相机：vr_left_eye, vr_right_eye（ZED_X CameraLeft/CameraRight）
