# Franka 打开抽屉

## 概述

本文档详细描述了基于 Franka Emika Panda 机械臂的打开抽屉任务环境。

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

---

## 环境描述

Franka 打开柜门任务环境基于真实的 Franka Emika Panda 7 自由度机械臂构建，旨在训练机器人接近柜门把手、抓取并拉开抽屉。

### 机器人结构

Franka Emika Panda 是一个 7 自由度机械臂，由以下主要部分组成：

-   **基座（Base）**：固定在地面上的机器人基座
-   **7 个关节**：
    -   joint1 ~ joint4：肩部和臂部旋转关节
    -   joint5 ~ joint7：手腕部旋转关节
-   **夹爪（Gripper）**：双指夹爪，包含两个手指关节
    -   finger_joint1：左指关节，带有接触垫（left_finger_pad）
    -   finger_joint2：右指关节，带有接触垫（right_finger_pad）
-   **末端执行器（TCP）**：夹爪中心点，用于抓取操作

### 环境物体

-   **柜子**：包含一个可打开的抽屉
-   **抽屉把手（drawer_top_handle）**：机器人需要抓取的目标部位
-   **抽屉关节（drawer_top_joint）**：抽屉的滑动关节，1 个自由度

### 任务目标

机器人需要完成以下操作目标：

1. **接近把手**：从初始位置移动到抽屉把手位置
2. **姿态对齐**：调整末端姿态与把手对齐
3. **抓取把手**：闭合夹爪抓取抽屉把手
4. **拉开抽屉**：向后拉动抽屉将其打开

---

## 动作空间

动作空间为 `Box(-inf, inf, (8,), float32)`，表示施加到 8 个关节的位置控制指令（相对于当前关节位置的偏移量）。

### 控制模式

环境使用位置控制模式，动作通过以下方式转换为关节目标位置：

```
目标关节角度 = 当前关节角度 + 动作值
```

### 动作维度详细说明

| 编号 | 动作说明         | 控制范围   | 对应关节名称   | 关节类型 |
| ---- | ---------------- | ---------- | -------------- | -------- |
| 0    | 关节 1 偏移量    | -inf ~ inf | joint1         | revolve  |
| 1    | 关节 2 偏移量    | -inf ~ inf | joint2         | hinge    |
| 2    | 关节 3 偏移量    | -inf ~ inf | joint3         | hinge    |
| 3    | 关节 4 偏移量    | -inf ~ inf | joint4         | hinge    |
| 4    | 关节 5 偏移量    | -inf ~ inf | joint5         | hinge    |
| 5    | 关节 6 偏移量    | -inf ~ inf | joint6         | hinge    |
| 6    | 关节 7 偏移量    | -inf ~ inf | joint7         | hinge    |
| 7    | 夹爪动作（概率） | -inf ~ inf | finger_joint\* | hinge    |

### 夹爪控制

夹爪动作使用概率控制：

1. **Sigmoid 映射**：将动作值映射到 [0, 1] 区间的概率

    ```
    p = 1 / (1 + exp(-action))
    ```

2. **伯努利采样**：根据概率 p 进行随机采样
    - 采样结果 < p：夹爪闭合（0.0）
    - 采样结果 >= p：夹爪打开（0.04）

---

## 观测空间

观测空间为 `Box(-5, 5, (25,), float32)`，包含机器人的本体感知信息、任务相关信息和抽屉状态。

### 观测组成部分

观测向量由以下部分组成（按顺序）：

1. **关节角度（8 维）**

    - 7 个机械臂关节角度（归一化到 [-1, 1]）
    - 归一化公式：`2 × (关节角度 - 下限) / (上限 - 下限) - 1`

2. **关节速度（8 维）**

    - 8 个关节的角速度（除以 2 进行缩放）

3. **目标相对位姿（7 维）**

    - 位置偏差（3 维）：把手位置 - 末端位置 [Δx, Δy, Δz]
    - 姿态偏差（4 维）：把手姿态 - 末端姿态（四元数）

