# Shadow Hand 立方体重定向

## 概述

本文档详细描述了基于 Shadow Hand 灵巧手的立方体重定向任务环境。该任务是机器人操作领域的经典基准测试，要求机器人在手中重新定向立方体以匹配随机目标姿态。

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

---

## 环境描述

Shadow Hand 立方体重定向任务基于真实的 Shadow Hand 24 自由度灵巧手构建，旨在训练机器人通过精细的多指协调，将手中的立方体旋转到目标姿态。

### 机器人结构

Shadow Hand 是一个高仿真的 24 自由度灵巧手，由以下主要部分组成：

-   **手掌（Palm）**：手的基础结构，固定在基座上
-   **5 个手指**：
    -   **拇指（Thumb）**：5 自由度，包括 CMC 旋转、MCP、IP 关节
    -   **食指（Index）**：4 自由度，包括 MCP、PIP、DIP 关节
    -   **中指（Middle）**：4 自由度，包括 MCP、PIP、DIP 关节
    -   **无名指（Ring）**：4 自由度，包括 MCP、PIP、DIP 关节
    -   **小指（Little）**：4 自由度，包括 MCP、PIP、DIP 关节
-   **执行器配置**：20 个驱动关节（4 个耦合关节由其他关节驱动）

### 手指关节详情

每个手指（除拇指外）包含：

-   **MCP 关节**：掌指关节，2 自由度（外展/内收 + 屈曲）
-   **PIP 关节**：近端指间关节，1 自由度（屈曲）
-   **DIP 关节**：远端指间关节，1 自由度（屈曲）

拇指包含：

-   **CMC 关节**：腕掌关节，2 自由度
-   **MCP 关节**：掌指关节，1 自由度
-   **IP 关节**：指间关节，1 自由度

### 环境物体

-   **立方体**：50mm × 50mm × 50mm 的正方体
    -   质量：约 0.028 kg
    -   摩擦系数：1.2
    -   初始位置：手掌中心上方 `(0.33, 0.00, 0.295)` 米
-   **目标可视化**：半透明目标姿态指示器（mocap body）

### 任务目标

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

1. **保持抓握**：维持立方体在手中的稳定抓握
2. **感知目标**：观察目标姿态（可视化指示器）
3. **精细操作**：通过多指协调旋转立方体
4. **姿态对齐**：将立方体姿态旋转至目标姿态（容差 ±0.1 弧度）

---

## 动作空间

动作空间为 `Box(-1, 1, (20,), float32)`，表示施加到 20 个驱动关节的位置控制指令（归一化）。

### 动作处理流程

```python
# 1. 缩放到执行器限制
targets = scale(actions, lower_limits, upper_limits)

# 2. 动作平滑（可选）
if act_moving_average < 1.0:
    targets = α * targets + (1-α) * prev_actions

# 3. 裁剪到限制
targets = clip(targets, lower_limits, upper_limits)

# 4. 应用控制
actuator_ctrls = targets
```

### 动作维度详细说明

| 编号  | 手指   | 关节  | 自由度 | 说明          |
| ----- | ------ | ----- | ------ | ------------- |
| 0-4   | 拇指   | J0-J4 | 5      | CMC, MCP, IP  |
| 5-8   | 食指   | J0-J3 | 4      | MCP, PIP, DIP |
| 9-12  | 中指   | J0-J3 | 4      | MCP, PIP, DIP |
| 13-16 | 无名指 | J0-J3 | 4      | MCP, PIP, DIP |
| 17-20 | 小指   | J0-J3 | 4      | MCP, PIP, DIP |

---

## 观测空间

观测空间为 `Box(-inf, inf, (157,), float32)`，包含机器人的本体感知信息、立方体状态、目标状态和指尖状态。

### 观测组成部分

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

#### 1. 手部关节状态（48 维）

-   **关节位置（24 维）**：未缩放的原始关节角度
-   **关节速度（24 维）**：缩放 0.2 倍的关节角速度

#### 2. 立方体状态（17 维）

