LinkNexus/huoyi_training_level
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add smooth/bridge tasks and fire ant stability training docs

Browse Source 
- register Smooth/Bridge M20 tasks and add env configs
- add smooth random terrain and roll/pitch orientation rewards
- update M20 URDF mass and wheel geometry, refresh prototype specs
- add fire ant smooth training task document
This commit is contained in:
syfffffjj
2026-11-02 16:36:38 +08:00
parent ee8963cf4c
commit 9eab8830a4
14 changed files with 1016 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf
+39 -39
View File
@@ -20,9 +20,9 @@
<link name="base_link"> <link name="base_link">
<inertial> <inertial>
<origin xyz="-0.000072 0.000171 0.076892" rpy="0 0 0"/> <origin xyz="-0.000072 0.000171 0.076892" rpy="0 0 0"/>
<mass value="17.401"/> <mass value="15.84"/>
<inertia ixx="0.133964838" ixy="-0.000732466" ixz="0.000157104" <inertia ixx="0.121947189" ixy="-0.000666758" ixz="0.000143011"
iyy="0.363696493" iyz="0.000165399" izz="0.450717599"/> iyy="0.331070194" iyz="0.000150561" izz="0.410284855"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
@@ -42,9 +42,9 @@
<link name="fl_knee"> <link name="fl_knee">
<inertial> <inertial>
<origin xyz="-0.000040 -0.001088 -0.158988" rpy="0 0 0"/> <origin xyz="-0.000040 -0.001088 -0.158988" rpy="0 0 0"/>
<mass value="0.955"/> <mass value="0.885"/>
<inertia ixx="0.014124479" ixy="0.000002919" ixz="0.000004851" <inertia ixx="0.013089177" ixy="0.000002705" ixz="0.000004495"
iyy="0.014470094" iyz="-0.000261898" izz="0.000478899"/> iyy="0.013409459" iyz="-0.000242701" izz="0.000443796"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
@@ -75,20 +75,20 @@
<link name="fl_wheel"> <link name="fl_wheel">
<inertial> <inertial>
<origin xyz="-0.000002 0.059326 -0.000009" rpy="0 0 0"/> <origin xyz="-0.000002 0.064 -0.000009" rpy="0 0 0"/>
<mass value="1.340"/> <mass value="1.340"/>
<inertia ixx="0.003296552" ixy="0.000000144" ixz="-0.000000077" <inertia ixx="0.004085980" ixy="0.000000178" ixz="-0.000000095"
iyy="0.005826562" iyz="0.000000424" izz="0.003296995"/> iyy="0.007221854" iyz="0.000000526" izz="0.004086529"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
<mesh filename="../meshes_solenopsis_visual/fl_wheel.STL" scale="0.001 0.001 0.001"/> <mesh filename="../meshes_solenopsis_visual/fl_wheel.STL" scale="0.001122449 0.001428571 0.001122449"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0.0457795 0" rpy="1.570796326795 0 0"/> <origin xyz="0 0.064 0" rpy="1.570796326795 0 0"/>
<geometry> <geometry>
<cylinder length="0.082959" radius="0.098804"/> <cylinder length="0.12" radius="0.11"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@@ -106,9 +106,9 @@
<link name="fr_knee"> <link name="fr_knee">
<inertial> <inertial>
<origin xyz="0.000040 0.001088 -0.158988" rpy="0 0 0"/> <origin xyz="0.000040 0.001088 -0.158988" rpy="0 0 0"/>
<mass value="0.955"/> <mass value="0.885"/>
<inertia ixx="0.014124479" ixy="0.000002919" ixz="-0.000004851" <inertia ixx="0.013089177" ixy="0.000002705" ixz="-0.000004495"
iyy="0.014470094" iyz="0.000261898" izz="0.000478899"/> iyy="0.013409459" iyz="0.000242701" izz="0.000443796"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
@@ -139,20 +139,20 @@
<link name="fr_wheel"> <link name="fr_wheel">
<inertial> <inertial>
<origin xyz="0.000002 -0.059326 -0.000009" rpy="0 0 0"/> <origin xyz="0.000002 -0.064 -0.000009" rpy="0 0 0"/>
<mass value="1.340"/> <mass value="1.340"/>
<inertia ixx="0.003296552" ixy="0.000000144" ixz="0.000000077" <inertia ixx="0.004085980" ixy="0.000000178" ixz="0.000000095"
iyy="0.005826562" iyz="-0.000000424" izz="0.003296995"/> iyy="0.007221854" iyz="-0.000000526" izz="0.004086529"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
<mesh filename="../meshes_solenopsis_visual/fr_wheel.STL" scale="0.001 0.001 0.001"/> <mesh filename="../meshes_solenopsis_visual/fr_wheel.STL" scale="0.001122449 0.001428571 0.001122449"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 -0.0457795 0" rpy="1.570796326795 0 0"/> <origin xyz="0 -0.064 0" rpy="1.570796326795 0 0"/>
<geometry> <geometry>
<cylinder length="0.082959" radius="0.098804"/> <cylinder length="0.12" radius="0.11"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@@ -168,9 +168,9 @@
<link name="hl_knee"> <link name="hl_knee">
<inertial> <inertial>
<origin xyz="-0.000040 -0.001088 -0.158988" rpy="0 0 0"/> <origin xyz="-0.000040 -0.001088 -0.158988" rpy="0 0 0"/>
<mass value="0.955"/> <mass value="0.885"/>
<inertia ixx="0.014124479" ixy="0.000002919" ixz="0.000004851" <inertia ixx="0.013089177" ixy="0.000002705" ixz="0.000004495"
iyy="0.014470094" iyz="-0.000261898" izz="0.000478899"/> iyy="0.013409459" iyz="-0.000242701" izz="0.000443796"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
@@ -201,20 +201,20 @@
<link name="hl_wheel"> <link name="hl_wheel">
<inertial> <inertial>
<origin xyz="-0.000002 0.059326 -0.000009" rpy="0 0 0"/> <origin xyz="-0.000002 0.064 -0.000009" rpy="0 0 0"/>
<mass value="1.340"/> <mass value="1.340"/>
<inertia ixx="0.003296552" ixy="0.000000144" ixz="-0.000000077" <inertia ixx="0.004085980" ixy="0.000000178" ixz="-0.000000095"
iyy="0.005826562" iyz="0.000000424" izz="0.003296995"/> iyy="0.007221854" iyz="0.000000526" izz="0.004086529"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
<mesh filename="../meshes_solenopsis_visual/hl_wheel.STL" scale="0.001 0.001 0.001"/> <mesh filename="../meshes_solenopsis_visual/hl_wheel.STL" scale="0.001122449 0.001428571 0.001122449"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0.0457795 0" rpy="1.570796326795 0 0"/> <origin xyz="0 0.064 0" rpy="1.570796326795 0 0"/>
<geometry> <geometry>
<cylinder length="0.082959" radius="0.098804"/> <cylinder length="0.12" radius="0.11"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@@ -230,9 +230,9 @@
<link name="hr_knee"> <link name="hr_knee">
<inertial> <inertial>
<origin xyz="0.000040 0.001088 -0.158988" rpy="0 0 0"/> <origin xyz="0.000040 0.001088 -0.158988" rpy="0 0 0"/>
<mass value="0.955"/> <mass value="0.885"/>
<inertia ixx="0.014124479" ixy="0.000002919" ixz="-0.000004851" <inertia ixx="0.013089177" ixy="0.000002705" ixz="-0.000004495"
iyy="0.014470094" iyz="0.000261898" izz="0.000478899"/> iyy="0.013409459" iyz="0.000242701" izz="0.000443796"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
@@ -263,20 +263,20 @@
<link name="hr_wheel"> <link name="hr_wheel">
<inertial> <inertial>
<origin xyz="0.000002 -0.059326 -0.000009" rpy="0 0 0"/> <origin xyz="0.000002 -0.064 -0.000009" rpy="0 0 0"/>
<mass value="1.340"/> <mass value="1.340"/>
<inertia ixx="0.003296552" ixy="0.000000144" ixz="0.000000077" <inertia ixx="0.004085980" ixy="0.000000178" ixz="0.000000095"
iyy="0.005826562" iyz="-0.000000424" izz="0.003296995"/> iyy="0.007221854" iyz="-0.000000526" izz="0.004086529"/>
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
<mesh filename="../meshes_solenopsis_visual/hr_wheel.STL" scale="0.001 0.001 0.001"/> <mesh filename="../meshes_solenopsis_visual/hr_wheel.STL" scale="0.001122449 0.001428571 0.001122449"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 -0.0457795 0" rpy="1.570796326795 0 0"/> <origin xyz="0 -0.064 0" rpy="1.570796326795 0 0"/>
<geometry> <geometry>
<cylinder length="0.082959" radius="0.098804"/> <cylinder length="0.12" radius="0.11"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
docs/fire_ant_prototype_specs.md
+23
View File
@@ -131,6 +131,29 @@ boxes 高度 = 0.03 ~ 0.22 m
随机粗糙高度 = 0.01 ~ 0.14 m 随机粗糙高度 = 0.01 ~ 0.14 m
``` ```
### 膝关节向内折叠限制
膝关节角为 0 时膝连杆竖直向下，因此膝关节角就是腿相对竖直方向的偏角。前腿向内折叠对应关节角减小，后腿向内折叠对应关节角增大。训练要求向内最多折叠到竖直向下，不能更向内：
```text
前膝 q >= 0
后膝 q <= 0
```
超出部分以二次惩罚计入奖励：
```text
penalty = sum(max(-q_front, 0)^2) + sum(max(q_hind, 0)^2)
```
实现位置：
- 惩罚函数：`mdp/rewards.py::knee_inward_fold_penalty`
- 奖励项与权重：`config/wheeled/deeprobotics_m20/rough_env_cfg.py` 中的 `knee_inward_fold_penalty`
- 当前权重为 `-1.0`，`max_inward_tilt` 为 `0.0`（即允许的最大向内倾角为 0，腿不能越过竖直向下姿态）
向外折叠（前膝 q > 0、后膝 q < 0）不受该惩罚约束，仍由常规关节位置限制约束。若训练中仍出现越界，可增大惩罚权重；若限制过于保守影响越障，可将 `max_inward_tilt` 调大（例如 `0.1` rad）或在阶段 2 适当减小权重。
## 6. 样机设计重点 ## 6. 样机设计重点
优先级建议： 优先级建议：
docs/fire_ant_smooth_training.md
+561
View File
@@ -0,0 +1,561 @@
# 火蚁平稳性训练任务文档
> **任务 ID**：`Smooth-Deeprobotics-M20-v0`
> **机器人**：火蚁 / Solenopsis（8 自由度轮腿）
> **训练目标**：在低频随机起伏地形上保持机身 pitch / roll 水平，同时稳定跟踪线速度与偏航速度
> **文档依据**：当前工作区代码 + 2026-10-28 两次实际运行的 `params/env.yaml`、`params/agent.yaml`、`train_smooth*.log`
> **文档更新时间**：2026-11-02
## 1. 任务定位
平稳性训练是 Stage-1 粗糙地形策略之上的微调任务。动作空间、观测维度、机器人模型、质量/接触随机化都保持不变，因此可以从 Stage-1 checkpoint 直接 resume；变化集中在“地形”和“姿态平稳性奖励”两处。
### 1.1 与 Stage-1（Rough）的关键差异
下表按当前工作区代码对比；实际运行的参数以各运行目录下的 `params/env.yaml` 为准。
| 类别 | Stage-1 Rough | 平稳性 Smooth |
| --- | --- | --- |
| 地形 | 台阶、箱子、随机粗糙、斜坡混合 | 100% 低频正弦平滑起伏，无台阶、无尖边 |
| 地形初始化等级 | `max_init_terrain_level = 2` | `max_init_terrain_level = 9` |
| 重置位姿 | x/y ±1.0 m，z 偏移 0，roll/pitch ±0.3 rad | x/y ±2.0 m，z 偏移 +0.30 m，roll/pitch ±0.05 rad |
| 姿态惩罚 | `flat_orientation_l2 = -0.35`、`roll_l2 = -0.45` | `flat_orientation_l2` 置 0；`roll_l2 = -0.4`、`pitch_l2 = -0.4` |
| 机身角速度惩罚 | `ang_vel_xy_l2 = -0.05` | `ang_vel_xy_l2 = -0.1` |
速度指令分布（`x ∈ [-2, 2] m/s`、`y = 0`、`yaw ∈ [-1.5, 1.5] rad/s`、heading 指令开启、2% 站立环境）在两种地形下保持一致。
### 1.2 平稳性由哪些设置共同保证
| 机制 | 具体设置 | 作用 |
| --- | --- | --- |
| 俯仰惩罚 | `pitch_l2 = -0.4`，弧度平方 | 直接压低机头/机尾俯仰角 |
| 横滚惩罚 | `roll_l2 = -0.4`，弧度平方 | 直接压低左右横滚角 |
| 角速度阻尼 | `ang_vel_xy_l2 = -0.1` | 抑制 roll/pitch 方向的晃动速度 |
| 竖直速度惩罚 | `lin_vel_z_l2 = -2.0` | 抑制上下弹跳 |
| 竖直姿态奖励 | `upward = +0.08`，`(1 - g_z)²` | 机身越接近水平奖励越高（直立时 g_z = -1，项值最大） |
| 机身高度约束 | `base_height_l2 = -0.5`，目标 0.45 m（相对机身下方地形均值） | 防止趴下或过度抬升 |
| 膝内折约束 | `knee_inward_fold_penalty = -1.0`，`max_inward_tilt = 0.0` | 膝不能向内侧折过竖直位置 |
| 倾倒终止 | `bad_orientation_2`，倾角超过 0.9 rad 终止 | 直接淘汰失稳策略 |
| 地形 | 平滑正弦起伏，无台阶冲击 | 只施加连续的 pitch/roll 扰动，专练平稳性 |
## 2. 相关文件
| 内容 | 路径 |
| --- | --- |
| 任务注册 | `source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py` |
| 平稳性环境配置 | `.../deeprobotics_m20/smooth_env_cfg.py` |
| 父类环境配置 | `.../deeprobotics_m20/rough_env_cfg.py` |
| PPO 配置 | `.../deeprobotics_m20/agents/rsl_rl_ppo_cfg.py` |
| 平地/基础环境 | `source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py` |
| 平稳地形生成 | `.../locomotion/velocity/mdp/terrain.py` |
| 自定义奖励/观测 | `.../locomotion/velocity/mdp/rewards.py`、`observations.py` |
| 机器人模型与执行器 | `source/rl_training/rl_training/assets/deeprobotics.py` |
| URDF | `deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf` |
## 3. 实际训练运行记录
本次平稳性训练由两次运行接力完成，日志与 checkpoint 都保存在 `logs/rsl_rl/huoyi_smooth/`。
### 3.1 运行 A：小地形试跑（2026-10-28_16-50-47）
| 项目 | 值 |
| --- | --- |
| 起点 checkpoint | `/home/suyufei/syf/huoyi_training/logs/rsl_rl/deeprobotics_m20_rough/2026-10-28_10-23-40_stage1_readapt_realmass/model_24800.pt` |
| 并行环境数 | 2048 |
| 迭代数 | 50 |
| GPU | `cuda:2` |
| 地形参数 | `height_range=(0.02, 0.05)`、`wavelength_range=(1.5, 3.0)`、`max_init_terrain_level=1` |
| 姿态奖励 | `flat_orientation_l2 = -0.8`（未拆分 roll/pitch） |
| 结束状态 | 迭代 24849，mean reward 60.34，mean episode length 919.83 |
| 产出 | `model_24800.pt`、`model_24849.pt` |
运行 A 的目的：先在小起伏、低地形等级上确认从粗糙地形策略切换过来不会崩，同时保留一份可回退的中间策略。
### 3.2 运行 B：正式平稳性训练（2026-10-28_18-16-44）
| 项目 | 值 |
| --- | --- |
| 起点 checkpoint | `logs/rsl_rl/huoyi_smooth/2026-10-28_16-50-47/model_24849.pt` |
| 并行环境数 | 4096 |
| 迭代数 | 2500（从 24849 续到 27348） |
| GPU | `cuda:0` |
| 地形参数 | `height_range=(0.10, 0.20)`、`wavelength_range=(1.0, 2.2)`、`max_init_terrain_level=9` |
| 姿态奖励 | `flat_orientation_l2 = 0`；`roll_l2 = -0.4`、`pitch_l2 = -0.4`；`ang_vel_xy_l2 = -0.1` |
| 总样本量 | 245,760,000 steps（2500 × 24 × 4096） |
| 训练耗时 | 约 2 小时 57 分（Iteration time 约 3.09–3.20 s） |
| 最终产出 | `model_27348.pt`，以及 `exported/policy.pt`、`exported/policy.onnx` |
运行 B 才是本任务的有效训练：地形起伏幅度、波长密度和初始地形等级都明显提高，逼迫策略在连续起伏上主动调平机身。
### 3.3 复现命令
运行 A（小地形试跑）：
```bash
python scripts/reinforcement_learning/rsl_rl/train.py \
--task Smooth-Deeprobotics-M20-v0 \
--num_envs 2048 \
--max_iterations 50 \
--headless \
--resume \
--load_run /home/suyufei/syf/huoyi_training/logs/rsl_rl/deeprobotics_m20_rough/2026-10-28_10-23-40_stage1_readapt_realmass \
--checkpoint /home/suyufei/syf/huoyi_training/logs/rsl_rl/deeprobotics_m20_rough/2026-10-28_10-23-40_stage1_readapt_realmass/model_24800.pt
```
运行 B（从运行 A 续训）：
```bash
python scripts/reinforcement_learning/rsl_rl/train.py \
--task Smooth-Deeprobotics-M20-v0 \
--num_envs 4096 \
--max_iterations 2500 \
--headless \
--resume \
--load_run /home/suyufei/syf/huoyi_training_level/logs/rsl_rl/huoyi_smooth/2026-10-28_16-50-47 \
--checkpoint /home/suyufei/syf/huoyi_training_level/logs/rsl_rl/huoyi_smooth/2026-10-28_16-50-47/model_24849.pt
```
说明：
- 恢复训练时策略/价值网络输入维度必须一致（本任务 policy 33 维、critic 223 维、动作 8 维）。
- `--checkpoint` 是脚本实际使用的参数名；日志 `params/agent.yaml` 中对应字段名为 `load_checkpoint`。
- 不传 `--max_iterations` 时使用配置默认值 5000；上述两次运行都用命令行覆盖了默认值。
- 环境种子默认 42，实际运行未覆盖。
### 3.4 播放与导出
```bash
python scripts/reinforcement_learning/rsl_rl/play.py \
--task Smooth-Deeprobotics-M20-v0 \
--num_envs 16 \
--checkpoint /home/suyufei/syf/huoyi_training_level/logs/rsl_rl/huoyi_smooth/2026-10-28_18-16-44/model_27348.pt
```
## 4. 仿真与场景设置
| 项目 | 值 |
| --- | --- |
| 物理步长 `sim.dt` | 0.005 s（200 Hz） |
| 控制抽帧 `decimation` | 4 |
| 控制步长 | 0.02 s（50 Hz） |
| 回合长度 `episode_length_s` | 20 s（1000 个控制步） |
| 并行环境数 `num_envs` | 4096（正式训练） |
| 环境间距 `env_spacing` | 2.5 m |
| 随机种子 | 42 |
| 设备 | `cuda:0` |
| 重力 | `(0, 0, -9.81)` |
| PhysX 位置/速度迭代 | 4 / 1 |
| CCD、稳定性增强 | 均关闭 |
| 地形物理材质 | 静摩擦 1.0、动摩擦 1.0、恢复系数 0.0，摩擦/恢复均为 multiply |
| 渲染间隔 | 4 个物理步 |
| 接触传感器更新周期 | 0.005 s，历史长度 3，`track_air_time=True` |
| 高度扫描更新周期 | 0.02 s |
实测运行环境：IsaacLab 2.3.2 / Isaac Sim 5.1，8 × RTX 4090（正式训练使用 0 号卡），72 核 Xeon Platinum 8352V，约 512 GB 内存。
## 5. 机器人模型
| 项目 | 值 |
| --- | --- |
| URDF | `deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf` |
| 自由度 | 8（4 个膝关节 + 4 个轮关节） |
| 整机仿真质量 | 约 23.69 kg |
| 轮半径 | 0.11 m（奖励计算同样使用 0.11） |
| 轮距 | 0.50 m（奖励计算同样使用 0.50） |
| 根初始高度 | (0, 0, 0.32) m |
| 初始关节角 | 前膝 `+0.5 rad`，后膝 `-0.5 rad`，轮 `0` |
| 软关节限位系数 | 0.9 |
| 自碰撞 | 关闭 |
### 5.1 关节顺序
```text
膝关节：fl_knee_joint, fr_knee_joint, hl_knee_joint, hr_knee_joint
轮关节：fl_wheel_joint, fr_wheel_joint, hl_wheel_joint, hr_wheel_joint
```
### 5.2 执行器（DelayedPDActuator）
| 参数 | 膝关节 | 轮关节 |
| --- | ---: | ---: |
| `effort_limit` | 75.0 N·m | 28.68 N·m |
| `velocity_limit` | 17.8024 rad/s | 109.956 rad/s |
| `stiffness` | 80.0 | 0.0 |
| `damping` | 2.0 | 0.6 |
| `friction` | 0.0 | 1.0 |
| `armature` | 0.0 | 0.00243216 |
| 动作延迟 | 0–5 个物理步 | 0–5 个物理步 |
膝关节是位置控制，轮关节是纯速度控制（stiffness 为 0）。执行器的刚度、阻尼会在每次 reset 时按 0.85–1.15 缩放。
## 6. 动作空间
动作为 8 维，四膝位置 + 四轮速度：
| 动作项 | 关节 | 缩放 | 默认偏置 | 裁剪 | 说明 |
| --- | --- | ---: | --- | --- | --- |
| `joint_pos` | 4 个膝关节 | 0.25 | `use_default_offset=True` | ±100 | 目标角 = 默认角 + 0.25 × 动作 |
| `joint_vel` | 4 个轮关节 | 5.0 | 默认 0 | ±100 | 目标轮速 = 5.0 × 动作 (rad/s) |
动作顺序被 `preserve_order=True` 固定，分别对应配置中的膝关节顺序与轮关节顺序，避免按正则匹配后顺序漂移导致 resume 时动作错位。
## 7. 地形设置
平稳性地形由 `mdp/terrain.py` 中的 `smooth_random_terrain` 生成，核心是一个“多个低频正弦波叠加”的连续高度场。
### 7.1 生成公式
对每个地形块：
1. 在 `8 m × 8 m` 的网格上生成坐标（水平分辨率 0.05 m）；
2. 叠加 `num_waves = 3` 个正弦波，每个波随机采样：
- 方向 `direction ~ U(0, 2π)`
- 波长 `wavelength ~ U(1.0, 2.2) m`
- 相位 `phase ~ U(0, 2π)`
- 权重 `weight ~ U(0.5, 1.0)`
3. 减去高度均值；
4. 按 `max(|h|)` 归一化到当前课程幅度 `amplitude`；
5. 按 `vertical_scale = 0.005 m` 量化为 int16 高度场。
课程幅度：
```text
amplitude = 0.10 + difficulty × (0.20 - 0.10) # difficulty ∈ [0, 1]
```
即课程起点起伏峰值 0.10 m，终点 0.20 m。地形没有台阶和尖锐边缘，机器人受到的是连续的 pitch/roll 扰动。
### 7.2 地形参数
| 参数 | 值 |
| --- | --- |
| 地形块尺寸 | 8.0 m × 8.0 m |
| 行数 × 列数 | 10 × 20 |
| 边界宽度 / 高度 | 2.0 m / 1.0 m |
| 水平分辨率 | 0.05 m |
| 垂直分辨率 | 0.005 m |
| 是否缓存 | 否（`use_cache=False`） |
| 课程 | 开启（`curriculum=True`），难度 0 → 1 |
| 子地形比例 | `smooth_random` = 1.0 |
| 起伏峰值范围 | 0.10 → 0.20 m（课程起止） |
| 波长范围 | 1.0 → 2.2 m |
| 正弦波数量 | 3 |
| 子地形边界宽度 | 0.25 m |
| 初始地形等级上限 | 9（10 行中的最高等级） |
### 7.3 地形课程规则
使用 IsaacLab 标准的 `terrain_levels_vel`：
- **升级**：本回合行走距离 > 地形块尺寸的一半（4 m）；
- **降级**：行走距离 < 指令速度 × 20 s × 0.5；
- 每个环境初始等级从 `[0, max_init_terrain_level]` 均匀采样。
正式训练开始时平均地形等级约 4.95，结束时升至 5.96（满级 9），说明策略在持续向更难地形推进。
### 7.4 地形材质
| 参数 | 值 |
| --- | --- |
| 静摩擦 | 1.0 |
| 动摩擦 | 1.0 |
| 恢复系数 | 0.0 |
| 摩擦组合方式 | multiply |
| 恢复组合方式 | multiply |
轮子材质另有随机化（见第 11 节），因此轮地有效摩擦主要由轮子材质范围决定。
## 8. 观测空间
采用非对称 Actor-Critic：策略只看本体感知，价值网络额外看地形高度扫描。
### 8.1 Policy 观测（33 维）
| 序号 | 观测项 | 维度 | 噪声 | 缩放 | 说明 |
| ---: | --- | ---: | --- | ---: | --- |
| 0 | `base_ang_vel` | 3 | U(-0.2, 0.2) | 0.25 | 机身角速度 |
| 1 | `projected_gravity` | 3 | U(-0.05, 0.05) | 1.0 | 重力在机体系的投影，直接反映 pitch/roll |
| 2 | `velocity_commands` | 3 | 无 | 1.0 | 线速度 x、y 与偏航速度指令 |
| 3 | `joint_pos` | 8 | U(-0.01, 0.01) | 1.0 | 4 个膝相对角 + 4 个轮关节占位（轮位置恒置 0） |
| 4 | `joint_vel` | 8 | U(-1.5, 1.5) | 0.05 | 4 膝 + 4 轮关节速度 |
| 5 | `actions` | 8 | 无 | 1.0 | 上一步动作 |
合计 3 + 3 + 3 + 8 + 8 + 8 = 33 维。
**策略是“盲”的**：`base_lin_vel` 和 `height_scan` 在 policy 组中被显式置空，机器人只能依靠 IMU、关节编码器和指令来保持平稳，不能直接“看到”地形。
### 8.2 Critic 观测（223 维）
| 序号 | 观测项 | 维度 | 说明 |
| ---: | --- | ---: | --- |
| 0 | `base_lin_vel` | 3 | 机身线速度（特权信息） |
| 1 | `base_ang_vel` | 3 | 机身角速度 |
| 2 | `projected_gravity` | 3 | 重力投影 |
| 3 | `velocity_commands` | 3 | 速度指令 |
| 4 | `joint_pos` | 8 | 关节位置 |
| 5 | `joint_vel` | 8 | 关节速度 |
| 6 | `actions` | 8 | 上一步动作 |
| 7 | `height_scan` | 187 | 机身下方 1.6 m × 1.0 m 地形高度网格（17 × 11，分辨率 0.1 m），裁剪到 ±1.0 |
critic 组不注入噪声。合计 3 + 3 + 3 + 3 + 8 + 8 + 8 + 187 = 223 维。
### 8.3 传感器
| 传感器 | 配置 |
| --- | --- |
| `contact_forces` | 覆盖 `/World/envs/env_.*/Robot/.*`，更新周期 0.005 s，历史长度 3，记录 air time |
| `height_scanner` | 挂在 `base_link`，从上方 20 m 向下投射，1.6 m × 1.0 m，分辨率 0.1 m，yaw 对齐，187 个点 |
| `height_scanner_base` | 挂在 `base_link`，0.1 m × 0.1 m，分辨率 0.05 m，仅用于 `base_height_l2` 估计脚下地形高度 |
## 9. 奖励设置（22 项）
奖励项权重与最终训练日志值如下。`Episode_Reward` 是 IsaacLab 奖励管理器的日志量：回合累积奖励（已按 `dt = 0.02` 积分）除以 20 s，因此它与权重和回合长度都相关，只适合看趋势和相对大小。
### 9.1 姿态平稳性
| 奖励项 | 权重 | 关键参数 | 末次日志值 | 作用 |
| --- | ---: | --- | ---: | --- |
| `lin_vel_z_l2` | -2.0 | 无 | -0.1122 | 抑制机身竖直方向弹跳 |
| `ang_vel_xy_l2` | -0.1 | 无 | -0.0815 | 抑制 roll/pitch 角速度 |
| `roll_l2` | -0.4 | 欧拉角 roll，弧度平方 | -0.0043 | 直接惩罚横滚角 |
| `pitch_l2` | -0.4 | 欧拉角 pitch，弧度平方 | -0.0049 | 直接惩罚俯仰角 |
| `base_height_l2` | -0.5 | 目标高度 0.45 m，`height_scanner_base` 估计脚下地形 | -0.0208 | 维持机身高度 |
| `upward` | +0.08 | `(1 - g_z)²`，直立时 g_z = -1 | +0.2872 | 机身越水平奖励越高 |
`flat_orientation_l2` 在平稳性任务中被置 0（不参与训练），由 `roll_l2` + `pitch_l2` 取代。运行 A 及更早的设置中 `flat_orientation_l2` 为 -0.8；正式训练把它拆成 -0.4 / -0.4，便于分别调节俯仰和横滚。
### 9.2 速度与偏航跟踪
| 奖励项 | 权重 | 关键参数 | 末次日志值 | 作用 |
| --- | ---: | --- | ---: | --- |
| `track_lin_vel_xy_exp` | +2.0 | std = 0.7071（即 √0.5） | +1.6189 | 跟踪 xy 线速度 |
| `track_ang_vel_z_exp` | +1.5 | std = 0.5 | +1.0915 | 跟踪偏航角速度 |
| `track_diff_wheel_yaw_exp` | +1.0 | std = 0.6，轮半径 0.11，轮距 0.50，左轮 (0,2)，右轮 (1,3)，指令阈值 0.05 | +0.5449 | 用左右轮速差估计偏航，鼓励差速转向 |
| `lin_vel_xy_yaw_command_l2` | -0.4 | 偏航阈值 0.25、线速度阈值 0.15 | -0.0012 | 原地转向时惩罚线速度漂移 |
### 9.3 关节、动作与功率
| 奖励项 | 权重 | 作用关节 | 末次日志值 | 作用 |
| --- | ---: | --- | ---: | --- |
| `joint_torques_l2` | -2.5e-5 | 4 膝 | -0.0356 | 抑制膝力矩 |
| `joint_acc_l2` | -3e-6 | 4 膝 | -0.1570 | 抑制膝加速度 |
| `joint_acc_wheel_l2` | -1e-7 | 4 轮 | -0.0413 | 轻微抑制轮加速度 |
| `joint_pos_limits` | -5.0 | 4 膝 | -0.0000 | 关节限位惩罚 |
| `joint_power` | -2e-5 | 4 膝 | -0.0014 | 抑制膝功率 |
| `knee_joint_pos_penalty` | -0.1 | 4 膝 | -0.0751 | 站立或低速时把膝拉回默认姿态（`stand_still_scale=5`、速度阈值 0.5、指令阈值 0.1） |
| `knee_inward_fold_penalty` | -1.0 | 前膝 / 后膝 | -0.0002 | 禁止膝向内折过竖直位置（`max_inward_tilt = 0.0`） |
| `action_rate_l2` | -0.015 | 全部动作 | -0.1226 | 动作平滑 |
| `stand_still` | -2.0 | 4 膝 | -0.0495 | 站立指令下关节偏离默认姿态的惩罚 |
`knee_inward_fold_penalty` 的方向约定：膝关节角为 0 时膝连杆竖直向下；前膝角减小为内折，后膝角增大为内折。`max_inward_tilt = 0.0` 表示不允许越过竖直位置，向外折叠仍由关节限位约束。
### 9.4 接触与轮地
| 奖励项 | 权重 | 关键参数 | 末次日志值 | 作用 |
| --- | ---: | --- | ---: | --- |
| `undesired_contacts` | -1.0 | 除轮子外的所有 body，力阈值 1.0 N | -0.0002 | 禁止机身、膝等部位触地拖行 |
| `contact_forces` | -1.5e-4 | 4 个轮子，阈值 100 N | -0.0166 | 抑制轮地过大冲击力 |
| `feet_contact_without_cmd` | +0.1 | 4 个轮子 | +0.0069 | 无速度指令时保持轮子着地 |
### 9.5 被关闭的奖励项
以下奖励项权重为 0，训练时被移除或置空：`is_terminated`、`flat_orientation_l2`、`body_lin_acc_l2`、`joint_vel_l2`、`joint_vel_wheel_l2`、`joint_torques_wheel_l2`、`joint_vel_limits`、`feet_air_time`、`feet_air_time_variance`、`feet_contact`、`feet_stumble`、`feet_slide`、`feet_height`、`feet_height_body`、`feet_distance_y_exp`、`feet_gait`、`wheel_vel_penalty`、`joint_mirror`、`action_mirror`、`action_sync`、`applied_torque_limits`、`joint_deviation_l1`、`stand_still_without_cmd`、`joint_pos_penalty`、`hipx_joint_pos_penalty`、`hipy_joint_pos_penalty`、`command_progress`、`support_contact_progress`、`support_contact_excess`。
注意：平稳性任务没有 `illegal_contact` 终止，机身/膝触地不会立即结束回合，而是通过 `undesired_contacts` 持续扣分；这与 Stage-2 允许短时支撑的设计不同。
## 10. 终止条件
| 终止项 | 参数 | `time_out` | 说明 |
| --- | --- | --- | --- |
| `time_out` | 20 s | 是 | 正常回合结束 |
| `terrain_out_of_bounds` | 距地形块原点 3.0 m 缓冲 | 是 | 跑出当前地形块，按超时处理，不计为摔倒 |
| `bad_orientation_2` | 倾角上限 0.9 rad（约 51.6°） | 否 | 机身过度倾斜，判定失稳 |
| `illegal_contact` | — | — | 已禁用，机身/膝触地不终止 |
`bad_orientation_2` 使用 `mdp.bad_orientation`，比较机身 z 轴与重力方向的夹角。它和 `roll_l2`、`pitch_l2` 形成“软惩罚 + 硬终止”的双层约束。
## 11. 速度指令
| 参数 | 值 |
| --- | --- |
| 指令类 | `UniformThresholdVelocityCommand` |
| 重采样周期 | 10 s |
| `lin_vel_x` | -2.0 ~ 2.0 m/s |
| `lin_vel_y` | 0.0（不允许横移） |
| `ang_vel_z` | -1.5 ~ 1.5 rad/s |
| heading 指令 | 开启，`heading_control_stiffness = 0.5` |
| heading 范围 | -π ~ π |
| 站立环境比例 | 2% |
| heading 环境比例 | 100% |
`UniformThresholdVelocityCommand` 会在采样后把平面线速度模长 ≤ 0.2 m/s 的指令清零，因此低速线速度指令会被视为 0；偏航速度不受此阈值影响。这保证了“要么有明显前进/后退，要么原地转向”，减少模棱两可的小速度指令。
## 12. 域随机化设置
所有事件均由父类 `EventCfg` 继承，平稳性任务未额外修改（除重置位姿在 `smooth_env_cfg.py` 中覆盖）。
| 事件 | 模式 | 对象 | 参数 |
| --- | --- | --- | --- |
| `randomize_rigid_body_mass` | startup | 除 `base_link` 外的所有 body | 质量 scale 0.85–1.15，重算惯量 |
| `randomize_rigid_body_mass_base` | startup | `base_link` | 质量 add -1.0 ~ +3.0 kg，重算惯量 |
| `randomize_rigid_body_inertia` | startup | 所有 body | 惯量 scale 0.85–1.15 |
| `randomize_com_positions` | startup | `base_link` | COM x/y ±0.03 m，z ±0.02 m |
| `randomize_apply_external_force_torque` | reset | `base_link` | 力 ±10 N，力矩 ±10 N·m |
| `randomize_reset_joints` | reset | 全部关节 | 位置 scale 1.0–1.0（等同不随机），速度 0 |
| `randomize_actuator_gains` | reset | 4 膝 + 4 轮 | 刚度、阻尼均 scale 0.85–1.15，均匀分布 |
| `randomize_reset_base` | reset | 机器人根 | 见下方重置位姿 |
| `randomize_push_robot` | interval 10–15 s | 机器人根 | x/y 速度各 ±0.5 m/s |
| `randomize_wheel_material` | startup | `.*_wheel` | 静摩擦 0.9–1.4，动摩擦 0.6–0.9，恢复 0–0.05，1024 桶 |
| `randomize_support_material` | startup | `base_link`、`.*_knee` | 静摩擦 0.4–0.8，动摩擦 0.25–0.4，恢复 0–0.05，1024 桶 |
| `randomize_rigid_body_material` | — | 全部 body | 已禁用，由上面两个材质事件替代 |
### 12.1 重置位姿
| 轴 | 范围 |
| --- | --- |
| x | -2.0 ~ 2.0 m（相对默认根位置偏移） |
| y | -2.0 ~ 2.0 m（相对默认根位置偏移） |
| z | +0.30 m（相对默认根高度的偏移） |
| roll | -0.05 ~ 0.05 rad |
| pitch | -0.05 ~ 0.05 rad |
| yaw | -π ~ π |
| 线速度 x | -0.1 ~ 0.1 m/s |
| 其余速度 | 0 |
注意：IsaacLab 的 `reset_root_state_uniform` 把 `z` 当作**相对默认根状态的偏移**。机器人 `init_state` 的根高度是 0.32 m，因此重置时实际高度约为 0.32 + 0.30 = 0.62 m（相对地形原点），会先经历一次落地再进入正常控制。若希望原地生成，应把 z 改为 0.0。
## 13. PPO 超参数
| 参数 | 值 |
| --- | --- |
| Runner | `OnPolicyRunner`（RSL-RL） |
| 策略结构 | ActorCritic，MLP 512-256-128，ELU |
| Actor 输入 / 输出 | 33 / 8 |
| Critic 输入 / 输出 | 223 / 1 |
| 初始噪声标准差 | 1.0，`noise_std_type="log"` |
| `num_steps_per_env` | 24（单环境 rollout 0.48 s） |
| 每轮样本数 | 24 × 4096 = 98,304 |
| `max_iterations` | 2500（命令行覆盖；配置默认 5000） |
| `save_interval` | 100 |
| `num_learning_epochs` | 5 |
| `num_mini_batches` | 4（mini-batch 24,576 样本） |
| 学习率 | 1e-3，adaptive schedule，`desired_kl = 0.01` |
| 折扣 / GAE | `gamma = 0.99`，`lam = 0.95` |
| PPO clip | 0.2 |
| 熵系数 | 0.01 |
| 价值损失系数 | 1.0，使用 clipped value loss |
| 梯度裁剪 | 1.0 |
| 观测归一化 | 关闭（`empirical_normalization=False`） |
| 动作裁剪 | ±100 |
| 日志 | TensorBoard |
| 实验名 | `huoyi_smooth` |
| 随机种子 | 42 |
单轮耗时约 3.1–3.2 s，采集速度约 8.8k steps/s，2500 轮合计约 2 小时 57 分。
## 14. 训练结果（运行 B，迭代 27348）
| 指标 | 值 |
| --- | --- |
| 有效迭代 | 2500（24849 → 27348） |
| 总样本量 | 245,760,000 steps |
| 训练耗时 | 2 小时 57 分 08 秒 |
| 平均回合奖励 | 55.95 |
| 平均回合长度 | 886.40 步（约 17.73 s / 20 s） |
| 平均地形等级 | 5.96 / 9 |
| 线速度跟踪误差 `error_vel_xy` | 0.3823 m/s |
| 偏航速度跟踪误差 `error_vel_yaw` | 0.4167 rad/s |
| `time_out` 终止比例 | 80.10% |
| `terrain_out_of_bounds` 比例 | 18.89% |
| `bad_orientation_2` 比例 | 1.04% |
| 价值损失 | 0.0929 |
| 策略替代损失 | 0.0028 |
| 熵损失 | 7.4557 |
| 平均动作噪声标准差 | 0.68 |
### 14.1 奖励分解（末次日志）
正向项合计约 +3.55，负向项合计约 -0.72，主要贡献来自：
| 奖励项 | 末次日志值 |
| --- | ---: |
| `track_lin_vel_xy_exp` | +1.6189 |
| `track_ang_vel_z_exp` | +1.0915 |
| `track_diff_wheel_yaw_exp` | +0.5449 |
| `upward` | +0.2872 |
| `feet_contact_without_cmd` | +0.0069 |
| `joint_acc_l2` | -0.1570 |
| `action_rate_l2` | -0.1226 |
| `lin_vel_z_l2` | -0.1122 |
| `ang_vel_xy_l2` | -0.0815 |
| `knee_joint_pos_penalty` | -0.0751 |
| `stand_still` | -0.0495 |
| `joint_acc_wheel_l2` | -0.0413 |
| `joint_torques_l2` | -0.0356 |
| `base_height_l2` | -0.0208 |
| `contact_forces` | -0.0166 |
| `pitch_l2` | -0.0049 |
| `roll_l2` | -0.0043 |
| 其他 | 均接近 0 |
### 14.2 关于 resume 首轮的“异常低值”
运行 B 的第一条日志（迭代 24849）显示 mean reward -0.94、mean episode length 12.99。这是 resume 时 `init_at_random_ep_len=True` 造成的统计瞬态：所有环境被随机放到回合计时器的任意位置，第一轮 rollout 内瞬间完成的“残余回合”被计入均值，并不代表策略崩溃。后续迭代迅速恢复到正常运行区间。
### 14.3 与运行 A 的对比
| 指标 | 运行 A（小地形） | 运行 B（正式） |
| --- | ---: | ---: |
| 并行环境 | 2048 | 4096 |
| 地形峰值 | 0.02–0.05 m | 0.10–0.20 m |
| 初始地形等级上限 | 1 | 9 |
| 平均奖励 | 60.34 | 55.95 |
| 平均回合长度 | 919.83 | 886.40 |
运行 A 的奖励更高，但地形明显更容易，两者不能直接比较；运行 B 是在更难的起伏地形和更高初始等级下取得的 55.95。
## 15. 调参要点与注意事项
1. **地形难度要分级推进。** 运行 A（0.02–0.05 m、初始等级 1）先确认策略不崩，运行 B 再切到 0.10–0.20 m、初始等级 9。直接从大起伏开始容易出现早期大量 `bad_orientation`。
2. **roll / pitch 分开调。** 平稳性任务把原来的 `flat_orientation_l2` 拆成 `roll_l2` 与 `pitch_l2`，当前各 -0.4；如果某一轴仍晃动，可单独加大该轴权重，而不会同时把另一轴压得过死。
3. **角速度惩罚不要太小。** `ang_vel_xy_l2` 从 Stage-1 的 -0.05 提到 -0.1，是抑制“角度不大但持续晃”的关键项。
4. **resume 时不要改观测/动作维度或顺序。** 当前契约为 policy 33 维、critic 223 维、动作 8 维，且膝关节顺序被 `preserve_order=True` 固定；改顺序会导致动作错位但训练不报错。
5. **`base_height_l2` 的 0.45 m 是相对 `height_scanner_base` 所测脚下地形均值的高度。** 修改该扫描器的尺寸或分辨率会改变高度目标的实际含义。
6. **重置 z 是偏移量。** 当前 z = +0.30 m 是叠加在默认根高度 0.32 m 上的偏移，实际约 0.62 m；若不需要落地过程，应改为 0.0。
7. **奖励里的轮半径 0.11 m、轮距 0.50 m 要与 URDF 同步。** `track_diff_wheel_yaw_exp` 用这两个常数把轮速差换算成偏航角速度，URDF 改轮径后必须同步修改。
8. **`undesired_contacts` 阈值只有 1.0 N。** 机身、膝几乎一碰地就会扣分，这是“不允许趴地爬行”的强约束；若发现策略过度保守，可先检查这一项。
9. **`terrain_out_of_bounds` 占 18.89%。** 它被标记为 `time_out=True`，不算摔倒，但会截断回合。8 m × 8 m 地形块配合最高 2 m/s 的指令，高速直行时容易跑出边界；若希望提高有效回合长度，可增大块尺寸或 `distance_buffer`。
10. **策略是盲的。** `base_lin_vel` 和 `height_scan` 只在 critic 中；部署时不需要地形感知，但也意味着策略完全依赖 IMU 与关节反馈来保持平稳。
11. **控制频率与延迟。** 50 Hz 控制、执行器 0–5 个物理步延迟（0–25 ms），仿真里的平稳性结论不能直接等同于实机，实机部署前应确认延迟与电机响应。
12. **不要用运行 A 的奖励值判断平稳性优劣。** 两次运行的地形分布不同，平均奖励不可直接横比。
## 16. 产物与日志
| 内容 | 路径 |
| --- | --- |
| 运行 A 目录 | `logs/rsl_rl/huoyi_smooth/2026-10-28_16-50-47/` |
| 运行 B 目录 | `logs/rsl_rl/huoyi_smooth/2026-10-28_18-16-44/` |
| 最终 checkpoint | `logs/rsl_rl/huoyi_smooth/2026-10-28_18-16-44/model_27348.pt` |
| 导出策略 | `.../2026-10-28_18-16-44/exported/policy.pt`、`policy.onnx` |
| 运行参数 | `.../2026-10-28_18-16-44/params/env.yaml`、`agent.yaml` |
| TensorBoard | `.../2026-10-28_18-16-44/events.out.tfevents.*` |
| 代码快照 | `.../2026-10-28_18-16-44/git/IsaacLab-2.3.2.diff`、`huoyi_training_level.diff` |
| 运行 A 日志 | `train_smooth50.log` |
| 运行 B 日志 | `train_smooth4096_1028_1816.log` |
| 未进入训练的启动日志 | `train_smooth4096_1028_1812.log`（启动阶段退出，未产生迭代） |
checkpoint 命名与迭代号对应：`model_24900.pt` … `model_27348.pt`，每 100 轮保存一次，最后一次为 27348。
## 17. 可选的后续实验
以下仅为建议，未在本次训练中执行：
1. 把地形峰值从 0.20 m 提高到 0.25 m，或把波长下限继续降到 0.8 m，观察 `bad_orientation` 与 `error_vel_xy` 的变化。
2. 针对 `terrain_out_of_bounds` 偏高的问题，尝试 10 m × 10 m 地形块或更大的 `distance_buffer`，减少高速直行被截断。
3. 在保持 policy 观测不变的前提下，尝试把 `ang_vel_xy_l2` 从 -0.1 调到 -0.15，观察横滚/俯仰速度是否进一步下降。
4. 单独统计 roll、pitch 的 RMS 与峰值，作为“平稳性”的量化指标，而不是只看总奖励。
5. 若后续 URDF 轮径或轮距变化，同步更新 `track_diff_wheel_yaw_exp` 的 `wheel_radius`、`track_width`，并重新校准 `base_height_l2` 的目标高度。
---
**文档维护**：修改 `smooth_env_cfg.py`、`rough_env_cfg.py`、`terrain.py`、`rewards.py` 或 PPO 配置后，请同步更新本文对应章节，并在“实际训练运行记录”中追加新的运行目录与关键指标。
eval_kit/profiles.py
+1 -1
View File
@@ -16,7 +16,7 @@ from eval_kit.core.schema import RobotProfile
SOLENOPSIS = RobotProfile( SOLENOPSIS = RobotProfile(
name="solenopsis", name="solenopsis",
# None = 运行时从仿真资产读真实质量（Solenopsis.urdf 当前为 26.581 kg）。 # None = 运行时从仿真资产读真实质量（Solenopsis.urdf 当前为 24.74 kg）。
# 旧 eval.py 的命令行默认值是 25.482，那是更早的 M20 简化模型名义质量， # 旧 eval.py 的命令行默认值是 25.482，那是更早的 M20 简化模型名义质量，
# 会让 COT 偏低约 4%。 # 会让 COT 偏低约 4%。
mass_kg=None, mass_kg=None,
source/rl_training/rl_training/assets/deeprobotics.py
+2 -1
View File
@@ -65,6 +65,7 @@ DEEPROBOTICS_LITE3_CFG = ArticulationCfg(
DEEPROBOTICS_M20_CFG = ArticulationCfg( DEEPROBOTICS_M20_CFG = ArticulationCfg(
spawn=sim_utils.UrdfFileCfg( spawn=sim_utils.UrdfFileCfg(
asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/M20/M20_urdf/urdf/Solenopsis.urdf", asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/M20/M20_urdf/urdf/Solenopsis.urdf",
usd_dir="/home/suyufei/tmp/IsaacLab_m20_wheel220",
fix_base=False, fix_base=False,
merge_fixed_joints=True, merge_fixed_joints=True,
joint_drive=None, joint_drive=None,
@@ -88,7 +89,7 @@ DEEPROBOTICS_M20_CFG = ArticulationCfg(
), ),
), ),
init_state=ArticulationCfg.InitialStateCfg( init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.306), pos=(0.0, 0.0, 0.32),
joint_pos={ joint_pos={
"f[l,r]_knee_joint": 0.5, "f[l,r]_knee_joint": 0.5,
"h[l,r]_knee_joint": -0.5, "h[l,r]_knee_joint": -0.5,
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py
+22
View File
@@ -44,3 +44,25 @@ gym.register(
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg", "cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg",
}, },
) )
gym.register(
id="Rough-Deeprobotics-M20-Bridge-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.bridge_env_cfg:DeeproboticsM20BridgeEnvCfg",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsM20BridgePPORunnerCfg",
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg",
},
)
gym.register(
id="Smooth-Deeprobotics-M20-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.smooth_env_cfg:DeeproboticsM20SmoothEnvCfg",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsM20SmoothPPORunnerCfg",
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg",
},
)
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/agents/rsl_rl_ppo_cfg.py
+22
View File
@@ -54,3 +54,25 @@ class DeeproboticsM20SupportRoughPPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg):
super().__post_init__() super().__post_init__()
self.experiment_name = "deeprobotics_m20_support" self.experiment_name = "deeprobotics_m20_support"
@configclass
class DeeproboticsM20BridgePPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg):
"""Single-side bridge fine-tuning, forward-only command."""
def __post_init__(self):
super().__post_init__()
self.max_iterations = 5000
self.experiment_name = "huoyi_bridge"
@configclass
class DeeproboticsM20SmoothPPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg):
"""Smooth random-undulation fine-tuning, forward-only command."""
def __post_init__(self):
super().__post_init__()
self.max_iterations = 5000
self.experiment_name = "huoyi_smooth"
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/bridge_env_cfg.py
+76
View File
@@ -0,0 +1,76 @@
# Copyright (c) 2025 Deep Robotics
# SPDX-License-Identifier: BSD 3-Clause
"""Single-side bridge task: left legs on a long raised bridge, right legs on flat ground."""
from isaaclab.terrains import TerrainGeneratorCfg
from isaaclab.utils import configclass
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
from .rough_env_cfg import DeeproboticsM20RoughEnvCfg
@configclass
class DeeproboticsM20BridgeEnvCfg(DeeproboticsM20RoughEnvCfg):
"""Stage 1 policy on a long single-side bridge, forward-only command."""
def __post_init__(self):
super().__post_init__()
# ------------------------------Terrain------------------------------
# One long bridge cell type. The bridge runs along +x, so the robot can
# keep walking forward without leaving the split-level configuration.
self.scene.terrain.terrain_generator = TerrainGeneratorCfg(
size=(12.0, 4.0),
border_width=2.0,
num_rows=10,
num_cols=20,
horizontal_scale=0.1,
vertical_scale=0.005,
slope_threshold=0.75,
use_cache=False,
curriculum=True,
sub_terrains={
"single_side_bridge": mdp.SingleSideBridgeTerrainCfg(
proportion=1.0,
bridge_height_range=(0.06, 0.14),
bridge_width=1.5,
)
},
)
self.scene.terrain.max_init_terrain_level = 0
# ------------------------------Spawn------------------------------
# Spawn exactly on the bridge edge: y=0 and yaw=0 put the left wheels
# on the bridge and the right wheels on the flat ground.
self.events.randomize_reset_base.params = {
"pose_range": {
"x": (-1.0, 1.0),
"y": (0.0, 0.0),
"z": (0.30, 0.30),
"roll": (-0.03, 0.03),
"pitch": (-0.03, 0.03),
"yaw": (0.0, 0.0),
},
"velocity_range": {
"x": (-0.1, 0.1),
"y": (0.0, 0.0),
"z": (0.0, 0.0),
"roll": (0.0, 0.0),
"pitch": (0.0, 0.0),
"yaw": (0.0, 0.0),
},
}
# ------------------------------Commands------------------------------
# Forward-only: no lateral and no yaw command.
self.commands.base_velocity.heading_command = False
self.commands.base_velocity.rel_standing_envs = 0.0
self.commands.base_velocity.rel_heading_envs = 0.0
self.commands.base_velocity.ranges.lin_vel_x = (0.2, 0.8)
self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0)
self.commands.base_velocity.ranges.ang_vel_z = (0.0, 0.0)
# Keep the same 33-dim policy / 8-dim action layout as the other tasks.
self.disable_zero_weight_rewards()
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/rough_env_cfg.py
+39 -68
View File
@@ -7,8 +7,8 @@
from isaaclab.managers import CurriculumTermCfg as CurrTerm from isaaclab.managers import CurriculumTermCfg as CurrTerm
from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import RewardTermCfg as RewTerm
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import SceneEntityCfg from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.utils import configclass from isaaclab.utils import configclass
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
@@ -92,7 +92,7 @@ class DeeproboticsM20RewardsCfg(RewardsCfg):
params={ params={
"command_name": "base_velocity", "command_name": "base_velocity",
"std": 0.6, "std": 0.6,
"wheel_radius": 0.096, "wheel_radius": 0.11,
"track_width": 0.50, "track_width": 0.50,
"left_wheel_indices": (0, 2), "left_wheel_indices": (0, 2),
"right_wheel_indices": (1, 3), "right_wheel_indices": (1, 3),
@@ -132,7 +132,7 @@ class DeeproboticsM20RewardsCfg(RewardsCfg):
"wheel_sensor_cfg": SceneEntityCfg("contact_forces", body_names=""), "wheel_sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
"asset_cfg": SceneEntityCfg("robot"), "asset_cfg": SceneEntityCfg("robot"),
"wheel_asset_cfg": SceneEntityCfg("robot", joint_names=""), "wheel_asset_cfg": SceneEntityCfg("robot", joint_names=""),
"wheel_radius": 0.096, "wheel_radius": 0.11,
"support_force_threshold": 15.0, "support_force_threshold": 15.0,
"wheel_obstacle_force_threshold": 35.0, "wheel_obstacle_force_threshold": 35.0,
"max_contact_time": 0.45, "max_contact_time": 0.45,
@@ -161,6 +161,16 @@ class DeeproboticsM20RewardsCfg(RewardsCfg):
}, },
) )
knee_inward_fold_penalty = RewTerm(
func=mdp.knee_inward_fold_penalty,
weight=0.0,
params={
"front_asset_cfg": SceneEntityCfg("robot", joint_names=""),
"hind_asset_cfg": SceneEntityCfg("robot", joint_names=""),
"max_inward_tilt": 0.0,
},
)
@configclass @configclass
class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
@@ -188,6 +198,8 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
knee_joint_names = [ knee_joint_names = [
"fl_knee_joint", "fr_knee_joint", "hl_knee_joint", "hr_knee_joint", "fl_knee_joint", "fr_knee_joint", "hl_knee_joint", "hr_knee_joint",
] ]
front_knee_joint_names = ["fl_knee_joint", "fr_knee_joint"]
hind_knee_joint_names = ["hl_knee_joint", "hr_knee_joint"]
joint_names = leg_joint_names + wheel_joint_names joint_names = leg_joint_names + wheel_joint_names
# fmt: on # fmt: on
@@ -219,63 +231,6 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
self.observations.policy.height_scan = None self.observations.policy.height_scan = None
self.observations.policy.joint_pos.params["asset_cfg"].joint_names = self.joint_names self.observations.policy.joint_pos.params["asset_cfg"].joint_names = self.joint_names
self.observations.policy.joint_vel.params["asset_cfg"].joint_names = self.joint_names self.observations.policy.joint_vel.params["asset_cfg"].joint_names = self.joint_names
self.observations.policy.wheel_contact = ObsTerm(
func=mdp.contact_state,
params={
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[self.foot_link_name]),
"threshold": 2.0,
},
clip=(0.0, 1.0),
scale=1.0,
)
self.observations.policy.support_contact = ObsTerm(
func=mdp.contact_state,
params={
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=self.support_link_names),
"threshold": 2.0,
},
clip=(0.0, 1.0),
scale=1.0,
)
self.observations.policy.wheel_contact_force = ObsTerm(
func=mdp.contact_force_norm,
params={
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[self.foot_link_name]),
"scale": 100.0,
"clip": 2.0,
},
clip=(0.0, 2.0),
scale=1.0,
)
self.observations.policy.support_contact_force = ObsTerm(
func=mdp.contact_force_norm,
params={
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=self.support_link_names),
"scale": 100.0,
"clip": 2.0,
},
clip=(0.0, 2.0),
scale=1.0,
)
self.observations.policy.wheel_slip = ObsTerm(
func=mdp.wheel_slip_proxy,
params={
"asset_cfg": SceneEntityCfg("robot", joint_names=self.wheel_joint_names, preserve_order=True),
"wheel_radius": 0.096,
},
clip=(0.0, 3.0),
scale=0.5,
)
self.observations.policy.applied_torque = ObsTerm(
func=mdp.applied_torque,
params={
"asset_cfg": SceneEntityCfg("robot", joint_names=self.joint_names, preserve_order=True),
"scale": 80.0,
"clip": 2.0,
},
clip=(-2.0, 2.0),
scale=1.0,
)
# ------------------------------Actions------------------------------ # ------------------------------Actions------------------------------
# reduce action scale # reduce action scale
@@ -328,8 +283,9 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
# Root penalties # Root penalties
self.rewards.lin_vel_z_l2.weight = -2.0 self.rewards.lin_vel_z_l2.weight = -2.0
self.rewards.ang_vel_xy_l2.weight = -0.02 self.rewards.ang_vel_xy_l2.weight = -0.05
self.rewards.flat_orientation_l2.weight = -0.5 self.rewards.flat_orientation_l2.weight = -0.35
self.rewards.roll_l2.weight = -0.45
self.rewards.base_height_l2.weight = -0.5 self.rewards.base_height_l2.weight = -0.5
self.rewards.base_height_l2.params["target_height"] = 0.45 self.rewards.base_height_l2.params["target_height"] = 0.45
self.rewards.base_height_l2.params["asset_cfg"].body_names = [self.base_link_name] self.rewards.base_height_l2.params["asset_cfg"].body_names = [self.base_link_name]
@@ -345,7 +301,7 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
self.rewards.joint_vel_l2.params["asset_cfg"].joint_names = self.leg_joint_names self.rewards.joint_vel_l2.params["asset_cfg"].joint_names = self.leg_joint_names
self.rewards.joint_vel_wheel_l2.weight = 0 self.rewards.joint_vel_wheel_l2.weight = 0
self.rewards.joint_vel_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names self.rewards.joint_vel_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names
self.rewards.joint_acc_l2.weight = -2e-7 self.rewards.joint_acc_l2.weight = -3e-6
self.rewards.joint_acc_l2.params["asset_cfg"].joint_names = self.leg_joint_names self.rewards.joint_acc_l2.params["asset_cfg"].joint_names = self.leg_joint_names
self.rewards.joint_acc_wheel_l2.weight = -1e-7 self.rewards.joint_acc_wheel_l2.weight = -1e-7
self.rewards.joint_acc_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names self.rewards.joint_acc_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names
@@ -361,17 +317,28 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
self.rewards.knee_joint_pos_penalty.weight = -0.1 self.rewards.knee_joint_pos_penalty.weight = -0.1
self.rewards.knee_joint_pos_penalty.params["asset_cfg"].joint_names = self.knee_joint_names self.rewards.knee_joint_pos_penalty.params["asset_cfg"].joint_names = self.knee_joint_names
# Inward fold limit: with the knee link vertical at q=0, front knees fold inward for
# q < 0 and hind knees for q > 0. Do not allow the legs to pass the vertical
# (straight-down) pose on the inward side.
self.rewards.knee_inward_fold_penalty.weight = -1.0
self.rewards.knee_inward_fold_penalty.params["front_asset_cfg"] = SceneEntityCfg(
"robot", joint_names=self.front_knee_joint_names
)
self.rewards.knee_inward_fold_penalty.params["hind_asset_cfg"] = SceneEntityCfg(
"robot", joint_names=self.hind_knee_joint_names
)
self.rewards.knee_inward_fold_penalty.params["max_inward_tilt"] = 0.0
self.rewards.wheel_vel_penalty.weight = 0 self.rewards.wheel_vel_penalty.weight = 0
self.rewards.wheel_vel_penalty.params["sensor_cfg"].body_names = self.foot_link_name self.rewards.wheel_vel_penalty.params["sensor_cfg"].body_names = self.foot_link_name
self.rewards.wheel_vel_penalty.params["asset_cfg"].joint_names = self.wheel_joint_names self.rewards.wheel_vel_penalty.params["asset_cfg"].joint_names = self.wheel_joint_names
self.rewards.joint_mirror.weight = -0.03 self.rewards.joint_mirror.weight = 0.0
self.rewards.joint_mirror.params["mirror_joints"] = [ self.rewards.joint_mirror.params["mirror_joints"] = [
["fl_knee_joint", "hr_knee_joint"], ["fl_knee_joint", "hr_knee_joint"],
["fr_knee_joint", "hl_knee_joint"], ["fr_knee_joint", "hl_knee_joint"],
] ]
# Action penalties # Action penalties
self.rewards.action_rate_l2.weight = -0.01 self.rewards.action_rate_l2.weight = -0.015
# Contact sensor # Contact sensor
self.rewards.undesired_contacts.weight = -1.0 self.rewards.undesired_contacts.weight = -1.0
@@ -424,7 +391,10 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
# ------------------------------Terminations------------------------------ # ------------------------------Terminations------------------------------
# self.terminations.illegal_contact.params["sensor_cfg"].body_names = [self.base_link_name] # self.terminations.illegal_contact.params["sensor_cfg"].body_names = [self.base_link_name]
self.terminations.illegal_contact = None self.terminations.illegal_contact = None
self.terminations.bad_orientation_2 = None self.terminations.bad_orientation_2 = DoneTerm(
func=mdp.bad_orientation,
params={"limit_angle": 0.9, "asset_cfg": SceneEntityCfg("robot")},
)
# ------------------------------Curriculums------------------------------ # ------------------------------Curriculums------------------------------
# self.curriculum.command_levels.params["range_multiplier"] = (0.2, 1.0) # self.curriculum.command_levels.params["range_multiplier"] = (0.2, 1.0)
@@ -469,7 +439,7 @@ class DeeproboticsM20SupportRoughEnvCfg(DeeproboticsM20RoughEnvCfg):
self.rewards.undesired_contacts.params["sensor_cfg"].body_names = [f"^(?!.*{self.foot_link_name}).*"] self.rewards.undesired_contacts.params["sensor_cfg"].body_names = [f"^(?!.*{self.foot_link_name}).*"]
self.rewards.undesired_contacts.params["threshold"] = 35.0 self.rewards.undesired_contacts.params["threshold"] = 35.0
self.rewards.command_progress.weight = 0.8 self.rewards.command_progress.weight = 0.8
self.rewards.support_contact_progress.weight = 1.0 self.rewards.support_contact_progress.weight = 0.0
self.rewards.support_contact_progress.params["sensor_cfg"].body_names = self.support_link_names self.rewards.support_contact_progress.params["sensor_cfg"].body_names = self.support_link_names
self.rewards.support_contact_progress.params["wheel_sensor_cfg"].body_names = [self.foot_link_name] self.rewards.support_contact_progress.params["wheel_sensor_cfg"].body_names = [self.foot_link_name]
self.rewards.support_contact_progress.params["wheel_asset_cfg"].joint_names = self.wheel_joint_names self.rewards.support_contact_progress.params["wheel_asset_cfg"].joint_names = self.wheel_joint_names
@@ -481,7 +451,8 @@ class DeeproboticsM20SupportRoughEnvCfg(DeeproboticsM20RoughEnvCfg):
# Stage 2 should still avoid the bad solution of crawling on the body. # Stage 2 should still avoid the bad solution of crawling on the body.
self.rewards.track_lin_vel_xy_exp.weight = 1.5 self.rewards.track_lin_vel_xy_exp.weight = 1.5
self.rewards.track_ang_vel_z_exp.weight = 1.0 self.rewards.track_ang_vel_z_exp.weight = 1.0
self.rewards.flat_orientation_l2.weight = -0.45 self.rewards.flat_orientation_l2.weight = -0.35
self.rewards.roll_l2.weight = -0.45
self.rewards.base_height_l2.weight = -0.35 self.rewards.base_height_l2.weight = -0.35
self.rewards.base_height_l2.params["target_height"] = 0.29 self.rewards.base_height_l2.params["target_height"] = 0.29
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/smooth_env_cfg.py
+79
View File
@@ -0,0 +1,79 @@
# Copyright (c) 2025 Deep Robotics
# SPDX-License-Identifier: BSD 3-Clause
"""Smooth random-undulation task: train both pitch and roll to stay at zero."""
from isaaclab.terrains import TerrainGeneratorCfg
from isaaclab.utils import configclass
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
from .rough_env_cfg import DeeproboticsM20RoughEnvCfg
@configclass
class DeeproboticsM20SmoothEnvCfg(DeeproboticsM20RoughEnvCfg):
"""Stage 1 policy on a smooth, randomly undulating terrain, forward-only command."""
def __post_init__(self):
super().__post_init__()
# ------------------------------Terrain------------------------------
# Low-frequency sinusoids only: no steps, no sharp edges.
self.scene.terrain.terrain_generator = TerrainGeneratorCfg(
size=(8.0, 8.0),
border_width=2.0,
num_rows=10,
num_cols=20,
horizontal_scale=0.05,
vertical_scale=0.005,
slope_threshold=None,
use_cache=False,
curriculum=True,
sub_terrains={
"smooth_random": mdp.SmoothRandomTerrainCfg(
proportion=1.0,
border_width=0.25,
height_range=(0.10, 0.20),
wavelength_range=(1.0, 2.2),
num_waves=3,
)
},
)
self.scene.terrain.max_init_terrain_level = 9
# ------------------------------Spawn------------------------------
self.events.randomize_reset_base.params = {
"pose_range": {
"x": (-2.0, 2.0),
"y": (-2.0, 2.0),
"z": (0.30, 0.30),
"roll": (-0.05, 0.05),
"pitch": (-0.05, 0.05),
"yaw": (-3.14, 3.14),
},
"velocity_range": {
"x": (-0.1, 0.1),
"y": (0.0, 0.0),
"z": (0.0, 0.0),
"roll": (0.0, 0.0),
"pitch": (0.0, 0.0),
"yaw": (0.0, 0.0),
},
}
# Commands keep the original Stage-1 distribution inherited from
# DeeproboticsM20RoughEnvCfg: x in [-2, 2] m/s, y = 0, yaw in [-1.5, 1.5]
# rad/s, heading command enabled, 2% standing and 100% heading envs.
# ------------------------------Rewards------------------------------
# Split the flat-orientation penalty into true Euler pitch and roll in
# radians. Giving -0.4 to each keeps the total strength of the previous
# -0.8 flat_orientation_l2 term while letting the two axes be tuned
# independently.
self.rewards.flat_orientation_l2.weight = 0.0
self.rewards.pitch_l2.weight = -0.4
self.rewards.roll_l2.weight = -0.4
self.rewards.ang_vel_xy_l2.weight = -0.1
self.disable_zero_weight_rewards()
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/__init__.py
+1
View File
@@ -19,3 +19,4 @@ from .curriculums import * # noqa: F401, F403
from .events import * # noqa: F401, F403 from .events import * # noqa: F401, F403
from .observations import * # noqa: F401, F403 from .observations import * # noqa: F401, F403
from .rewards import * # noqa: F401, F403 from .rewards import * # noqa: F401, F403
from .terrain import * # noqa: F401, F403
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/rewards.py
+42
View File
@@ -330,6 +330,33 @@ def joint_pos_penalty(
return reward return reward
def knee_inward_fold_penalty(
env: ManagerBasedRLEnv,
front_asset_cfg: SceneEntityCfg,
hind_asset_cfg: SceneEntityCfg,
max_inward_tilt: float = 0.0,
) -> torch.Tensor:
"""Penalize front/hind knee joints that fold inward past ``max_inward_tilt``.
The knee link points straight down when its joint angle is zero, so the signed joint
position is the leg's tilt from vertical (all four knee axes are ``-Y`` in the URDF).
Front legs fold inward when the angle decreases, hind legs fold inward when it
increases. ``max_inward_tilt`` is the largest inward tilt allowed from the vertical
(straight-down) pose: 0.0 stops the leg at vertical, ``pi / 2`` would allow it to
reach horizontal. Only the inward direction is limited; outward folding is left to
the regular joint position limits. The penalty is quadratic in the excess angle and
is zero while the joint stays inside the limit.
"""
front_asset: Articulation = env.scene[front_asset_cfg.name]
hind_asset: Articulation = env.scene[hind_asset_cfg.name]
front_pos = front_asset.data.joint_pos[:, front_asset_cfg.joint_ids]
hind_pos = hind_asset.data.joint_pos[:, hind_asset_cfg.joint_ids]
# front: q >= -max_inward_tilt, hind: q <= max_inward_tilt
front_excess = torch.clamp(-max_inward_tilt - front_pos, min=0.0)
hind_excess = torch.clamp(hind_pos - max_inward_tilt, min=0.0)
return torch.sum(front_excess.square() + hind_excess.square(), dim=1)
def wheel_vel_penalty( def wheel_vel_penalty(
env: ManagerBasedRLEnv, env: ManagerBasedRLEnv,
sensor_cfg: SceneEntityCfg, sensor_cfg: SceneEntityCfg,
@@ -944,6 +971,21 @@ def flat_orientation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = Scen
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7 # reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
return reward return reward
def roll_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
"""Penalize body roll angle in radians (squared)."""
asset: RigidObject = env.scene[asset_cfg.name]
roll, _, _ = math_utils.euler_xyz_from_quat(asset.data.root_quat_w)
return torch.square(roll)
def pitch_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
"""Penalize body pitch angle in radians (squared)."""
asset: RigidObject = env.scene[asset_cfg.name]
_, pitch, _ = math_utils.euler_xyz_from_quat(asset.data.root_quat_w)
return torch.square(pitch)
def feet_air_time_including_ang_z( def feet_air_time_including_ang_z(
env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float
) -> torch.Tensor: ) -> torch.Tensor:
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/terrain.py
+107
View File
@@ -0,0 +1,107 @@
"""Custom terrains for the FireAnt locomotion tasks."""
from __future__ import annotations
import numpy as np
import trimesh
from isaaclab.terrains import HfTerrainBaseCfg, SubTerrainBaseCfg
from isaaclab.terrains.height_field.utils import height_field_to_mesh
from isaaclab.utils import configclass
def single_side_bridge_terrain(
difficulty: float, cfg: "SingleSideBridgeTerrainCfg"
) -> tuple[list[trimesh.Trimesh], np.ndarray]:
"""Long raised bridge on the +y half of the cell; flat ground on the -y half.
The bridge edge is exactly at the cell center line (local ``y = size[1] / 2``),
so a robot spawned at the cell center with zero yaw has its left wheels on the
bridge and its right wheels on the flat ground.
"""
height = cfg.bridge_height_range[0] + difficulty * (
cfg.bridge_height_range[1] - cfg.bridge_height_range[0]
)
# Ground slab: top surface at z=0, extends over the whole cell.
ground = trimesh.creation.box(
extents=(cfg.size[0], cfg.size[1], 0.05),
transform=trimesh.transformations.translation_matrix(
(cfg.size[0] / 2.0, cfg.size[1] / 2.0, -0.025)
),
)
# Bridge: full cell length along x, raised strip on the +y side.
bridge = trimesh.creation.box(
extents=(cfg.size[0], cfg.bridge_width, height),
transform=trimesh.transformations.translation_matrix(
(
cfg.size[0] / 2.0,
cfg.size[1] / 2.0 + cfg.bridge_width / 2.0,
height / 2.0,
)
),
)
origin = np.array((cfg.size[0] / 2.0, cfg.size[1] / 2.0, 0.0))
return [ground, bridge], origin
@configclass
class SingleSideBridgeTerrainCfg(SubTerrainBaseCfg):
"""Configuration for a long single-side bridge terrain."""
function = single_side_bridge_terrain
bridge_height_range: tuple[float, float] = (0.06, 0.14)
"""Bridge height at the start and end of the difficulty curriculum (m)."""
bridge_width: float = 1.5
"""Width of the raised bridge on the +y side of the cell (m)."""
@height_field_to_mesh
def smooth_random_terrain(difficulty: float, cfg: "SmoothRandomTerrainCfg") -> np.ndarray:
"""Smooth low-frequency random height field.
The surface is a sum of a few low-frequency sinusoids. It has no steps or
sharp edges, so the robot has to keep its body level over gentle, random
pitch and roll disturbances.
"""
width_pixels = int(cfg.size[0] / cfg.horizontal_scale)
length_pixels = int(cfg.size[1] / cfg.horizontal_scale)
x = np.arange(width_pixels, dtype=np.float64) * cfg.horizontal_scale
y = np.arange(length_pixels, dtype=np.float64) * cfg.horizontal_scale
grid_x, grid_y = np.meshgrid(x, y, indexing="ij")
amplitude = cfg.height_range[0] + difficulty * (cfg.height_range[1] - cfg.height_range[0])
heights = np.zeros_like(grid_x)
for _ in range(cfg.num_waves):
direction = np.random.uniform(0.0, 2.0 * np.pi)
wavelength = np.random.uniform(*cfg.wavelength_range)
phase = np.random.uniform(0.0, 2.0 * np.pi)
weight = np.random.uniform(0.5, 1.0)
projection = grid_x * np.cos(direction) + grid_y * np.sin(direction)
heights += weight * np.sin(2.0 * np.pi * projection / wavelength + phase)
heights -= heights.mean()
peak = float(np.max(np.abs(heights)))
if peak > 1e-9:
heights *= amplitude / peak
return np.rint(heights / cfg.vertical_scale).astype(np.int16)
@configclass
class SmoothRandomTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a smooth random-undulation terrain."""
function = smooth_random_terrain
height_range: tuple[float, float] = (0.02, 0.05)
"""Peak height of the undulations at the start and end of the curriculum (m)."""
wavelength_range: tuple[float, float] = (1.5, 3.0)
"""Spatial wavelength range of the undulations (m)."""
num_waves: int = 3
"""Number of sinusoids summed to form the surface."""
source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py
+2
View File
@@ -386,6 +386,8 @@ class RewardsCfg:
lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=0.0) lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=0.0)
ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=0.0) ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=0.0)
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0) flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0)
roll_l2 = RewTerm(func=mdp.roll_l2, weight=0.0)
pitch_l2 = RewTerm(func=mdp.pitch_l2, weight=0.0)
base_height_l2 = RewTerm( base_height_l2 = RewTerm(
func=mdp.base_height_l2, func=mdp.base_height_l2,
weight=0.0, weight=0.0,