# rc_mjlab

基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。

> 本目录对应 `v0.6.0`：比赛使用的最终训练架构。它在前两版训练代码上重新平衡随机化强度，引入分轴速度奖励、自适应命令课程、障碍逐步释放、楼梯稳定约束和训练诊断指标。当前目录中的 `model_rough.pt` 是早期参考权重；比赛最终使用的 `model_6800.onnx` 将随最终部署版本归档。

---

## 🛠️ 项目简介

本项目针对一台 **4 腿 × 3 关节 + 4 驱动轮（轮腿混合）** 的移动机器人，在 MuJoCo 物理引擎中利用 PPO 算法进行多任务运动控制策略训练。

系统设计特点包括：
1. **高保真动力学步进**：物理仿真计算步长设为 **`2ms` (0.002s)**，为碰撞、地面力学传递提供极高的解算频宽与稳定性。
2. **50Hz 控制决策循环**：通过在环境中设置 `decimation = 10`，策略决策周期为 `20ms` ($0.002\text{s} \times 10 = 0.02\text{s}$)，即控制决策频率为 **`50Hz`**，完全对齐真机控制周期。
3. **混合滤波执行器**：
   - 腿部 12 个位置控制关节采用位置 PD 伺服（$K_p=40, K_d=1$），并叠加截止频率为 **`5Hz`** 的低通滤波器进行动作平滑，减小高频机械抖动。
   - 轮部 4 个速度驱动关节采用阻尼速度伺服（$K_d=0.5$），叠加截止频率为 **`15Hz`** 的低通速度滤波器，保证转速响应的灵敏度。
4. **大规模并行加速**：利用 GPU 并行（通过 Warp 和 MuJoCo GPU 物理管线），支持最多 $4096$ 环境同时训练，并包含对动作变化率、关节加速度的惩罚项以平抑噪声。

---

## 📦 项目结构

```
rc_mjlab/
├── src/robot/                    # RL 训练任务包（主体代码）
│   ├── __init__.py               # 任务注册（Robot-Flat-v0 / Robot-Rough-v0 / Robot-Crawl-v0）
│   ├── robot_cfg.py              # 机器人物理参数（PD 增益、执行器上限、碰撞属性）
│   ├── config/
│   │   ├── env_cfgs.py           # 三套环境完整配置（观测、奖励、事件、地形、终止条件）
│   │   └── rl_cfg.py             # PPO 超参数（网络结构、学习率、折扣因子等）
│   ├── mdp/
│   │   ├── rewards.py            # 自定义奖励函数（速度追踪、姿态约束、接触、越障反射惩罚等）
│   │   ├── curriculums.py        # 地形关卡课程（严格速度约束版）+ 自适应速度范围
│   │   ├── lowpass_actions.py    # 低通滤波动作包装（腿 5 Hz / 轮 15 Hz IIR 滤波）
│   │   ├── disturbances.py       # 持续外力扰动（一阶低通滤波平滑随机外力/扭矩）
│   │   ├── mode_command.py       # 离散步态模式命令（保留扩展用）
│   │   └── only_positive_rewards.py  # HIMLoco 风格：每步总奖励截断为 ≥ 0，防止消极逃避
│   └── terrains/
│       └── competition_terrains.py   # 竞赛自定义地形（高墙障碍、低杆障碍）
├── sim2sim/                      # Sim2Sim 物理部署与高精度交互回放工具
│   ├── nav_sim2sim.py            # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪
│   ├── sim2sim.py                # 简易版键盘调试工具
│   ├── interface/
│   │   └── mujoco_io.py          # MuJoCo 输入输出与传感器、低通滤波器接口
│   ├── tools/
│   │   └── math_utils.py         # 姿态重力等数学转换
│   ├── policy/                   # 保存的 pt 策略权重
│   └── terrain/
│       └── scene_terrain.xml     # 完整越障比赛场地的物理 XML 定义
├── mjcf/
│   ├── wheelleg.xml              # 机器人 MuJoCo 模型（含网格引用）
│   ├── scene.xml                 # mjlab 场景入口文件
│   └── meshes/                   # STL/OBJ 碰撞与外观网格
├── model_rough.pt                # 本阶段用于回放和 Sim2Sim 的 Rough 策略
├── pyproject.toml                # 项目依赖（uv 管理，含清华镜像源加速）
└── uv.lock                       # 精确依赖锁定文件
```

