# Demo 采集说明

本目录包含从 SceneSmith 生成场景中采集的 pick-and-place demo。

## Demo 列表

| 文件 | 任务 | 场景 |
|------|------|------|
| `demo_bedroom_nightstand_to_desk.json` | 从床头柜取物放到书桌 | 卧室 |
| `demo_bedroom_nightstand_to_desk_trajectory.png` | 轨迹可视化 | 卧室 |
| `demo_bedroom_wardrobe_to_desk.json` | 从衣柜附近取物放到书桌 | 卧室 |
| `demo_bedroom_wardrobe_to_desk_trajectory.png` | 轨迹可视化 | 卧室 |

## 可视化说明

每张 `*_trajectory.png` 包含 4 个子图：

1. **3D 轨迹**：末端执行器路径，颜色区分阶段
   - 蓝色 = approach（接近）
   - 橙色 = pre_grasp（下降）
   - 红色 = grasp（抓取）
   - 紫色 = lift（提升）
   - 绿色 = transport（搬运）
   - 黄色 = place（放置）

2. **俯视图 (XY)**：任务路径平面投影

3. **高度时间曲线**：末端执行器（蓝实线）和物体（红虚线）Z轴变化

4. **夹爪宽度**：0 = 完全闭合，1 = 完全张开

## Demo JSON 格式

```json
{
  "task": "Pick an object from the nightstand and place it on the desk",
  "scene_id": "bedroom_nightstand_to_desk",
  "target_object": "bedroom_nightstand_0",
  "initial_pos": [-0.55, -1.73, 0.5],
  "goal_pos": [0.79, 1.58, 0.85],
  "goal_reference": "bedroom_desk_0",
  "success": true,
  "total_duration_s": 7.5,
  "num_steps": 150,
  "trajectory": [
    {
      "step_id": 0,
      "phase": "approach",
      "end_effector_pos": [x, y, z],
      "end_effector_quat_wxyz": [w, x, y, z],
      "gripper_width": 1.0,
      "target_object_pos": [x, y, z],
      "timestamp": 0.0
    }
  ]
}
```

## 重要说明

本次 demo 是**脚本化轨迹**（scripted demo），不是真实 robot 采集：
- 轨迹是几何插值（直线 + 竖直升降），不来自 robot controller
- 没有物理碰撞避免
- 没有力矩控制

SceneSmith 官方不提供 robot URDF 和 policy。真实 demo 采集需要：
1. 加载 `sim_files/house.dmd.yaml` 到 Drake / MuJoCo
2. 添加 robot URDF
3. 用 `policy_interface.py` 获取目标位姿
4. 用真实 controller 执行并记录

## 采集方法

```bash
python scripts/collect_demo.py \
    --scene-state scenes/scene_01_bedroom/sim_files/house_state.json \
    --dmd scenes/scene_01_bedroom/sim_files/house.dmd.yaml \
    --task "Pick an object from the nightstand and place it on the desk" \
    --output-dir demos/ \
    --scene-id bedroom_demo
```