# RM65 打开抽屉

## 概述

本文档描述 `rm65-open-cabinet` 操作任务环境。该环境使用 RM65 六自由度机械臂与并联夹爪，目标是接近柜体底部抽屉把手、完成抓取并将抽屉拉开。

```{video} /_static/videos/rm65_open_cabinet.mp4
:poster: _static/images/poster/rm65_open_cabinet.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```

---

## 环境描述

该任务基于 RM65 机械臂和柜体抽屉场景构建。虽然环境注册名为 `rm65-open-cabinet`，但当前实际操作对象是柜体底部抽屉，代码中对应目标为 `drawer_bottom_handle` 与 `drawer_bottom_joint`。

### 机器人结构

RM65 机器人在该环境中包含以下主要部分：

-   **基座（base_link）**：固定在工作台前方
-   **6 个机械臂关节**：`joint_1` ~ `joint_6`
-   **并联夹爪**：主驱动关节为 `gripper_Left_1_Joint`，其余 5 个夹爪关节通过联动关系同步
-   **末端执行器（TCP）**：`gripper` 站点，用于计算末端与把手的相对位姿
-   **手指接触点**：`left_finger_pad`、`right_finger_pad`，用于判定是否对准把手以及是否发生穿透

### 场景物体

-   **柜体**：包含多个门和抽屉
-   **目标把手**：`drawer_bottom_handle`
-   **目标关节**：`drawer_bottom_joint`，滑动范围为 `0.0 ~ 0.4 m`

### 任务目标

机器人需要完成以下阶段：

1. **靠近把手**：将末端执行器移动到抽屉把手附近
2. **姿态对齐**：让夹爪姿态与把手姿态匹配
3. **稳定抓取**：闭合夹爪并连续保持抓取
4. **打开抽屉**：沿抽屉滑动方向将底部抽屉拉开

---

## 动作空间

动作空间为 `Box(-inf, inf, (7,), float32)`。

其中前 6 维控制机械臂关节目标，最后 1 维控制夹爪开合。

### 控制模式

-   **机械臂**：默认使用 `joint_target` 模式，并启用归一化目标控制
    机械臂动作会先裁剪到 `[-1, 1]`，再线性映射到各关节控制范围
-   **夹爪**：默认使用 `binary` 模式
    原始动作经过 Sigmoid 映射为闭合概率，再通过带迟滞的二值开关决定夹爪开闭

### 动作维度详细说明

| 编号 | 动作说明     | 原始输入范围  | 实际控制对象           |
| ---- | ------------ | ------------- | ---------------------- |
| 0    | 关节 1 目标  | `(-inf, inf)` | `joint_1`              |
| 1    | 关节 2 目标  | `(-inf, inf)` | `joint_2`              |
| 2    | 关节 3 目标  | `(-inf, inf)` | `joint_3`              |
| 3    | 关节 4 目标  | `(-inf, inf)` | `joint_4`              |
| 4    | 关节 5 目标  | `(-inf, inf)` | `joint_5`              |
| 5    | 关节 6 目标  | `(-inf, inf)` | `joint_6`              |
| 6    | 夹爪开合命令 | `(-inf, inf)` | `gripper_Left_1_Joint` |

### 控制约束

-   控制周期 `ctrl_dt = 0.025s`，对应 40Hz 控制频率
-   机械臂默认启用速度限制、加速度限制、动作延迟与执行器一阶滞后
-   训练时会在每个 episode 内随机化机械臂延迟、滞后、速度上限和加速度上限，以增强 sim2real 鲁棒性
-   夹爪使用迟滞阈值控制：
    -   开启到闭合阈值：`0.78`
    -   闭合到开启阈值：`0.62`
    -   最小切换间隔：`0.25s`

---

## 观测空间

观测空间为 `Box(-inf, inf, (84,), float32)`，最终观测会裁剪到 `[-5, 5]`。

### 观测组成

观测由以下 4 部分拼接而成：

1. **关节位置（7 维）**
    - 6 个机械臂关节
    - 1 个夹爪主驱动关节
    - 统一归一化到 `[-1, 1]`
2. **关节速度（7 维）**
    - 使用相邻两帧关节位置差分估计
    - 再除以 `2` 做缩放
