# Franka 抓取立方体

## 概述

本文档详细描述了基于 Franka Emika Panda 机械臂的抓取立方体任务环境。

```{video} /_static/videos/franka_lift_cube.mp4
:poster: _static/images/poster/franka_lift_cube.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：左指关节
    -   finger_joint2：右指关节
-   **末端执行器（TCP）**：夹爪中心点，用于抓取操作

### 任务目标

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

1. **接近目标**：从初始位置移动到立方体位置
2. **抓取立方体**：闭合夹爪抓取立方体
3. **提升立方体**：将立方体提升到目标高度
4. **精确定位**：将立方体移动到指定的目标位置（XYZ 三维坐标）

环境提供可视化辅助：

-   **立方体**：可抓取的红色立方体，初始位于桌面上的随机位置
-   **目标位置**：任务指定的立方体最终应到达的三维位置

---

## 动作空间

动作空间为 `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）

### 关节位置限制

所有关节位置在执行后被限制在以下范围内：

| 关节 | 最小值  | 最大值  |
| ---- | ------- | ------- |
| 1    | -2.8973 | 2.8973  |
| 2    | -1.7628 | 1.7628  |
| 3    | -2.8973 | 2.8973  |
| 4    | -3.0718 | -0.0698 |
| 5    | -2.8973 | 2.8973  |
| 6    | -0.0175 | 3.7525  |
| 7    | -π/2    | π/2     |
| 夹爪 | 0       | 0.04    |

---

## 观测空间

观测空间为 `Box(-inf, inf, (36,), float32)`，包含机器人的本体感知信息、物体状态和历史动作。

### 观测组成部分

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

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

-   7 个机械臂关节相对于默认姿态的角度偏移
-   2 个夹爪关节角度

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

-   9 个关节的角速度

3. **立方体当前位姿（9 维）**

-   位置（3 维）：[x, y, z]
-   四元数（4 维）：[qx, qy, qz, qw]
-   旋转（Euler，2 维）：[roll, pitch]

4. **目标位置命令（7 维）**

-   目标 XYZ 坐标（3 维）
-   目标四元数（4 维）

5. **上一时刻动作（8 维）**

### 观测详细说明

| 编号  | 观测内容                    | 维度 | 单位   |
| ----- | --------------------------- | ---- | ------ |
| 0-8   | 关节角度偏移（9 个关节）    | 9    | rad    |
| 9-17  | 关节角速度（9 个关节）      | 9    | rad/s  |
| 18-26 | 立方体当前位姿（位置+姿态） | 9    | rad    |
| 27-33 | 目标位置命令（位置+四元数） | 7    | 无量纲 |
| 34-41 | 上一时刻动作（8 维）        | 8    | 无量纲 |

---

## 奖励函数

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

### 主要奖励项

1. **接近奖励**（权重：1.5）

-   计算公式：`1.5 × (1 - tanh(d_hand_cube / 0.1))`
-   激励机器人末端接近立方体
-   d_hand_cube：末端到立方体的欧氏距离

2. **提升奖励**（权重：30）

-   条件：立方体高度 > 0.04m 且末端与立方体距离 < 0.05m
-   激励机器人抓取并提升立方体

3. **目标跟踪奖励**（变权重）

-   **粗略跟踪**（权重：10）：使用 Sigmoid 函数，中心距离 0.3m
-   **精细跟踪**（权重：20）：使用 tanh 函数，比例系数 0.4m
-   **接近奖励**（权重：10）：距离 < 0.2m 时使用，比例系数 0.05m
-   **接近加成**（权重：200）：额外奖励，鼓励接近目标
-   所有跟踪奖励仅在立方体高度 > 0.04m 且抓取成功时生效

### 惩罚项

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

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

### 计算公式

```

动作变化率 = ||current_action - last_action||²
关节速度平方和 = ||joint_vel||²

```

---

## 初始状态

### 机器人初始化

**位置初始化：**

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

-   基座位置：固定在桌面上
-   关节角度：设置为默认姿态，并添加随机噪声

**关节角度噪声：**

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

**速度初始化：**

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

### 立方体初始化

立方体在桌面上的位置随机采样：

-   X 坐标：`[-0.1, 0.1]`
-   Y 坐标：`[-0.25, 0.25]`
-   Z 坐标：固定为 0.05 桌面以上）

### 目标位置生成

目标位置在以下范围内随机采样：

-   X 坐标：`[0.4, 0.6]`
-   Y 坐标：`[-0.25, 0.25]`
-   Z 坐标：`[0.25, 0.5]`

---

## 使用方法

### 训练

```bash
uv run scripts/train.py --env franka-lift-cube
```

### 策略评估

```bash
uv run scripts/play.py --env franka-lift-cube
```

### TensorBoard

```bash
uv run tensorboard --logdir runs/franka-lift-cube
```
