**语言**: [English](README.md) | [中文](#)

---

# MuJoCo 人形机器人仿真控制

本项目使用 MuJoCo 物理引擎进行人形机器人仿真，并通过游戏手柄实时控制机器人运动。

> **🎉 开箱即用**: 程序已经集成了**行者泰山**训练好的策略模型（`.pt` 文件），配置完成后可以直接运行，无需额外训练。

## 📺 演示视频

观看演示视频了解机器人实际运行效果：

<div align="center">

### ▶️ [在 Bilibili 观看演示视频](https://www.bilibili.com/video/BV1jx2YBqEPD/)

**视频链接**: https://www.bilibili.com/video/BV1jx2YBqEPD/

</div>

## 核心功能

程序包含**行者泰山**全向拟人行走，支持以下特性：
- ✅ **全向拟人行走**: 支持前后左右任意方向移动和旋转
- ✅ **稳定爬楼梯**: 可以直接使用最大速度爬楼梯，自动适应台阶高度
- ✅ **抗扰动**: 具备抗外力扰动能力，可在扰动测试模式下验证
- ✅ **停下脚位置自动归正**: 停止移动时，机器人会自动调整脚部位置至最佳姿态

> **⚠️ 已知问题**: 由于 MuJoCo 仿真器的物理引擎限制，存在以下已知现象：
> - 脚第一次爬台阶时可能会存在**陷入楼梯**的情况（后续爬台阶正常）
> - 原地站立较长时间可能出现**打滑现象**
> 


## 目录

- [核心功能](#核心功能)
- [功能特点](#功能特点)
- [环境要求](#环境要求)
- [快速开始](#快速开始)
  - [步骤1: 校准游戏手柄](#步骤1-校准游戏手柄)
  - [步骤2: 运行仿真](#步骤2-运行仿真)
- [游戏手柄控制说明](#游戏手柄控制说明)
  - [摇杆控制](#摇杆控制)
  - [按钮控制](#按钮控制)
  - [方向键控制](#方向键控制)
- [运行模式](#运行模式)
- [配置文件说明](#配置文件说明)
- [常见问题](#常见问题)

## 功能特点

- ✅ **预训练模型**: 已集成行者泰山训练好的策略模型，开箱即用
- ✅ 实时游戏手柄控制机器人运动
- ✅ 三种运行模式：行走、奔跑、抗扰动测试
- ✅ 可切换视角跟踪和可视化选项
- ✅ 支持多种游戏手柄（罗技、北通等）
- ✅ 自动校准游戏手柄中位值
- ✅ 支持重置机器人状态

## 环境要求

- Python 3.7+
- MuJoCo
- PyTorch
- Pygame
- PyYAML
- NumPy

## 快速开始

### 基本执行步骤

1. **插入手柄**: 将游戏手柄插入主机（电脑）
2. **配置环境**: 打开终端，配置好环境（默认的强化学习环境即可）
3. **运行程序**: 执行命令启动仿真（必须指定配置文件）
   ```bash
   python deploy_mujoco_gamepad.py e3.yaml
   ```
4. **开启视角跟随**: 摁手柄 **Y键** 开启视角跟随模式
5. **控制机器人**:
   - **右摇杆**: 控制 xy 线速度（前后左右移动）
   - **左摇杆**: 控制偏航角速度（旋转）
   - **方向键（➡️⬅️⬆️⬇️）**: 控制视角方向
   - **B键**: 重置机器人状态

> **💡 提示**: 使用罗技或北通手柄时，可直接运行，无需校准。

---

### 步骤1: 校准游戏手柄

> **💡 提示**: 如果您使用的是**罗技 (Logitech)** 或**北通 (Betop)** 手柄，项目已经包含了默认的校准文件，**可以直接跳过校准步骤**，直接运行仿真程序。只有在使用其他品牌的手柄或默认校准不准确时，才需要运行校准脚本。

在使用其他品牌游戏手柄控制机器人之前，**需要先校准游戏手柄**，以确保摇杆中位值正确。

#### 1.1 运行校准脚本

```bash
python calibrate_gamepad.py
```

#### 1.2 选择手柄类型

程序会提示选择手柄类型：
- **1. 罗技 (Logitech)** - 默认选项，左摇杆(0,1), 右摇杆(2,3)
- **2. 北通 (Betop)** - 左摇杆(0,1), 右摇杆(3,4)
- **3. 其他/自定义** - 手动指定轴映射

#### 1.3 校准摇杆中位值

1. 按照提示，将**所有摇杆移动到中位**（不施加任何力）
2. 按任意键开始采样
3. 程序会自动采样100次并计算中位值

#### 1.4 校准按钮映射

按照提示依次按下以下按钮：
- **LB**: 切换模式按钮
- **X**: 切换显示足底力
- **Y**: 切换是否跟踪pelvis
- **B**: 重置机器人状态
- **A**: 切换显示足端接触状态
- **RB**: 备用按钮（可选）

> **注意**: 如果某个按钮不想校准，可以直接按回车跳过。

#### 1.5 测试校准结果

校准完成后，可以选择测试校准结果，移动摇杆和按下按钮查看实时值。

#### 1.6 保存校准文件

校准结果会自动保存到对应的JSON文件：
- 罗技手柄: `gamepad_calibration_logitech.json`
- 北通手柄: `gamepad_calibration_betop.json`
- 自定义手柄: `gamepad_calibration_custom.json`

### 步骤2: 运行仿真

运行仿真程序：

```bash
# 使用 e3.yaml 配置文件（必须指定）
python deploy_mujoco_gamepad.py e3.yaml
```

**注意**: 运行程序时**必须**指定配置文件 `e3.yaml`。配置文件位于 `configs/` 目录下。

#### 2.1 启动仿真

程序启动后：
1. 会自动加载机器人模型和策略文件
2. 初始化游戏手柄并加载校准文件
3. 打开 MuJoCo 仿真窗口

#### 2.2 开始控制

使用游戏手柄控制机器人运动，详见[游戏手柄控制说明](#游戏手柄控制说明)。

## 游戏手柄控制说明

### 摇杆控制

#### 右摇杆（移动控制）
- **上下**: 控制机器人前后速度
  - 向上推: 前进（正方向）
  - 向下推: 后退（负方向）
- **左右**: 控制机器人左右速度
  - 向左推: 向左移动（负方向）
  - 向右推: 向右移动（正方向）

#### 左摇杆（角速度和扰动力控制）
- **左右**: 控制机器人角速度（旋转）
  - 向左推: 逆时针旋转（负方向）
  - 向右推: 顺时针旋转（正方向）
  - **注意**: 在抗扰动测试模式下，左摇杆左右方向不控制角速度
- **上下**: 仅在抗扰动测试模式下使用，控制扰动力前后方向
- **左右 + 上下**: 在抗扰动测试模式下，控制扰动力方向和大小

### 按钮控制

| 按钮 | 功能 | 说明 |
|------|------|------|
| **LB** | 切换模式 | 循环切换：行走 → 奔跑 → 抗扰动测试 → 行走 |
| **X** | 切换显示足底力 | 在仿真窗口中显示/隐藏足底力向量 |
| **Y** | 切换pelvis跟踪 | 启用/禁用相机跟踪机器人pelvis |
| **B** | 重置机器人状态 | 将机器人重置到初始位置和姿态 |
| **A** | 切换显示接触状态 | 显示/隐藏足端接触状态 |
| **RB** | 备用按钮 | 当前未使用 |

### 方向键控制

**仅在启用pelvis跟踪时有效**（按下Y按钮启用跟踪后）：

- **左右**: 围绕机器人旋转视角
  - 左: 逆时针旋转视角
  - 右: 顺时针旋转视角
- **上下**: 控制视角远近
  - 上: 拉远视角
  - 下: 拉近视角

## 运行模式

### 模式1: 行走模式
- **最大前进速度**: 1.2 m/s
- **最大后退速度**: 0.8 m/s
- **最大左右速度**: 0.8 m/s
- **最大角速度**: 1.3 rad/s
- **适用场景**: 正常行走、精细控制

### 模式2: 奔跑模式
- **最大前进速度**: 2.0 m/s
- **最大后退速度**: 0.8 m/s
- **最大左右速度**: 0.8 m/s
- **最大角速度**: 1.3 rad/s
- **适用场景**: 快速移动、高速行走

### 模式3: 抗扰动测试模式
- **速度限制**: 与行走模式相同
- **特殊功能**: 左摇杆控制扰动力
  - 左摇杆X轴: 控制扰动力左右方向
  - 左摇杆Y轴: 控制扰动力前后方向
  - 扰动力大小: 由摇杆幅度决定（最大100N）
  - 扰动力施加位置: `torso_link`（躯干）
- **角速度控制**: 在此模式下禁用
- **适用场景**: 测试机器人抗扰动能力

**切换模式**: 按下 **LB** 按钮循环切换模式，当前模式会在控制台显示。

## 配置文件说明

配置文件位于 `configs/` 目录下，例如 `e3.yaml`。

### 主要配置项

```yaml
# 策略文件路径
policy_path: "policy/motion_5000.pt"

# 机器人模型文件路径
xml_path: "model/e3/scene_terrain.xml"

# 仿真参数
simulation_duration: 600.0  # 仿真持续时间（秒）
simulation_dt: 0.002         # 仿真时间步长
control_decimation: 10       # 控制更新频率

# PD控制器参数
kps: [100, 80, 80, ...]      # 位置增益
kds: [4, 2, 2, ...]          # 速度增益

# 默认关节角度
default_angles: [-0.10, 0.0, ...]

# 缩放参数
ang_vel_scale: 0.25          # 角速度缩放
dof_pos_scale: 1.0           # 关节位置缩放
dof_vel_scale: 0.05          # 关节速度缩放
action_scale: 0.25           # 动作缩放
cmd_scale: [2.0, 2.0, 0.25]  # 速度命令缩放

# 观测和动作维度
num_actions: 21              # 动作维度
num_obs: 72                  # 观测维度
include_phase_in_obs: False  # 是否在观测中包含相位

# 初始速度命令
cmd_init: [0., 0, 0]         # [前后速度, 左右速度, 角速度]

# 游戏手柄类型
gamepad_type: logitech       # logitech, betop, 或 custom
```

### 游戏手柄配置

在配置文件中可以指定游戏手柄类型：

```yaml
gamepad_type: logitech  # 可选: logitech, betop, custom
```

如果使用自定义手柄，还可以指定轴映射：

```yaml
gamepad_type: custom
axis_mapping: [0, 1, 2, 3]  # [左摇杆X, 左摇杆Y, 右摇杆X, 右摇杆Y]
```

## 常见问题

### 1. 机器人一直偏或不受控制

**原因**: 游戏手柄未校准或校准不准确。

**解决方法**:
1. 运行 `calibrate_gamepad.py` 重新校准
2. 确保校准过程中所有摇杆都在中位
3. 检查校准文件是否正确加载

### 2. 游戏手柄未检测到

**原因**: 游戏手柄未连接或驱动问题。

**解决方法**:
1. 检查游戏手柄是否正确连接
2. 在Linux系统上，可能需要安装 `python3-pygame` 或相关驱动
3. 运行校准脚本测试手柄是否被识别

### 3. 按钮无响应

**原因**: 按钮未校准或按钮ID错误。

**解决方法**:
1. 运行校准脚本重新校准按钮
2. 检查校准文件中的按钮映射是否正确
3. 确认按钮ID与手柄实际按钮对应

### 4. 仿真窗口无法打开

**原因**: MuJoCo viewer初始化失败。

**解决方法**:
1. 检查MuJoCo是否正确安装
2. 确认显示环境变量设置正确（Linux系统可能需要设置 `DISPLAY`）
3. 检查模型文件路径是否正确

### 5. 机器人运动不自然

**原因**: PD控制器参数或缩放参数不合适。

**解决方法**:
1. 调整配置文件中的 `kps` 和 `kds` 参数
2. 检查 `action_scale` 和 `cmd_scale` 参数
3. 确认 `default_angles` 是否正确

### 6. 抗扰动模式下扰动力不生效

**原因**: 未切换到抗扰动模式或扰动力body ID错误。

**解决方法**:
1. 按下 **LB** 按钮切换到抗扰动测试模式
2. 检查控制台输出，确认模式已切换
3. 确认模型文件中存在 `torso_link` body

## 文件结构

```
deploy_mujoco/
├── calibrate_gamepad.py              # 游戏手柄校准脚本
├── deploy_mujoco_gamepad.py          # 主仿真程序
├── configs/                          # 配置文件目录
│   └── e3.yaml                       # 机器人配置文件
├── model/                            # 机器人模型目录
│   └── e3/
│       ├── e3.xml                    # 机器人模型文件
│       ├── scene_terrain.xml         # 场景文件
│       └── meshes/                   # 网格文件
├── policy/                           # 策略文件目录
│   └── motion_5000.pt                # 训练好的策略模型
├── utils/                            # 工具函数目录
│   ├── gamepad_utils.py              # 游戏手柄工具
│   ├── mode_utils.py                 # 模式控制工具
│   ├── viewer_utils.py               # 视图控制工具
│   ├── disturbance_utils.py          # 扰动力工具
│   └── math_utils.py                 # 数学工具
└── gamepad_calibration_*.json        # 游戏手柄校准文件
```

## 技术支持

如有问题或建议，请查看代码注释或联系开发团队。

---

**祝您使用愉快！** 🚀