---

## 🚀 常用命令

### 1. 训练与回放

```bash
# 运行平地基础训练 (Robot-Flat-v0)
uv run train Robot-Flat-v0

# 运行多障碍复杂地形训练 (Robot-Rough-v0)，可从 Flat 的Checkpoint热启动
uv run train Robot-Rough-v0 --agent.resume True --agent.experiment-name robot_flat

# 运行爬坡与匍匐限高任务 (Robot-Crawl-v0)
uv run train Robot-Crawl-v0

# 使用默认 20 个并行环境回放最新 checkpoint 效果
uv run play Robot-Rough-v0
```

### 2. 交互式 Sim2Sim 自动导航仪表盘

我们提供了一个强大的 GUI 交互和全自动障碍赛追踪平台，位于 `sim2sim` 目录下：

```bash
# 启动 2D 交互导航平台
cd sim2sim
uv run python nav_sim2sim.py
```

---

## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)

该平台包含一个 **Pygame 2D HUD 监控面板** 和一个 **实时 MuJoCo 3D 渲染器**，支持对仿真参数和任务执行的精细控制。

### 1. 按钮面板分区与布局

面板在垂直方向进行了高紧凑性排版，避免控件重叠，并在底端留有安全间距：
* **【预设任务列表】**（按物理穿越顺序排列）：
  - **S形绕杆 (Slalom)**：绕过红蓝两色障碍杆路径。
  - **限高下蹲 (Crawl)**：降低机身高度穿过低杆障碍。
  - **砂砾碎石 (Gravel)**：平稳低速通过多颗粒非结构碎石坑。
  - **高墙越障 (Wall)**：高速度冲向障碍高墙，利用前轮攀爬反射爬越。
  - **台阶攀爬 (Stairs)**：攀越分段式台阶。
  - **斜坡木桥 (Bridge)**：穿过A坡并稳健从B坡落地。
  - **障碍赛大满贯 (Grand)**：**科技紫**圆角高亮按钮。点击后，机器人将以**顺时针**方向，自动、连贯且闭环地一次性穿越上述全部 6 个核心比赛障碍，并在木桥落地后，通过安全通道直角返航至起终点。
* **【系统与视图控制】**：
  - **清除与停止 (Stop)**：一键紧急停止并重置当前目标航点。
  - **视角居中 (Center)**：一键锁定相机随机器人机身移动。
  - **物理流速三联排 (倍速- / 标准 / 倍速+)**：在不破坏物理计算数值稳定性的前提下，实现对仿真总体时间的平滑加速与慢放（支持 `0.2x` ~ `5.0x`，可随时点击“标准”一键归位 `1.0x`）。
* **【目标微调与命令终端】**：
  - 拥有高精度航点微调发令键。
  - 底部命令行支持输入 `speed <倍率>` 更改仿真速度，或者输入 `grand` 直接开启大满贯。

---

## 📊 机器人系统规格参数

### 1. 机器人本体参数

| 参数项 | 基准数值 | 说明 |
|---|---|---|
| **物理步长 ($dt_{physics}$)** | `0.002s` (2ms) | 底层 MuJoCo 求解器步长，物理精度极高 |
| **控制决策频率 ($Freq_{ctrl}$)** | `50Hz` (20ms) | $decimation = 10$，环境每 10 个子步进行一次交互决策 |
| **单轮仿真时长** | `30.0s` | 最大决策步数上限为 $30.0 / 0.02 = 1500$ 步 |
| **腿部控制** | 位置 PD 伺服 | 目标关节角限幅 ±0.25 rad，叠加 **5Hz** 低通滤波器 |
| **轮部控制** | 阻尼速度伺服 | 目标速度限幅 ±10.0 rad/s，叠加 **15Hz** 低通滤波器 |
| **结构形式** | 4腿 × 3关节 + 4轮 | 腿：hip abduction, hip pitch, knee；轮半径 0.1m，左右轮距 0.32m |
| **关节扭矩上限** | 17.0 Nm | 关节最大输出力矩（训练时含 80%~100% 随机缩放） |
| **最大关节角速度** | 13.0 rad/s | 关节最大运动速度限制 |