-   **位置（3 维）**：世界坐标系中的立方体位置 `(x, y, z)`
-   **姿态（4 维）**：四元数 `(x, y, z, w)`
-   **线速度（3 维）**：立方体线速度
-   **角速度（3 维）**：缩放 0.2 倍的角速度
-   **归一化因子**：速度观测乘以 `vel_obs_scale = 0.2`

#### 3. 目标状态（11 维）

-   **目标位置（3 维）**：固定在 `(0.33, 0.00, 0.295)`
-   **目标姿态（4 维）**：随机采样的目标四元数
-   **相对旋转（4 维）**：立方体到目标的相对四元数

#### 4. 指尖状态（65 维）

5 个指尖的状态，每个指尖 13 维：

-   **位置（3 维）**：指尖在笛卡尔空间的位置
-   **姿态（4 维）**：指尖四元数
-   **速度（6 维）**：线速度和角速度

**指尖链接名称**：

-   `rh_ffdistal`：食指指尖
-   `rh_mfdistal`：中指指尖
-   `rh_rfdistal`：无名指指尖
-   `rh_lfdistal`：小指指尖
-   `rh_thdistal`：拇指指尖

#### 5. 动作历史（20 维）

-   上一步的动作值，用于策略的时间上下文

### 观测详细说明

| 编号    | 观测内容                     | 维度 | 范围         | 单位   |
| ------- | ---------------------------- | ---- | ------------ | ------ |
| 0-23    | 手部关节位置（未归一化）     | 24   | 关节限制范围 | rad    |
| 24-47   | 手部关节速度（× 0.2）        | 24   | ±π/2         | rad/s  |
| 48-50   | 立方体位置                   | 3    | 实数         | m      |
| 51-54   | 立方体姿态（四元数 w,x,y,z） | 4    | 单位范数     | 无量纲 |
| 55-57   | 立方体线速度                 | 3    | 实数         | m/s    |
| 58-60   | 立方体角速度（× 0.2）        | 3    | 实数         | rad/s  |
| 61-63   | 目标位置                     | 3    | 固定值       | m      |
| 64-67   | 目标姿态（四元数 w,x,y,z）   | 4    | 单位范数     | 无量纲 |
| 68-71   | 相对旋转（四元数 w,x,y,z）   | 4    | 单位范数     | 无量纲 |
| 72-136  | 指尖状态（5×13）             | 65   | -            | -      |
| 137-156 | 上一步动作                   | 20   | [-1, 1]      | 无量纲 |

---

## 奖励函数

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

### 主要奖励项

1. **旋转对齐奖励**（核心目标）

    ```
    rot_reward = rot_reward_scale / (|rot_dist| + rot_eps)
    ```

    - **缩放系数**：`1.0`
    - **Epsilon**：`0.1`
    - **旋转距离计算**：使用四元数旋转距离公式
    - **激励**：随着立方体姿态接近目标，奖励呈反比增长

2. **位置距离惩罚**

    ```
    dist_reward = dist_reward_scale × goal_dist
    ```

    - **缩放系数**：`-10.0`
    - **距离计算**：立方体到目标位置的欧氏距离
    - **激励**：阻止立方体掉落，保持在目标位置附近

3. **动作正则化惩罚**

    ```
    action_penalty = action_penalty_scale × ||actions||²
    ```

    - **缩放系数**：`-0.0002`
    - **目的**：鼓励平滑、节能的运动

### 条件奖励

4. **成功奖励**

    ```
    if |rot_dist| ≤ success_tolerance:
        reward += reach_goal_bonus
    ```

    - **奖励值**：`2.0`
    - **容差**：`0.1` 弧度（约 5.7°）
    - **目的**：达成目标对齐的稀疏奖励

5. **掉落惩罚**

    ```
    if goal_dist ≥ fall_dist:
        reward += fall_penalty
        terminated = True
    ```

    - **惩罚值**：`0.0`（仅终止，无额外惩罚）
    - **距离阈值**：`0.24`
    - **目的**：立方体掉落时终止回合

---

## 初始状态

