diff --git a/deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf b/deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf index f09c2d8..1240b9b 100644 --- a/deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf +++ b/deep_robotics_model/M20/M20_urdf/urdf/Solenopsis.urdf @@ -20,9 +20,9 @@ - - + + @@ -42,9 +42,9 @@ - - + + @@ -75,20 +75,20 @@ - + - + - + - + - + @@ -106,9 +106,9 @@ - - + + @@ -139,20 +139,20 @@ - + - + - + - + - + @@ -168,9 +168,9 @@ - - + + @@ -201,20 +201,20 @@ - + - + - + - + - + @@ -230,9 +230,9 @@ - - + + @@ -263,20 +263,20 @@ - + - + - + - + - + diff --git a/docs/fire_ant_prototype_specs.md b/docs/fire_ant_prototype_specs.md index de445f4..752ad5a 100644 --- a/docs/fire_ant_prototype_specs.md +++ b/docs/fire_ant_prototype_specs.md @@ -131,6 +131,29 @@ boxes 高度 = 0.03 ~ 0.22 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. 样机设计重点 优先级建议： diff --git a/docs/fire_ant_smooth_training.md b/docs/fire_ant_smooth_training.md new file mode 100644 index 0000000..2e5999a --- /dev/null +++ b/docs/fire_ant_smooth_training.md @@ -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 配置后，请同步更新本文对应章节，并在“实际训练运行记录”中追加新的运行目录与关键指标。 diff --git a/eval_kit/profiles.py b/eval_kit/profiles.py index 4761760..9201417 100644 --- a/eval_kit/profiles.py +++ b/eval_kit/profiles.py @@ -16,7 +16,7 @@ from eval_kit.core.schema import RobotProfile SOLENOPSIS = RobotProfile( name="solenopsis", - # None = 运行时从仿真资产读真实质量（Solenopsis.urdf 当前为 26.581 kg）。 + # None = 运行时从仿真资产读真实质量（Solenopsis.urdf 当前为 24.74 kg）。 # 旧 eval.py 的命令行默认值是 25.482，那是更早的 M20 简化模型名义质量， # 会让 COT 偏低约 4%。 mass_kg=None, diff --git a/source/rl_training/rl_training/assets/deeprobotics.py b/source/rl_training/rl_training/assets/deeprobotics.py index 20ac20c..6ab8110 100644 --- a/source/rl_training/rl_training/assets/deeprobotics.py +++ b/source/rl_training/rl_training/assets/deeprobotics.py @@ -65,6 +65,7 @@ DEEPROBOTICS_LITE3_CFG = ArticulationCfg( DEEPROBOTICS_M20_CFG = ArticulationCfg( spawn=sim_utils.UrdfFileCfg( asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/M20/M20_urdf/urdf/Solenopsis.urdf", + usd_dir="/home/suyufei/tmp/IsaacLab_m20_wheel220", fix_base=False, merge_fixed_joints=True, joint_drive=None, @@ -88,7 +89,7 @@ DEEPROBOTICS_M20_CFG = ArticulationCfg( ), ), init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.306), + pos=(0.0, 0.0, 0.32), joint_pos={ "f[l,r]_knee_joint": 0.5, "h[l,r]_knee_joint": -0.5, diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py index 9acad03..5f120fd 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/__init__.py @@ -44,3 +44,25 @@ gym.register( "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", + }, +) diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/agents/rsl_rl_ppo_cfg.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/agents/rsl_rl_ppo_cfg.py index 8a6e0c9..7bce2cd 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/agents/rsl_rl_ppo_cfg.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/agents/rsl_rl_ppo_cfg.py @@ -54,3 +54,25 @@ class DeeproboticsM20SupportRoughPPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg): super().__post_init__() 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" diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/bridge_env_cfg.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/bridge_env_cfg.py new file mode 100644 index 0000000..612abc7 --- /dev/null +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/bridge_env_cfg.py @@ -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() diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/rough_env_cfg.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/rough_env_cfg.py index ab051e5..47653ce 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/rough_env_cfg.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/rough_env_cfg.py @@ -7,8 +7,8 @@ from isaaclab.managers import CurriculumTermCfg as CurrTerm from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.utils import configclass import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp @@ -92,7 +92,7 @@ class DeeproboticsM20RewardsCfg(RewardsCfg): params={ "command_name": "base_velocity", "std": 0.6, - "wheel_radius": 0.096, + "wheel_radius": 0.11, "track_width": 0.50, "left_wheel_indices": (0, 2), "right_wheel_indices": (1, 3), @@ -132,7 +132,7 @@ class DeeproboticsM20RewardsCfg(RewardsCfg): "wheel_sensor_cfg": SceneEntityCfg("contact_forces", body_names=""), "asset_cfg": SceneEntityCfg("robot"), "wheel_asset_cfg": SceneEntityCfg("robot", joint_names=""), - "wheel_radius": 0.096, + "wheel_radius": 0.11, "support_force_threshold": 15.0, "wheel_obstacle_force_threshold": 35.0, "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 class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): @@ -188,6 +198,8 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): knee_joint_names = [ "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 # fmt: on @@ -219,63 +231,6 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): self.observations.policy.height_scan = None 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.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------------------------------ # reduce action scale @@ -328,8 +283,9 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): # Root penalties self.rewards.lin_vel_z_l2.weight = -2.0 - self.rewards.ang_vel_xy_l2.weight = -0.02 - self.rewards.flat_orientation_l2.weight = -0.5 + self.rewards.ang_vel_xy_l2.weight = -0.05 + 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.params["target_height"] = 0.45 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_wheel_l2.weight = 0 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_wheel_l2.weight = -1e-7 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.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.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.joint_mirror.weight = -0.03 + self.rewards.joint_mirror.weight = 0.0 self.rewards.joint_mirror.params["mirror_joints"] = [ ["fl_knee_joint", "hr_knee_joint"], ["fr_knee_joint", "hl_knee_joint"], ] # Action penalties - self.rewards.action_rate_l2.weight = -0.01 + self.rewards.action_rate_l2.weight = -0.015 # Contact sensor self.rewards.undesired_contacts.weight = -1.0 @@ -424,7 +391,10 @@ class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg): # ------------------------------Terminations------------------------------ # self.terminations.illegal_contact.params["sensor_cfg"].body_names = [self.base_link_name] 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------------------------------ # 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["threshold"] = 35.0 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["wheel_sensor_cfg"].body_names = [self.foot_link_name] 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. self.rewards.track_lin_vel_xy_exp.weight = 1.5 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.params["target_height"] = 0.29 diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/smooth_env_cfg.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/smooth_env_cfg.py new file mode 100644 index 0000000..222e0a7 --- /dev/null +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/config/wheeled/deeprobotics_m20/smooth_env_cfg.py @@ -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() diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/__init__.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/__init__.py index af6c758..216d286 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/__init__.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/__init__.py @@ -19,3 +19,4 @@ from .curriculums import * # noqa: F401, F403 from .events import * # noqa: F401, F403 from .observations import * # noqa: F401, F403 from .rewards import * # noqa: F401, F403 +from .terrain import * # noqa: F401, F403 diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/rewards.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/rewards.py index 56638e9..160754f 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/rewards.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/rewards.py @@ -330,6 +330,33 @@ def joint_pos_penalty( 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( env: ManagerBasedRLEnv, 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 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( env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float ) -> torch.Tensor: diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/terrain.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/terrain.py new file mode 100644 index 0000000..755ce1d --- /dev/null +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/mdp/terrain.py @@ -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.""" diff --git a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index a8e0671..46a9866 100644 --- a/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/rl_training/rl_training/tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -386,6 +386,8 @@ class RewardsCfg: 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) 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( func=mdp.base_height_l2, weight=0.0,