3. **目标相对位姿（7 维）**
    - 末端到把手的相对位置 `Δx, Δy, Δz`
    - 把手相对末端的四元数姿态差
4. **动作历史（63 维）**
    - 最近 `9` 步原始动作历史
    - 每步 `7` 维，共 `9 × 7 = 63` 维

### 观测维度详细说明

| 编号范围 | 内容说明                   | 维度 |
| -------- | -------------------------- | ---- |
| 0-6      | 归一化关节位置             | 7    |
| 7-13     | 关节速度                   | 7    |
| 14-16    | 末端到把手的相对位置       | 3    |
| 17-20    | 末端到把手的相对姿态四元数 | 4    |
| 21-83    | 最近 9 步动作历史          | 63   |

### 观测噪声

该环境默认开启 sim2real 观测扰动，主要包括：

-   关节位置与速度噪声
-   把手位置与姿态噪声
-   把手观测偏置
-   把手观测随机丢帧，并在丢帧时保持上一帧结果

---

## 奖励函数

奖励函数采用分阶段复合设计，既鼓励接近与对齐，也鼓励稳定抓取和持续拉开抽屉。

### 主要奖励项

1. **距离奖励**

    ```python
    dist_reward = 15.0 * (1 - tanh(distance / 0.4))
    ```

    鼓励末端执行器持续靠近把手。

2. **姿态奖励**

    基于末端姿态与把手姿态的四元数相似度计算，并仅在末端距离把手较近时生效。

3. **夹爪闭合奖励**

    当末端距离小于 `0.035m` 且左右手指在把手上下方对齐时，闭合夹爪可获得正奖励；
    否则闭合会受到惩罚。该项还会随夹爪闭合程度缩放。

4. **抽屉打开奖励**

    ```python
    open_reward = (exp(open_dist) - 1.0) * 420.0
    ```

    只有在满足“已经抓住或进入抓取阶段”且末端仍靠近把手时才生效。

5. **抽屉增量奖励**

    对本步新增的打开距离给予额外奖励，鼓励稳定、连续地拉动抽屉。

6. **稳定抓取奖励**

    当末端距离小于 `0.03m`、夹爪闭合比例大于 `0.7`，并持续保持 `6` 步后，环境认为已经抓住把手，并提供持续奖励。

7. **里程碑奖励**

    - 抽屉打开超过 `0.15m` 时奖励 `35`
    - 抽屉打开超过 `0.22m` 时额外奖励 `70`

### 惩罚项

1. **滑脱惩罚**

    如果已经进入抓取阶段但后来失去抓取，同时抽屉已经被拉开，则施加额外惩罚。

2. **手指穿透惩罚**

    当手指接触点穿过把手上下边界时施加惩罚，用于抑制不合理接触。

3. **夹爪频繁切换惩罚**

    当末端靠近把手时，频繁在开合之间切换会触发惩罚。

4. **动作变化惩罚**

    对相邻两步动作差的平方和施加惩罚。

5. **关节速度惩罚**

    对关节速度平方和施加惩罚，后期训练权重更高。

6. **终止惩罚**

    当触发终止条件时，额外施加 `-10.0` 惩罚。

---

## 初始状态

### 机器人初始化

-   机械臂默认关节初值为零位姿
-   夹爪默认处于张开状态
-   复位时默认不添加关节角随机噪声
-   所有关节速度初始化为零

### 场景初始化

-   目标抽屉初始为完全关闭状态
-   抽屉与柜体其余部分固定在场景默认位置

### 随机化内容

每次 reset 会重新采样部分 sim2real 参数，包括：

-   机械臂动作延迟
-   机械臂执行器滞后
-   机械臂速度/加速度上限
-   把手观测偏置

---

## Episode 终止条件

以下任一条件满足时，episode 会提前终止：

1. **TCP 落到把手后方过多**
    - 终止阈值：`tcp_x - handle_x < -0.02`
2. **关节速度过大**
    - 任意机器人关节速度绝对值超过 `3.93 rad/s`

此外，环境最大时长为 `30s`。

---

## 使用方法

### 训练

```bash
uv run scripts/train.py --env rm65-open-cabinet --train-backend torch
```

### 策略评估

```bash
uv run scripts/play.py --env rm65-open-cabinet
```

### TensorBoard

```bash
uv run tensorboard --logdir runs/rm65_open_cabinet
```