### 手部初始化

**位置初始化：**

手掌固定在世界坐标系中，位置由模型文件确定。

**关节角度初始化：**

-   使用模型默认关节位置
-   添加均匀随机噪声：`[-0.2, 0.2]` 弧度
-   作用范围：所有 24 个手部自由度

**速度初始化：**

所有关节速度初始化为零。

### 立方体初始化

**位置初始化：**

-   固定位置：`(0.33, 0.00, 0.295)` 手掌中心上方）
-   添加均匀随机噪声：`[-0.01, 0.01]` ±1cm）

**姿态初始化：**

-   使用 Shoemake 方法生成均匀分布的随机四元数
-   确保在 SO(3) 空间上的均匀采样

**速度初始化：**

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

### 目标初始化

**位置初始化：**

-   固定位置：`(0.33, 0.00, 0.295)`（与立方体初始位置相同）

**姿态初始化：**

-   使用 Shoemake 方法生成均匀分布的随机目标四元数
-   每次重置时重新采样

---

## 终止条件

回合在以下情况下终止：

1. **掉落终止**：立方体距离目标位置 ≥ `fall_dist`（0.24m）
2. **超时终止**：达到 `max_episode_steps`（默认 1000 步）
3. **NaN 保护**：检测到旋转距离或位置距离为 NaN

### 成功保持机制

环境使用连续成功计数器：

-   当满足旋转容差时，计数器递增
-   当达到 `max_consecutive_successes`（50）时，触发成功终止并重置目标
-   旋转容差：`0.1` 弧度

---

## 使用方法

### 训练

```bash
uv run scripts/train.py --env shadow-hand-repose
```

### 策略评估

```bash
uv run scripts/play.py --env shadow-hand-repose
```

### 环境可视化

```bash
uv run scripts/view.py --env shadow-hand-repose
```

### TensorBoard

```bash
uv run tensorboard --logdir runs/shadow-hand-repose
```

---

## 配置参数

### 环境参数

| 参数名                | 默认值 | 说明               |
| --------------------- | ------ | ------------------ |
| `max_episode_seconds` | 10.0   | 最大回合时长（秒） |
| `ctrl_dt`             | 0.01   | 控制时间步（秒）   |
| `max_episode_steps`   | 1000   | 最大回合步数       |
| `num_hand_dofs`       | 24     | 手部总自由度       |
| `num_actuators`       | 20     | 驱动关节数量       |

### 奖励参数

| 参数名                 | 默认值  | 说明             |
| ---------------------- | ------- | ---------------- |
| `dist_reward_scale`    | -10.0   | 位置距离奖励     |
| `rot_reward_scale`     | 1.0     | 旋转对齐奖励     |
| `rot_eps`              | 0.1     | 旋转奖励 epsilon |
| `action_penalty_scale` | -0.0002 | 动作正则化       |
| `success_tolerance`    | 0.1     | 成功容差（弧度） |
| `reach_goal_bonus`     | 2.0     | 成功奖励         |
| `fall_dist`            | 0.24    | 掉落距离阈值     |
| `fall_penalty`         | 0.0     | 掉落惩罚         |

### 重置噪声参数

| 参数名                 | 默认值 | 说明                    |
| ---------------------- | ------ | ----------------------- |
| `reset_position_noise` | 0.01   | 立方体位置噪声（米）    |
| `reset_dof_pos_noise`  | 0.2    | 关节位置噪声（弧度）    |
| `reset_dof_vel_noise`  | 0.0    | 关节速度噪声（弧度/秒） |

### 观测缩放参数

| 参数名          | 默认值 | 说明             |
| --------------- | ------ | ---------------- |
| `vel_obs_scale` | 0.2    | 速度观测缩放因子 |

---

## 参考资料

该环境基于以下经典工作实现：

-   **OpenAI Dactyl** (2018)：首个成功的手内操作 sim-to-real 迁移
-   **Isaac Gym** (2021)：高性能 GPU 加速的物理仿真
-   **Isaac Lab** (2023)：模块化机器人学习框架