4. **抽屉关节位置（1 维）**

    - 抽屉当前打开距离

5. **抽屉关节速度（1 维）**
    - 抽屉当前打开速度

### 观测详细说明

| 编号  | 观测内容                   | 维度 | 范围         | 单位   |
| ----- | -------------------------- | ---- | ------------ | ------ |
| 0-7   | 归一化关节角度（8 个关节） | 8    | [-1, 1]      | 无量纲 |
| 8-15  | 归一化关节速度（8 个关节） | 8    | ≈[-π/2, π/2] | rad/s  |
| 16-18 | 到把手的相对位置           | 3    | [-5, 5]      | m      |
| 19-22 | 到把手的相对姿态（四元数） | 4    | [-5, 5]      | 无量纲 |
| 23    | 抽屉关节位置               | 1    | [-5, 5]      | m      |
| 24    | 抽屉关节速度               | 1    | [-5, 5]      | m/s    |

所有观测值被裁剪在 [-5, 5] 范围内以保持数值稳定性。

---

## 奖励函数

奖励函数采用复合设计，包含多个奖励和惩罚项。

### 主要奖励项

1. **距离奖励**（权重：10）

    - 计算公式：`10 × (1 - tanh(d_gripper_handle / 0.1))`
    - 激励机器人末端接近抽屉把手
    - d_gripper_handle：末端到把手的欧氏距离

2. **姿态匹配奖励**

    - 计算公式：四元数相似度函数
    - 激励机器人末端姿态与把手姿态对齐

3. **夹爪闭合奖励**（条件奖励）

    - 当距离 < 0.025m：闭合夹爪获得 +100 奖励
    - 当距离 >= 0.025m：闭合夹爪获得 -20 惩罚
    - 打开夹爪：无奖励（0）
    - 激励机器人在接近时闭合夹爪抓取

4. **打开抽屉奖励**（指数奖励）

    - 计算公式：`20 × (exp(open_dist) - 1)`
    - open_dist：抽屉打开距离（裁剪在 [0, 1] 范围）
    - 随着抽屉打开程度增加，奖励指数增长

5. **防止非法打开**
    - 当抽屉已打开（open_dist > 0）但末端未接触把手（距离 > 0.03m）时，取消打开奖励
    - 防止机器人使用其他方式强行打开抽屉

### 惩罚项

1. **动作变化率惩罚**

    - 计算公式：`||current_action - last_action||²`

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

    - 计算公式：`||joint_vel||²`

3. **手指位置穿透惩罚**
    - 当手指接触垫低于把手表面时施加惩罚
    - 防止手指模型穿透抽屉

### 惩罚系数调度

惩罚项系数随训练进度调整：

| 惩罚项         | 前期权重（steps < 8000） | 后期权重（steps >= 8000） |
| -------------- | ------------------------ | ------------------------- |
| 动作变化率     | 1e-3                     | 2e-3                      |
| 关节速度平方和 | 0                        | 2e-7                      |

### 终止惩罚

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

---

## 初始状态

### 机器人初始化

**位置初始化：**

机器人在世界坐标系中的初始位置固定：

-   基座位置：固定在地面上
-   关节角度：设置为默认姿态

**默认关节姿态：**

```
[0.0, -30°, 0°, -156°, 0.0, 186°, -45°, 0.04, 0.04] (弧度)
```

**关节角度噪声：**

每个关节角度在 `[-0.125, 0.125]` 弧度范围内添加均匀随机噪声。

**速度初始化：**

所有线速度和角速度初始化为零。

### 柜子初始化

柜子固定在地面上，抽屉处于关闭状态（关节位置为 0）。

---

## 使用方法

### 训练

```bash
uv run scripts/train.py --env franka-open-cabinet
```

### 策略评估

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

### TensorBoard

```bash
uv run tensorboard --logdir runs/franka-open-cabinet
```