### 2. 状态观测空间 (Actor Obs, 53维)

网络输入包含 $6$ 步历史数据，并在训练时注入均匀高斯噪声以提升泛化能力：

| 观测项目 | 维度 | 缩放比例 | 噪声范围 |
|---|---|---|---|
| 基座角速度 (ang_vel) | 3 | 0.25 | $[-0.2, 0.2]$ rad/s |
| 投影重力向量 (projected_gravity) | 3 | 1.0 | $[-0.05, 0.05]$ |
| 指令速度 (vx, vy, wz/heading) | 3 | 1.0 | — |
| 腿部关节相对角度 (joint_pos_rel) | 12 | 1.0 | $[-0.01, 0.01]$ rad |
| 腿部关节角速度 (joint_vel) | 12 | 0.05 | $[-1.5, 1.5]$ rad/s |
| 轮子角速度 (wheel_vel) | 4 | 0.05 | $[-1.0, 1.0]$ rad/s |
| 上一步动作缓存 (last_actions) | 16 | 1.0 | — |

> **Critic 附加观测**：包含高精度基座物理线速度、轮地实际接触状态、以及 $1.6\text{m} \times 1.0\text{m}$ 分辨率为 $0.08\text{m}$ 的高度雷达扫描网格，提供大范围越障感知。

---

## ⚖️ 奖惩体系设计 (Robot-Rough-v0)

复杂地形任务采用 **“仅正奖励截断”** 机制（即每步累加的总奖励若小于0则强制截断为0），防止机器人在困难关卡早期选择倒下自杀来规避负惩罚。

### 1. 运动追踪与状态惩罚

| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
|---|---|---|
| **track_lin_vel** | `+4.5` | L1 范数水平线速度跟踪奖励，平缓高速漂移 |
| **track_ang_vel** | `+2.0` | 偏航角速度指数跟踪奖励 |
| **stand_still** | `-2.0` | 当速度指令为 0 时，严厉惩罚关节多余晃动，保持稳立 |
| **joint_pos_penalty** | `-0.8` | 当速度指令为 0 时，惩罚关节角度偏离初始对齐姿态，维持高刚度 |
| **roll_penalty** | `-1.0` | 机身横滚角 (Roll) 倾斜惩罚，抑制左右倾倒抖动 |
| **pitch_penalty** | `-1.5` | 俯仰角 (Pitch) 死区惩罚，限制仰角不超过 29 度，抑制越障瞬间前轮翘头和后翻 |
| **base_height_l2** | `-0.5` | 机身高度偏离 0.36m 惩罚（基于高度扫描均值，允许自适应高低） |

### 2. 能量正则与平滑惩罚 (平抑高频抖动)

| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
|---|---|---|
| **action_rate_curriculum** | `-0.005` | 动作变化率 L2 惩罚，迫使连续两个决策步的输出动作变化平滑 |
| **joint_torques** | `-1.0e-4` | 关节输出扭矩 L2 正则，降低电机总发热和冲击性载荷 |
| **leg_joint_acc_l2** | `-2.5e-7` | 限制腿部 12 关节**角加速度**，直接抑制关节高频电磁和机械震荡 |
| **wheel_joint_acc_l2** | `-2.5e-9` | 限制 4 个驱动轮的**角加速度**，平缓轮速切换，降低打滑振荡 |
| **joint_pos_limits** | `-0.2` | 极度接近关节极限限位阻挡时的硬惩罚 |

### 3. 接触反射与安全约束

| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
|---|---|---|
| **feet_contact_without_cmd** | `+0.1` | 当速度指令为 0 时，鼓励四轮保持稳定接地的正向收益 |
| **body_collision** | `-1.0` | 腿部连杆（大腿、小腿）触地碰撞惩罚，迫使抬腿跨越障碍 |
| **base_collision** | `-5.0` | 机身/底盘硬撞障碍物时的严厉惩罚，逼迫机器人学会抬起前轮支撑攀爬 |
| **is_terminated** | `0.0` | 关闭越障任务的提早终止，允许机器人跌倒后自行挣扎起立，提高生存极限 |

---

## 🌀 域随机化 (Domain Randomization)

为了使训练的控制策略具有卓越的零样本真机部署能力，在环境重置及仿真运行中注入了高强度的域随机化参数：

| 随机化项目 | 扰动操作 | 随机范围 |
|---|---|---|
| **机身质心偏移 (base_com)** | 加法 | X, Y, Z 三轴分别随机偏置 `[-0.05, 0.05]` 米 |
| **角度传感器零偏 (encoder_bias)** | 加法 | 关节传感器绝对偏置 `[-0.015, 0.015]` rad (约 $\pm 0.85^{\circ}$) |
| **几何表面摩擦力 (body_friction)** | 绝对值 | 地面及机器人碰撞几何体摩擦力在 `[0.3, 1.2]` 均匀随机 |
| **关节摩擦阻尼 (joint_friction)** | 乘法 | 所有旋转轴关节运动阻尼摩擦在原值的 `[0.7, 1.3]` 倍间随机 |
| **关节传动刚度 (actuator_stiffness)** | 乘法 | Kp 刚度系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 |
| **关节传动阻尼 (actuator_damping)** | 乘法 | Kd 阻尼系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 |
| **力矩输出上限 (actuator_effort_limit)**| 乘法 | 最大输出扭矩极限随机在原值的 `[0.8, 1.0]` 倍均匀缩放 |
| **负载质量 (payload_mass)** | 加法 | 在机身处添加载荷质量，扰动范围在 `[-1.0, 3.0]` kg |
| **瞬时侧向推撞 (push_robot)** | 脉冲 | 每隔 `[5.0, 10.0]` 秒，瞬间施加 X/Y 轴 `[-0.5, 0.5]` m/s 冲击速度 |
| **一阶低通持续风阻 (continuous_disturbance)** | 连续 | 机身持续叠加随机外力（±15N）与力矩（±10Nm），低通周期 0.5s |

---

## 🏆 多地形关卡难度控制 (Robot-Rough-v0)

共有 8 种子地形按照比例混合，通过自适应升级距离控制关卡难度的推进：

| 地形名称 | 混合比例 (Proportion) | 最大配置难度 |
|---|---|---|
| **平地 (flat)** | `5%` | 作为初始安定性恢复区域 |
| **金字塔台阶 (pyramid_stairs)** | `25%` | 最大阶梯高度上限 `0.30` 米，级宽 0.30m |
| **倒金字塔台阶 (pyramid_stairs_inv)** | `10%` | 最大倒台阶高度上限 `0.30` 米，级宽 0.30m |
| **随机高度网格 (random_grid)** | `10%` | 最大网格方块起伏上限 `0.30` 米 |
| **随机粗糙地形 (random_rough)** | `5%` | 地表最大颗粒随机噪声起伏 `0.06` 米 |
| **柏林噪声地形 (perlin_noise)** | `5%` | 大范围高平缓起伏最大高度 `0.06` 米 |
| **越障高墙地形 (rc_wall)** | `25%` | 自定义跳跃垂直高墙，最大墙高上限 `0.45` 米 |
| **平台斜坡地形 (sloped_terrain)** | `15%` | 最大坡度限制 `0.325` (约 $18.5^{\circ}$) |

> **地形升级规则**：当机器人朝指令方向行进距离超过当前地块的一半（4米），且实际行进距离大于速度指令对应期望距离的 45% 时，该环境关卡等级 +1。  
> **地形降级规则**：当指令速度大于 0.1m/s 但实际行进距离小于期望距离的 25%，或者实际移动不足 2.0米时，环境难度等级 -1。