Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
55094bf682 | ||
|
zeitvex
|
4ee4af028c | ||
|
zeitvex
|
c05c1cb162 | ||
|
zeitvex
|
d8c5d34091 | ||
|
zeitvex
|
60f7a08e91 |
@@ -36,6 +36,9 @@ checkpoints/
|
||||
wandb/
|
||||
sim2sim_log_*.txt
|
||||
**/sim2sim_temp.xml
|
||||
**/route_check_runs/
|
||||
**/route_experiments/suite_*/
|
||||
**/tools/nav_tools/points/auto_candidates/
|
||||
|
||||
# IDE and operating system files
|
||||
.idea/
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
# 训练代码演进
|
||||
|
||||
本项目将“训练代码架构”和“训练产生的模型 checkpoint”分别管理。代码、奖励、课程或观测契约发生变化时形成新的 Git 版本；同一架构下继续训练产生的模型编号作为实验和部署工件记录。
|
||||
|
||||
| 版本 | 来源快照 | 主要目的 |
|
||||
| --- | --- | --- |
|
||||
| `v0.4.0` | `uni_mjlab(1)` | 第一份完整的新 MJCF 与新 mjlab 训练工程 |
|
||||
| `v0.5.0` | `uni_mjlab_new` | 扩大观测、延迟和动力学随机化，探索更强 Sim2Real 鲁棒性 |
|
||||
| `v0.6.0` | `best` | 面向比赛越障重新设计奖励、课程、站姿和诊断体系 |
|
||||
|
||||
## 从随机化增强到比赛训练
|
||||
|
||||
`v0.6.0` 不是简单地继续增大 `v0.5.0` 的随机范围。实践中重新降低了部分噪声、延迟和动力学随机化强度，并把训练重点转向可控的比赛任务课程：
|
||||
|
||||
- 投影重力噪声由 `±0.08` 调回 `±0.05`。
|
||||
- 腿和轮动作最大随机延迟由 4 步调回 2 步。
|
||||
- 摩擦、刚度和阻尼随机化调回较窄范围。
|
||||
- 移除该阶段的连续机身外力扰动与腿部质量随机化。
|
||||
- 将 x、y 和 yaw 跟踪拆成独立奖励和独立指令课程。
|
||||
- 障碍由课程逐步释放，高墙训练地形改为五道重复横墙。
|
||||
- 增加楼梯侧向漂移和偏航漂移惩罚。
|
||||
- 增加大量只用于训练评估的误差、轮速、姿态和接触指标。
|
||||
|
||||
这种调整反映的是从“广泛鲁棒性探索”转向“比赛场景定向优化”，不表示 `v0.5.0` 被删除；它仍由对应 Tag 完整保留。
|
||||
|
||||
## 模型与比赛部署
|
||||
|
||||
比赛训练可能先获得基模，再修改参数继续训练和筛选 checkpoint。最终关系为：
|
||||
|
||||
```text
|
||||
训练代码架构：v0.6.0 / best
|
||||
比赛 Rough 策略：model_6800.onnx
|
||||
比赛真机工程：last_not_slalom_1050
|
||||
比赛得分：1050
|
||||
```
|
||||
|
||||
`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。
|
||||
@@ -9,6 +9,11 @@
|
||||
| `v0.3.0` | 第一代软件闭环 | 早期训练、MJCF、MuJoCo、Sim2Sim、IK 与 Python Sim2Real |
|
||||
| `v0.3.1` | 实机记录 | 补充第一代 Sim2Real 实机视频 |
|
||||
| `v0.4.0` | 新训练基线 | 第一份完整的新 MJCF、新 mjlab 框架和 Rough 策略工程 |
|
||||
| `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
|
||||
| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
@@ -17,3 +22,52 @@
|
||||
- mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。
|
||||
- 保留轮腿分组执行器随机化所需的本地补丁。
|
||||
- 本阶段归档 `model_rough.pt`，不将生成日志、缓存和临时 XML 纳入版本库。
|
||||
|
||||
## `v0.5.0` 的训练变化
|
||||
|
||||
- MJCF、mjlab 基准和已有模型文件保持不变。
|
||||
- 投影重力噪声由 `±0.05` 扩大到 `±0.08`。
|
||||
- 腿与轮动作的最大随机延迟由 2 步增加到 4 步。
|
||||
- 地面摩擦随机范围由 `0.3–1.0` 扩大到 `0.15–1.25`。
|
||||
- 执行器刚度和阻尼缩放由 `0.9–1.1` 扩大到 `0.5–1.5`。
|
||||
- 增加膝部和轮部质量的 `0.7–1.3` 随机缩放。
|
||||
- 增加作用于机身的连续随机外力和力矩扰动。
|
||||
|
||||
## `v0.6.0` 的比赛训练架构
|
||||
|
||||
- 保持 `v0.4.0` 引入的新 MJCF 和 mjlab 框架不变。
|
||||
- 将线速度奖励拆分为 x/y 两轴，并独立配置偏航角速度奖励。
|
||||
- 增加自适应 x/y/yaw 指令范围课程。
|
||||
- 增加障碍地形逐步释放与更严格的地形晋级逻辑。
|
||||
- 增加楼梯横向速度和偏航漂移约束。
|
||||
- 默认站姿调整为髋俯仰 `0.550`、膝关节 `-1.125`，初始机身高度为 `0.42 m`。
|
||||
- 增加速度误差、轮速跟踪、动作和姿态等训练诊断指标。
|
||||
- 该 Tag 保存比赛训练代码架构，不把每次继续训练产生的 checkpoint 误记为新的软件版本。
|
||||
|
||||
训练阶段的详细关系见 [`training_evolution.md`](training_evolution.md)。
|
||||
|
||||
## `v0.7.0` 的独立 MuJoCo 工具
|
||||
|
||||
- 比赛训练架构、MJCF、模型和依赖锁文件保持 `v0.6.0` 状态不变。
|
||||
- 增加解析姿态表、RL 友好姿态筛选和 MuJoCo 静态姿态优化。
|
||||
- 增加 IK/差速轮参数扫描，可导出 JSON 结果。
|
||||
- 增加 Robot、Controller、Dynamics、MPCController 和 GUI 调试链路。
|
||||
- 记录历史工具常量与新版 MJCF 质量、比赛默认站姿之间的参数边界，避免将分析结果直接当作已校准真机参数。
|
||||
|
||||
## `v0.8.0` 的后期 Sim2Sim
|
||||
|
||||
- 比赛训练任务、MJCF 和 `v0.7.0` 的 MuJoCo 工具保持不变。
|
||||
- 策略运行器增加 ONNX 加载，并允许在缺少 `pynput` 时关闭后台键盘监听继续运行。
|
||||
- MuJoCo 执行器重建同时兼容新旧 Spec 删除接口。
|
||||
- 增加 PT→ONNX 导出、IK 补偿扫描、纯 IK 绕桩和 ONNX 批量路线检查入口。
|
||||
- 归档比赛最终 Rough 策略 `model_6800.onnx`；其 SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
|
||||
- Crawl 权重、运行日志、临时 XML 和大量重复路线实验不在本阶段归档。
|
||||
|
||||
## `v0.8.1` 的导航打点工具
|
||||
|
||||
- 补充 Pygame 地图/PCD/航点综合编辑器、避障区域编辑器和坐标变换工具。
|
||||
- 补充路线安全检查、候选航点优化、XML/航点镜像和批量 Sim2Sim 实验入口。
|
||||
- 按源文件时间保留 14 份比赛路线 JSON，不将开发期文件名误解释为正式版本号。
|
||||
- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
|
||||
- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
|
||||
- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。
|
||||
|
||||
@@ -5,7 +5,7 @@
|
||||
```text
|
||||
05_software/
|
||||
├─ train/
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、Sim2Sim 和本地 mjlab 依赖
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
└─ sim2real/ # 第一代 Python 策略真机部署
|
||||
@@ -18,6 +18,8 @@ MJCF + mjlab task
|
||||
|
|
||||
v
|
||||
PPO 训练策略
|
||||
|
|
||||
+----> MuJoCo 姿态 / IK / MPC 调试
|
||||
|
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
@@ -26,7 +28,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -2,17 +2,30 @@
|
||||
|
||||
`rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
|
||||
|
||||
当前内容对应 `v0.4.0`，是第一份完整采用新版 MJCF 和新版 mjlab 框架的训练工程。
|
||||
当前内容对应 `v0.8.1`：训练代码保持 `v0.6.0` 的比赛架构，包含后期 MuJoCo、Sim2Sim、比赛最终 Rough ONNX 策略，并补充完整导航打点工具、路线迭代和抽样 PCD。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
|
||||
|
||||
## 内容
|
||||
|
||||
- `src/robot`：Flat、Rough、Crawl 训练任务和自定义 MDP
|
||||
- `mjcf`：轮足机器人 MuJoCo 模型和网格
|
||||
- `sim2sim`：策略加载、交互控制和比赛地形验证
|
||||
- `mujoco_sim`：不依赖训练循环的姿态、IK、动力学和 MPC 分析
|
||||
- `tools/nav_tools`：地图/PCD 查看、航点编辑、路线检查和比赛路线数据
|
||||
- `mjlab`：固定版本的本地训练框架依赖
|
||||
- `model_rough.pt`：本阶段 Rough 策略权重
|
||||
- `model_6800.onnx`：比赛最终使用的 Rough 策略
|
||||
- `pyproject.toml`、`uv.lock`：Python 环境与依赖锁定
|
||||
|
||||
与 `v0.3.0` 相比，本版本更新了 MJCF 质量和惯性参数，并将 mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。机械 CAD 未发生变化。
|
||||
|
||||
与 `v0.4.0` 相比，本版本没有再次修改 MJCF 和训练框架，只调整训练环境配置：投影重力噪声从 `±0.05` 扩大到 `±0.08`，最大动作延迟从 2 步增加到 4 步，扩大摩擦、刚度和阻尼随机化，增加腿部质量随机化与连续外力/力矩扰动。
|
||||
|
||||
`v0.6.0` 在 `v0.5.0` 之后转向比赛任务优化：降低部分过强随机化，加入分轴速度跟踪奖励、自适应指令课程、障碍地形释放课程、楼梯横向/偏航约束以及更完整的训练诊断。详细对比见 [`../../01_doc/training_evolution.md`](../../01_doc/training_evolution.md)。
|
||||
|
||||
`v0.7.0` 不修改比赛训练架构，增加独立 MuJoCo 工具；入口和参数边界见 [`rc_mjlab/mujoco_sim/README.md`](rc_mjlab/mujoco_sim/README.md)。
|
||||
|
||||
`v0.8.0` 继续保持训练架构和 MJCF 不变，归档后期 Sim2Sim 增量与比赛 Rough ONNX 策略；入口和归档边界见 [`rc_mjlab/sim2sim/README.md`](rc_mjlab/sim2sim/README.md)。
|
||||
|
||||
`v0.8.1` 补充完整导航打点工具和小体积预览点云；入口、路线清单和抽样边界见 [`rc_mjlab/tools/nav_tools/README.md`](rc_mjlab/tools/nav_tools/README.md)。
|
||||
|
||||
工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
|
||||
|
||||
@@ -8,6 +8,8 @@
|
||||
- `mjlab[cu128]`
|
||||
- PyTorch CUDA 12.8 环境
|
||||
- `pynput`
|
||||
- 后期 Sim2Sim 可选依赖：Pygame、ONNX Runtime
|
||||
- 导航打点可选依赖：Pygame、Pillow
|
||||
|
||||
精确解析结果保存在 `uv.lock`。项目使用本地可编辑的 `mjlab`：
|
||||
|
||||
@@ -39,4 +41,16 @@ uv run train Robot-Flat-v0
|
||||
uv run play Robot-Rough-v0
|
||||
```
|
||||
|
||||
根 `uv.lock` 保留比赛训练环境的历史解析结果。后期 Sim2Sim 新增依赖单独保存在 `sim2sim/requirements.txt`，运行时叠加，避免重新锁定时升级历史 MuJoCo nightly：
|
||||
|
||||
```bash
|
||||
uv run --with-requirements sim2sim/requirements.txt python sim2sim/nav_sim2sim.py
|
||||
```
|
||||
|
||||
导航打点工具同样不修改历史锁文件：
|
||||
|
||||
```bash
|
||||
uv run --with-requirements tools/nav_tools/requirements.txt python tools/nav_tools/nav_map_viewer.py
|
||||
```
|
||||
|
||||
GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。
|
||||
|
||||
> 本目录对应 `v0.4.0`：第一份完整的新 MJCF 与新框架训练工程。该快照包含 `model_rough.pt`；未包含独立 `mujoco_sim` 工具和单独的 Crawl 策略权重，相关早期内容仍可通过 `v0.3.0` 查看。
|
||||
> 当前目录对应 `v0.8.1`：保留比赛训练架构、后期 MuJoCo/Sim2Sim 和比赛最终 Rough 策略，并补充导航地图、打点工具、路线迭代与抽样 PCD。`model_rough.pt` 仍作为早期参考权重保留。
|
||||
|
||||
---
|
||||
|
||||
@@ -41,6 +41,10 @@ rc_mjlab/
|
||||
│ └── competition_terrains.py # 竞赛自定义地形（高墙障碍、低杆障碍）
|
||||
├── sim2sim/ # Sim2Sim 物理部署与高精度交互回放工具
|
||||
│ ├── nav_sim2sim.py # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪
|
||||
│ ├── nav_route_sim2sim_check.py # ONNX 策略批量路线检查
|
||||
│ ├── ik_slalom_sim2sim.py # 纯 IK、路径跟踪与绕桩验证
|
||||
│ ├── ik_compensation_sweep.py # IK 补偿参数扫描
|
||||
│ ├── export_onnx.py # PT actor 导出与 ONNX 一致性检查
|
||||
│ ├── sim2sim.py # 简易版键盘调试工具
|
||||
│ ├── interface/
|
||||
│ │ └── mujoco_io.py # MuJoCo 输入输出与传感器、低通滤波器接口
|
||||
@@ -53,7 +57,10 @@ rc_mjlab/
|
||||
│ ├── wheelleg.xml # 机器人 MuJoCo 模型（含网格引用）
|
||||
│ ├── scene.xml # mjlab 场景入口文件
|
||||
│ └── meshes/ # STL/OBJ 碰撞与外观网格
|
||||
├── model_rough.pt # 本阶段用于回放和 Sim2Sim 的 Rough 策略
|
||||
├── mujoco_sim/ # 姿态、IK、动力学和 MPC 独立工具
|
||||
├── tools/nav_tools/ # 地图/PCD/航点编辑、路线检查与比赛路线数据
|
||||
├── model_rough.pt # 早期 Rough 参考 checkpoint
|
||||
├── model_6800.onnx # 比赛最终 Rough 策略
|
||||
├── pyproject.toml # 项目依赖（uv 管理，含清华镜像源加速）
|
||||
└── uv.lock # 精确依赖锁定文件
|
||||
```
|
||||
@@ -88,6 +95,10 @@ cd sim2sim
|
||||
uv run python nav_sim2sim.py
|
||||
```
|
||||
|
||||
后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。
|
||||
|
||||
导航打点工具、路线快照和抽样点云说明见 [`tools/nav_tools/README.md`](tools/nav_tools/README.md)。
|
||||
|
||||
---
|
||||
|
||||
## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,57 @@
|
||||
# MuJoCo 独立工具集
|
||||
|
||||
本目录保存不依赖训练循环的 MuJoCo、姿态、IK、动力学和 MPC 分析工具。脚本通过父目录读取 `../mjcf/scene.xml` 与 `../mjcf/wheelleg.xml`，因此应从 `rc_mjlab` 工程根目录运行。
|
||||
|
||||
## 工具分类
|
||||
|
||||
| 入口 | 用途 |
|
||||
| --- | --- |
|
||||
| `posture_tool.py` | 基于解析运动学快速生成站立和低姿态参数表 |
|
||||
| `rl_friendly_opt.py` | 按轮心位置、关节力矩和雅可比条件数筛选适合 RL 的姿态 |
|
||||
| `posture_optimizer.py` | 解析计算与 MuJoCo 扫描结合的姿态优化 |
|
||||
| `static_posture_optimizer.py` | 在重力和地面接触下评估静态站姿、支撑域和离地间隙 |
|
||||
| `ik_diff_sweep.py` | 扫描 IK 姿态和差速轮跟踪参数，可导出 JSON |
|
||||
| `run.py` | 启动完整 MuJoCo 控制、GUI 和 MPC 调试链路 |
|
||||
| `robot.py`、`controller.py` | 仿真机器人接口和控制器 |
|
||||
| `dynamics.py`、`mpc.py`、`mpc_controller.py` | Pinocchio 动力学与 OSQP MPC |
|
||||
|
||||
## 依赖
|
||||
|
||||
执行工程根目录的 `uv sync` 后，训练环境已经提供 NumPy、SciPy 和 MuJoCo。不同工具还需要：
|
||||
|
||||
- 纯解析工具：Python、NumPy。
|
||||
- MuJoCo 扫描：`mujoco`、NumPy。
|
||||
- 完整 MPC：`pinocchio`、`osqp`、SciPy。
|
||||
- GUI：系统可用的 Tk/Tkinter。
|
||||
|
||||
Pinocchio 和 OSQP 没有加入训练环境锁文件，因为它们只服务于可选 MPC 工具，且 Pinocchio 的安装方式与操作系统、Conda/Python 环境有关。
|
||||
|
||||
## 常用命令
|
||||
|
||||
在 `05_software/train/rc_mjlab` 下执行：
|
||||
|
||||
```bash
|
||||
# 不启动 MuJoCo 的快速姿态表
|
||||
uv run python mujoco_sim/posture_tool.py
|
||||
uv run python mujoco_sim/rl_friendly_opt.py
|
||||
|
||||
# 姿态扫描
|
||||
uv run python mujoco_sim/posture_optimizer.py --analyze
|
||||
uv run python mujoco_sim/static_posture_optimizer.py --quick
|
||||
|
||||
# IK 与差速轮参数快速扫描
|
||||
uv run python mujoco_sim/ik_diff_sweep.py --quick
|
||||
|
||||
# 完整 GUI/MPC 仿真，需要可选依赖
|
||||
uv run python mujoco_sim/run.py
|
||||
```
|
||||
|
||||
## 参数边界
|
||||
|
||||
这是一份历史工具快照，保留当时用于分析和调参的常量：
|
||||
|
||||
- `config.py`、`posture_optimizer.py` 和 `static_posture_optimizer.py` 中的解析质量常量为 `12.3 kg`。
|
||||
- 当前新版 MJCF 的惯性质量合计约为 `18.0377 kg`。
|
||||
- `config.py` 的姿态表默认值为髋俯仰 `0.666`、膝关节 `-1.546`；比赛训练架构的 Rough 默认姿态为 `0.550/-1.125`。
|
||||
|
||||
MuJoCo 直接加载模型的工具会使用 MJCF 内的质量和惯性；显式读取 `ROBOT_MASS` 的解析计算和 MPC 工具仍使用历史常量。使用输出作为新版本控制参数前，应先根据目标机械状态完成质量、惯性和默认姿态复核。
|
||||
@@ -0,0 +1,81 @@
|
||||
"""Robot constants and control parameters."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
# Paths
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
|
||||
MJCF_PATH = REPO_ROOT / "mjcf" / "wheelleg.xml"
|
||||
|
||||
# Robot geometry
|
||||
WHEEL_RADIUS = 0.10 # m
|
||||
WHEEL_TRACK = 0.32 # m, left-right distance
|
||||
ROBOT_MASS = 12.3 # kg
|
||||
MAX_TORQUE = 17.0 # Nm per joint
|
||||
MAX_JOINT_VEL = 13.0 # rad/s
|
||||
|
||||
# Leg link lengths from MJCF, measured to wheel center.
|
||||
L_THIGH = 0.25
|
||||
L_CALF = 0.20
|
||||
|
||||
# Leg names and joint ordering
|
||||
LEG_NAMES = ("fl", "fr", "rl", "rr")
|
||||
LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
|
||||
WHEEL_JOINT = "wheel_joint"
|
||||
|
||||
# Default standing pose aligned with the soft wheel-X height table.
|
||||
# height ~= 0.37m, wheel x-offset ~= 0, peak/RMS leg torque balanced.
|
||||
DEFAULT_JOINT_ANGLES = {
|
||||
"hip_abduction": 0.0,
|
||||
"hip_pitch": 0.666,
|
||||
"knee": -1.546,
|
||||
}
|
||||
|
||||
# Actuator modes, configured at runtime:
|
||||
# Leg joints: position PD, ctrl = target angle
|
||||
# Wheel joints: velocity, ctrl = target velocity in rad/s
|
||||
|
||||
# Control rates
|
||||
SIM_DT = 0.002
|
||||
CTRL_DT = 0.02
|
||||
CTRL_DECIMATION = int(CTRL_DT / SIM_DT)
|
||||
|
||||
# Wheel drive
|
||||
WHEEL_VEL_MAX = 10.0
|
||||
|
||||
# Body pose control gains for height/roll/pitch compensation.
|
||||
KP_HEIGHT = 3.0
|
||||
KP_ROLL = 0.5
|
||||
KP_PITCH = 0.5
|
||||
|
||||
# Calibrated height-to-joint-angle table.
|
||||
# Constraint: avoid large wheel-center X offset from the hip/leg. This is a
|
||||
# soft support-geometry guardrail, not a strict x=0 requirement.
|
||||
# The optimizer also considers peak motor torque and RMS torque, so one hot
|
||||
# motor is not hidden by a low average across all motors.
|
||||
# Format: (height_m, hip_pitch_rad, knee_rad)
|
||||
HEIGHT_TABLE = [
|
||||
(0.17, 0.914, -2.628),
|
||||
(0.19, 0.926, -2.528),
|
||||
(0.21, 0.924, -2.428),
|
||||
(0.23, 0.912, -2.328),
|
||||
(0.25, 0.892, -2.226),
|
||||
(0.27, 0.864, -2.122),
|
||||
(0.29, 0.834, -2.014),
|
||||
(0.31, 0.798, -1.906),
|
||||
(0.33, 0.758, -1.792),
|
||||
(0.35, 0.714, -1.672),
|
||||
(0.37, 0.666, -1.546),
|
||||
(0.39, 0.612, -1.412),
|
||||
(0.41, 0.552, -1.266),
|
||||
(0.43, 0.484, -1.104),
|
||||
(0.45, 0.404, -0.918),
|
||||
]
|
||||
|
||||
# Gait parameters
|
||||
GAIT_FREQ = 2.5
|
||||
GAIT_DUTY = 0.6
|
||||
SWING_HEIGHT = 0.06
|
||||
|
||||
# Trot phase offsets: FL/RR in phase, FR/RL in phase
|
||||
PHASE_OFFSETS = {"fl": 0.0, "fr": 0.5, "rl": 0.5, "rr": 0.0}
|
||||
@@ -0,0 +1,367 @@
|
||||
"""Main controller: wheel mode + trot mode for wheeled-legged robot.
|
||||
|
||||
Wheel mode: differential drive + leg posture hold (height/roll/pitch compensation)
|
||||
Trot mode: quadruped gait with wheel-assisted propulsion
|
||||
|
||||
Actuator interface:
|
||||
- Leg joints: ctrl = target angle (PD: kp=60, kd=3)
|
||||
- Wheel joints: ctrl = target velocity in rad/s (gain=2.0)
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from robot import Robot, RobotState
|
||||
from dynamics import Dynamics
|
||||
from mpc_controller import MPCController
|
||||
from config import (
|
||||
LEG_NAMES, DEFAULT_JOINT_ANGLES, WHEEL_RADIUS, WHEEL_TRACK,
|
||||
WHEEL_VEL_MAX, KP_ROLL, KP_PITCH, HEIGHT_TABLE,
|
||||
GAIT_FREQ, GAIT_DUTY, SWING_HEIGHT, PHASE_OFFSETS,
|
||||
)
|
||||
|
||||
RL_ROUGH_Q = np.array([0.0, 0.550, -1.125], dtype=float)
|
||||
LEG_STATE_IDX = np.array([0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14], dtype=int)
|
||||
|
||||
|
||||
class Controller:
|
||||
"""Wheeled-legged robot controller."""
|
||||
|
||||
def __init__(self, robot: Robot):
|
||||
self.robot = robot
|
||||
self.dynamics = Dynamics()
|
||||
|
||||
# User commands
|
||||
self.vel_x = 0.0 # m/s forward
|
||||
self.vel_y = 0.0 # m/s lateral
|
||||
self.yaw_rate = 0.0 # rad/s
|
||||
self.height = 0.37 # m desired body height (wheel center under hip)
|
||||
self.wheel_posture = "table" # "table" follows height slider; "rl" matches src/robot default
|
||||
|
||||
# Mode: "wheel", "trot", or "mpc"
|
||||
self.mode = "wheel"
|
||||
|
||||
# Prone (lie down) state
|
||||
self.prone = False
|
||||
|
||||
# MPC controller
|
||||
self._mpc_ctrl = MPCController(robot)
|
||||
self._mpc_active = False # track torque mode state
|
||||
|
||||
# Gait state
|
||||
self._gait_phase = 0.0
|
||||
|
||||
# Smoothed commands for trot mode (avoid sudden jumps)
|
||||
self._smooth_vx = 0.0
|
||||
self._smooth_vy = 0.0
|
||||
self._smooth_yaw = 0.0
|
||||
self._last_leg_targets = np.tile(
|
||||
np.array(
|
||||
[
|
||||
DEFAULT_JOINT_ANGLES["hip_abduction"],
|
||||
DEFAULT_JOINT_ANGLES["hip_pitch"],
|
||||
DEFAULT_JOINT_ANGLES["knee"],
|
||||
],
|
||||
dtype=float,
|
||||
),
|
||||
4,
|
||||
)
|
||||
|
||||
# Wheel-mode sensor feedback.
|
||||
self.yaw_rate_kp = 0.45
|
||||
self.roll_comp_gain = KP_ROLL
|
||||
self.pitch_comp_gain = KP_PITCH
|
||||
self.encoder_posture_kp = 0.12
|
||||
self.encoder_posture_max = 0.025
|
||||
self.encoder_guard_start = 0.28
|
||||
self.encoder_guard_stop = 0.65
|
||||
self.imu_guard_start = np.deg2rad(12.0)
|
||||
self.imu_guard_stop = np.deg2rad(28.0)
|
||||
self.yaw_wheel_gain = 1.0
|
||||
self.max_yaw_wheel_speed = 4.0
|
||||
|
||||
# Default leg angles
|
||||
self._default_q = np.array([
|
||||
DEFAULT_JOINT_ANGLES["hip_abduction"],
|
||||
DEFAULT_JOINT_ANGLES["hip_pitch"],
|
||||
DEFAULT_JOINT_ANGLES["knee"],
|
||||
])
|
||||
|
||||
# Swing leg memory
|
||||
self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
|
||||
self._last_contact = {leg: True for leg in LEG_NAMES}
|
||||
|
||||
def compute(self, state: RobotState, dt: float) -> tuple[np.ndarray, np.ndarray]:
|
||||
# Smooth all velocity commands (both modes)
|
||||
alpha = min(dt * 3.0, 1.0) # ~0.33s time constant
|
||||
self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
|
||||
self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
|
||||
self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
|
||||
|
||||
if self.prone:
|
||||
self._ensure_position_mode()
|
||||
return self._prone_mode()
|
||||
if self.mode == "mpc":
|
||||
return self._mpc_mode(state, dt)
|
||||
if self.mode == "wheel":
|
||||
self._ensure_position_mode()
|
||||
return self._wheel_mode(state, dt)
|
||||
else:
|
||||
self._ensure_position_mode()
|
||||
return self._trot_mode(state, dt)
|
||||
|
||||
def _mpc_mode(self, state: RobotState, dt: float):
|
||||
"""MPC locomotion: MIT motor protocol (PD + MPC feedforward torque)."""
|
||||
# Switch to torque mode if not already
|
||||
if not self._mpc_active:
|
||||
self.robot.enable_torque_mode()
|
||||
self._mpc_active = True
|
||||
|
||||
# Sync commands to MPC controller
|
||||
self._mpc_ctrl.vel_x = self.vel_x
|
||||
self._mpc_ctrl.vel_y = self.vel_y
|
||||
self._mpc_ctrl.yaw_rate = self.yaw_rate
|
||||
self._mpc_ctrl.height = self.height
|
||||
|
||||
# Compute and apply (sets ctrl directly via set_ctrl_mit)
|
||||
self._mpc_ctrl.compute(state, dt)
|
||||
# Return dummy - ctrl already set
|
||||
return np.zeros(12), np.zeros(4)
|
||||
|
||||
def _ensure_position_mode(self):
|
||||
"""Switch back to position PD mode if coming from MPC."""
|
||||
if self._mpc_active:
|
||||
self.robot.enable_position_mode()
|
||||
self._mpc_active = False
|
||||
|
||||
def _prone_mode(self):
|
||||
"""Lie down: actual prone pose from real robot."""
|
||||
leg_targets = np.zeros(12)
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
leg_targets[i*3] = side * 0.3 # fl/rl: +0.3, fr/rr: -0.3
|
||||
leg_targets[i*3+1] = 1.5 # hip pitch
|
||||
leg_targets[i*3+2] = -2.65 # knee hard limit from MJCF
|
||||
return leg_targets, np.zeros(4)
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
# WHEEL MODE
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _wheel_mode(self, state: RobotState, dt: float):
|
||||
"""Wheel drive + leg posture hold.
|
||||
|
||||
vel_y: limited effect in wheel mode (differential drive cannot produce
|
||||
pure lateral motion). Uses hip_abduction lean for small lateral force.
|
||||
For significant lateral motion, use trot mode.
|
||||
"""
|
||||
leg_targets = self._posture_control(state)
|
||||
safe_vx, safe_yaw = self._wheel_velocity_envelope(self._smooth_vx, self._smooth_yaw)
|
||||
yaw_feedback = safe_yaw + self.yaw_rate_kp * (safe_yaw - float(state.ang_vel[2]))
|
||||
wheel_targets = self._differential_drive(safe_vx, yaw_feedback)
|
||||
wheel_targets *= self._sensor_command_scale(state, leg_targets)
|
||||
self._last_leg_targets = leg_targets.copy()
|
||||
return leg_targets, wheel_targets
|
||||
|
||||
def _posture_control(self, state: RobotState) -> np.ndarray:
|
||||
"""Leg joint targets from the soft wheel-X height table."""
|
||||
leg_targets = np.zeros(12)
|
||||
|
||||
# Calibrated height→angle lookup (minimizes motor torque at each height)
|
||||
_H = [r[0] for r in HEIGHT_TABLE]
|
||||
_HIP = [r[1] for r in HEIGHT_TABLE]
|
||||
_KNEE = [r[2] for r in HEIGHT_TABLE]
|
||||
|
||||
h_clamp = np.clip(self.height, _H[0], _H[-1])
|
||||
q_hip_base = float(np.interp(h_clamp, _H, _HIP))
|
||||
q_knee_base = float(np.interp(h_clamp, _H, _KNEE))
|
||||
q_ab_base = 0.0
|
||||
if self.wheel_posture == "rl":
|
||||
q_ab_base, q_hip_base, q_knee_base = RL_ROUGH_Q
|
||||
|
||||
roll_corr = -self.roll_comp_gain * state.rpy[0]
|
||||
pitch_corr = -self.pitch_comp_gain * state.rpy[1]
|
||||
lateral_lean = 0.3 * self.vel_y
|
||||
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
leg_targets[i*3] = np.clip(q_ab_base + side * roll_corr + lateral_lean, -0.5, 0.5)
|
||||
leg_targets[i*3+1] = np.clip(q_hip_base + pitch_corr, -1.0, 2.5)
|
||||
leg_targets[i*3+2] = np.clip(q_knee_base, -2.65, -0.3)
|
||||
|
||||
encoder_err = self._last_leg_targets - state.joint_pos[LEG_STATE_IDX]
|
||||
leg_targets += np.clip(
|
||||
self.encoder_posture_kp * encoder_err,
|
||||
-self.encoder_posture_max,
|
||||
self.encoder_posture_max,
|
||||
)
|
||||
leg_targets[0::3] = np.clip(leg_targets[0::3], -0.5, 0.5)
|
||||
leg_targets[1::3] = np.clip(leg_targets[1::3], -1.0, 2.5)
|
||||
leg_targets[2::3] = np.clip(leg_targets[2::3], -2.65, -0.3)
|
||||
return leg_targets
|
||||
|
||||
def _wheel_velocity_envelope(self, vel_x: float, yaw_rate: float) -> tuple[float, float]:
|
||||
"""Limit x/yaw combinations that are unsafe for the RL posture."""
|
||||
ax = abs(vel_x)
|
||||
if ax >= 0.8:
|
||||
yaw_lim = 0.35
|
||||
elif ax >= 0.5:
|
||||
yaw_lim = 0.55
|
||||
elif ax >= 0.25:
|
||||
yaw_lim = 0.75
|
||||
else:
|
||||
yaw_lim = 1.0
|
||||
return float(vel_x), float(np.clip(yaw_rate, -yaw_lim, yaw_lim))
|
||||
|
||||
def _sensor_command_scale(self, state: RobotState, leg_targets: np.ndarray) -> float:
|
||||
"""Back off wheels when IMU or encoder feedback says posture is degrading."""
|
||||
leg_error = float(np.max(np.abs(state.joint_pos[LEG_STATE_IDX] - leg_targets)))
|
||||
tilt = float(np.hypot(state.rpy[0], state.rpy[1]))
|
||||
scale = 1.0
|
||||
|
||||
if leg_error >= self.encoder_guard_stop:
|
||||
scale = 0.0
|
||||
elif leg_error > self.encoder_guard_start:
|
||||
span = max(1e-6, self.encoder_guard_stop - self.encoder_guard_start)
|
||||
scale *= 1.0 - (leg_error - self.encoder_guard_start) / span
|
||||
|
||||
if tilt >= self.imu_guard_stop:
|
||||
scale = 0.0
|
||||
elif tilt > self.imu_guard_start:
|
||||
span = max(1e-6, self.imu_guard_stop - self.imu_guard_start)
|
||||
scale *= 1.0 - (tilt - self.imu_guard_start) / span
|
||||
|
||||
return float(np.clip(scale, 0.0, 1.0))
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
# TROT MODE
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _trot_mode(self, state: RobotState, dt: float):
|
||||
"""Trot gait with wheel assist."""
|
||||
# Advance gait phase
|
||||
self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
|
||||
|
||||
# Contact state
|
||||
contacts = {}
|
||||
for leg in LEG_NAMES:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
contacts[leg] = phase < GAIT_DUTY
|
||||
|
||||
# Pinocchio update
|
||||
q_pin, dq_pin = self.robot.get_qpos_qvel_for_pinocchio()
|
||||
self.dynamics.update(q_pin, dq_pin)
|
||||
|
||||
leg_targets = np.zeros(12)
|
||||
wheel_targets = np.zeros(4)
|
||||
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
if contacts[leg]:
|
||||
# Stance: posture hold
|
||||
leg_targets[i*3:(i+1)*3] = self._stance_leg_target(state, leg)
|
||||
self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
|
||||
self._last_contact[leg] = True
|
||||
# Wheel: drive with smoothed velocity
|
||||
wheel_targets[i] = self._differential_drive_single(
|
||||
self._smooth_vx, self._smooth_yaw, leg)
|
||||
else:
|
||||
# Swing: IK trajectory
|
||||
swing_phase = self._get_swing_phase(leg)
|
||||
target_foot = self._compute_swing_target(leg, state, swing_phase)
|
||||
q_ik = self.dynamics.inverse_kinematics(leg, target_foot, q_pin)
|
||||
leg_targets[i*3:(i+1)*3] = q_ik
|
||||
self._last_contact[leg] = False
|
||||
# Wheel: zero (free during swing)
|
||||
wheel_targets[i] = 0.0
|
||||
|
||||
return leg_targets, wheel_targets
|
||||
|
||||
def _stance_leg_target(self, state: RobotState, leg: str) -> np.ndarray:
|
||||
"""Stance leg: table-interpolated height + attitude compensation."""
|
||||
_H = [r[0] for r in HEIGHT_TABLE]
|
||||
_HIP = [r[1] for r in HEIGHT_TABLE]
|
||||
_KNEE = [r[2] for r in HEIGHT_TABLE]
|
||||
|
||||
h_clamp = np.clip(self.height, _H[0], _H[-1])
|
||||
q_hip = float(np.interp(h_clamp, _H, _HIP))
|
||||
q_knee = float(np.interp(h_clamp, _H, _KNEE))
|
||||
|
||||
roll_corr = -KP_ROLL * state.rpy[0]
|
||||
pitch_corr = -KP_PITCH * state.rpy[1]
|
||||
side = 1.0 if leg[1] == "l" else -1.0
|
||||
lateral_lean = 0.3 * self.vel_y
|
||||
|
||||
return np.array([
|
||||
np.clip(side * roll_corr + lateral_lean, -0.5, 0.5),
|
||||
np.clip(q_hip + pitch_corr, -1.0, 2.5),
|
||||
np.clip(q_knee, -2.65, -0.3),
|
||||
])
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
# DIFFERENTIAL DRIVE
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray:
|
||||
"""4 wheel velocities from body commands."""
|
||||
linear_wheel = vel_x / WHEEL_RADIUS
|
||||
yaw_wheel = self.yaw_wheel_gain * 0.5 * WHEEL_TRACK * yaw_rate / WHEEL_RADIUS
|
||||
yaw_wheel = float(np.clip(yaw_wheel, -self.max_yaw_wheel_speed, self.max_yaw_wheel_speed))
|
||||
vel_left = linear_wheel - yaw_wheel
|
||||
vel_right = linear_wheel + yaw_wheel
|
||||
targets = np.zeros(4)
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
targets[i] = vel_left if leg[1] == "l" else vel_right
|
||||
return np.clip(targets, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
|
||||
|
||||
def _differential_drive_single(self, vel_x: float, yaw_rate: float, leg: str) -> float:
|
||||
if leg[1] == "l":
|
||||
v = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
else:
|
||||
v = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
|
||||
return np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
# SWING TRAJECTORY
|
||||
# ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _get_swing_phase(self, leg: str) -> float:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
if phase < GAIT_DUTY:
|
||||
return 0.0
|
||||
return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
|
||||
|
||||
def _compute_swing_target(self, leg: str, state: RobotState,
|
||||
swing_phase: float) -> np.ndarray:
|
||||
"""Swing foot target with Raibert heuristic using COMMANDED velocity."""
|
||||
p_start = self._swing_start_foot[leg]
|
||||
p_end = self._compute_touchdown(leg, state)
|
||||
|
||||
s = swing_phase
|
||||
s_mj = 10*s**3 - 15*s**4 + 6*s**5
|
||||
|
||||
pos = p_start + (p_end - p_start) * s_mj
|
||||
|
||||
# Z lift
|
||||
z_lift = 64.0 * s**3 * (1.0 - s)**3
|
||||
pos[2] = p_start[2] + SWING_HEIGHT * z_lift
|
||||
|
||||
return pos
|
||||
|
||||
def _compute_touchdown(self, leg: str, state: RobotState) -> np.ndarray:
|
||||
"""Raibert heuristic using COMMANDED velocity.
|
||||
|
||||
When commands are zero, foot lands at its takeoff position (no net motion).
|
||||
When commands are nonzero, foot placement is offset by commanded velocity.
|
||||
"""
|
||||
# Base: land where the foot took off (zero net displacement)
|
||||
td = self._swing_start_foot[leg].copy()
|
||||
|
||||
# Add commanded velocity offset (Raibert-style)
|
||||
t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY
|
||||
yaw = state.rpy[2]
|
||||
c, s = np.cos(yaw), np.sin(yaw)
|
||||
R_z = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
|
||||
cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
|
||||
|
||||
td[0] += cmd_vel_world[0] * t_stance * 0.5
|
||||
td[1] += cmd_vel_world[1] * t_stance * 0.5
|
||||
td[2] = WHEEL_RADIUS # ground level
|
||||
return td
|
||||
@@ -0,0 +1,97 @@
|
||||
"""Pinocchio dynamics: FK, Jacobian, IK for the wheeled-legged robot."""
|
||||
|
||||
import numpy as np
|
||||
import pinocchio as pin
|
||||
from config import MJCF_PATH, LEG_NAMES
|
||||
|
||||
# Foot frame names in Pinocchio model (wheel link centers)
|
||||
FOOT_FRAMES = {leg: f"{leg}_wheel_Link" for leg in LEG_NAMES}
|
||||
|
||||
# Leg joint names for each leg
|
||||
_LEG_JOINT_NAMES = {
|
||||
leg: [f"{leg}_{jt}" for jt in ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")]
|
||||
for leg in LEG_NAMES
|
||||
}
|
||||
|
||||
|
||||
class Dynamics:
|
||||
"""Pinocchio-based kinematics/dynamics. Deployable on real hardware."""
|
||||
|
||||
def __init__(self):
|
||||
self.model = pin.buildModelFromMJCF(str(MJCF_PATH))
|
||||
self.data = self.model.createData()
|
||||
|
||||
# Cache frame IDs
|
||||
self._foot_fids = {}
|
||||
for leg, fname in FOOT_FRAMES.items():
|
||||
self._foot_fids[leg] = self.model.getFrameId(fname)
|
||||
|
||||
# Cache joint velocity indices for each leg (3 joints)
|
||||
self._leg_v_indices = {}
|
||||
for leg, jnames in _LEG_JOINT_NAMES.items():
|
||||
indices = []
|
||||
for jn in jnames:
|
||||
jid = self.model.getJointId(jn)
|
||||
indices.append(self.model.joints[jid].idx_v)
|
||||
self._leg_v_indices[leg] = indices
|
||||
|
||||
# Cache joint config indices for each leg
|
||||
self._leg_q_indices = {}
|
||||
for leg, jnames in _LEG_JOINT_NAMES.items():
|
||||
indices = []
|
||||
for jn in jnames:
|
||||
jid = self.model.getJointId(jn)
|
||||
indices.append(self.model.joints[jid].idx_q)
|
||||
self._leg_q_indices[leg] = indices
|
||||
|
||||
def update(self, q: np.ndarray, dq: np.ndarray):
|
||||
"""Forward kinematics + Jacobians.
|
||||
|
||||
Args:
|
||||
q: Pinocchio config (nq=23: pos3, quat_xyzw4, joints16)
|
||||
dq: Pinocchio velocity (nv=22: v_body3, w_body3, joints16)
|
||||
"""
|
||||
pin.forwardKinematics(self.model, self.data, q, dq)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
|
||||
def get_foot_pos(self, leg: str) -> np.ndarray:
|
||||
"""Foot (wheel center) position in world frame (3,)."""
|
||||
return self.data.oMf[self._foot_fids[leg]].translation.copy()
|
||||
|
||||
def get_foot_jacobian_leg(self, leg: str) -> np.ndarray:
|
||||
"""3x3 linear Jacobian of foot w.r.t. 3 leg joints (world frame)."""
|
||||
fid = self._foot_fids[leg]
|
||||
J_full = pin.getFrameJacobian(
|
||||
self.model, self.data, fid, pin.LOCAL_WORLD_ALIGNED)[:3, :]
|
||||
cols = self._leg_v_indices[leg]
|
||||
return J_full[:, cols]
|
||||
|
||||
def inverse_kinematics(self, leg: str, target_pos: np.ndarray,
|
||||
q_current: np.ndarray, max_iter=30, eps=1e-4) -> np.ndarray:
|
||||
"""Numerical IK for one leg. Returns (3,) joint angles.
|
||||
|
||||
Args:
|
||||
leg: Leg name
|
||||
target_pos: Desired foot position in world frame (3,)
|
||||
q_current: Current full Pinocchio config (nq=23)
|
||||
"""
|
||||
q = q_current.copy()
|
||||
fid = self._foot_fids[leg]
|
||||
q_indices = self._leg_q_indices[leg]
|
||||
|
||||
for _ in range(max_iter):
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
err = target_pos - self.data.oMf[fid].translation
|
||||
if np.linalg.norm(err) < eps:
|
||||
break
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
J = pin.getFrameJacobian(
|
||||
self.model, self.data, fid, pin.LOCAL_WORLD_ALIGNED)[:3, :]
|
||||
J_leg = J[:, self._leg_v_indices[leg]]
|
||||
dq = np.linalg.solve(J_leg.T @ J_leg + 1e-6 * np.eye(3), J_leg.T @ err)
|
||||
for i, idx in enumerate(q_indices):
|
||||
q[idx] += dq[i]
|
||||
|
||||
return np.array([q[idx] for idx in q_indices])
|
||||
@@ -0,0 +1,127 @@
|
||||
"""GUI control panel for the wheeled-legged robot."""
|
||||
|
||||
import tkinter as tk
|
||||
from tkinter import ttk
|
||||
|
||||
|
||||
class GUI:
|
||||
"""Tkinter control panel: sliders + gait buttons + status display."""
|
||||
|
||||
def __init__(self, controller):
|
||||
self.ctrl = controller
|
||||
self.root = tk.Tk()
|
||||
self.root.title("WheelLeg Control")
|
||||
self.root.geometry("400x500")
|
||||
self.root.protocol("WM_DELETE_WINDOW", self._on_close)
|
||||
self._closed = False
|
||||
|
||||
self._build()
|
||||
|
||||
def _build(self):
|
||||
# Mode buttons
|
||||
mf = ttk.LabelFrame(self.root, text="Mode")
|
||||
mf.pack(fill="x", padx=8, pady=4)
|
||||
for mode in ("wheel", "trot", "mpc"):
|
||||
ttk.Button(mf, text=mode.upper(),
|
||||
command=lambda m=mode: self._set_mode(m)
|
||||
).pack(side="left", padx=4, expand=True)
|
||||
ttk.Button(mf, text="PRONE/STAND",
|
||||
command=self._toggle_prone).pack(side="left", padx=4, expand=True)
|
||||
|
||||
# Command sliders
|
||||
cf = ttk.LabelFrame(self.root, text="Commands")
|
||||
cf.pack(fill="x", padx=8, pady=4)
|
||||
|
||||
self.vel_x_var = tk.DoubleVar(value=0.0)
|
||||
self.vel_y_var = tk.DoubleVar(value=0.0)
|
||||
self.yaw_var = tk.DoubleVar(value=0.0)
|
||||
self.height_var = tk.DoubleVar(value=self.ctrl.height)
|
||||
|
||||
self._slider(cf, "Vel X", self.vel_x_var, -1.5, 1.5)
|
||||
self._slider(cf, "Vel Y*", self.vel_y_var, -0.5, 0.5)
|
||||
self._slider(cf, "Yaw", self.yaw_var, -2.0, 2.0)
|
||||
self._slider(cf, "Height", self.height_var, 0.17, 0.45)
|
||||
|
||||
ttk.Label(cf, text="* Vel Y: trot mode only (diff-drive can't sidestep)",
|
||||
font=("", 8)).pack(anchor="w", padx=8)
|
||||
|
||||
ttk.Button(cf, text="Reset", command=self._reset).pack(pady=4)
|
||||
|
||||
# Status display
|
||||
sf = ttk.LabelFrame(self.root, text="Status")
|
||||
sf.pack(fill="both", expand=True, padx=8, pady=4)
|
||||
self.status_text = tk.Text(sf, height=12, width=45, font=("Consolas", 9))
|
||||
self.status_text.pack(fill="both", expand=True, padx=4, pady=4)
|
||||
|
||||
def _slider(self, parent, label, var, lo, hi):
|
||||
f = ttk.Frame(parent)
|
||||
f.pack(fill="x", padx=4, pady=2)
|
||||
ttk.Label(f, text=label, width=7).pack(side="left")
|
||||
ttk.Scale(f, from_=lo, to=hi, variable=var,
|
||||
command=lambda *_: self._sync()).pack(side="left", fill="x", expand=True)
|
||||
lbl = ttk.Label(f, text="0.00", width=6)
|
||||
lbl.pack(side="left")
|
||||
var.trace_add("write", lambda *_, v=var, l=lbl: l.config(text=f"{v.get():.2f}"))
|
||||
|
||||
def _set_mode(self, mode):
|
||||
self.ctrl.mode = mode
|
||||
self.ctrl.prone = False
|
||||
|
||||
def _toggle_prone(self):
|
||||
self.ctrl.prone = not self.ctrl.prone
|
||||
|
||||
def _sync(self):
|
||||
self.ctrl.vel_x = self.vel_x_var.get()
|
||||
self.ctrl.vel_y = self.vel_y_var.get()
|
||||
self.ctrl.yaw_rate = self.yaw_var.get()
|
||||
self.ctrl.height = self.height_var.get()
|
||||
|
||||
def _reset(self):
|
||||
self.vel_x_var.set(0.0)
|
||||
self.vel_y_var.set(0.0)
|
||||
self.yaw_var.set(0.0)
|
||||
self._sync()
|
||||
|
||||
def _on_close(self):
|
||||
self._closed = True
|
||||
self.root.destroy()
|
||||
|
||||
@property
|
||||
def closed(self):
|
||||
return self._closed
|
||||
|
||||
def update_status(self, state, step):
|
||||
"""Update status text with current robot state."""
|
||||
txt = (
|
||||
f"Mode: {self.ctrl.mode} Step: {step}\n"
|
||||
f"Pos: x={state.pos[0]:.3f} y={state.pos[1]:.3f} z={state.pos[2]:.3f}\n"
|
||||
f"RPY: r={np.degrees(state.rpy[0]):.1f}° p={np.degrees(state.rpy[1]):.1f}° "
|
||||
f"y={np.degrees(state.rpy[2]):.1f}°\n"
|
||||
f"Vel: vx={state.lin_vel[0]:.3f} vy={state.lin_vel[1]:.3f} vz={state.lin_vel[2]:.3f}\n"
|
||||
f"Cmd: vx={self.ctrl.vel_x:.2f} yaw={self.ctrl.yaw_rate:.2f} h={self.ctrl.height:.3f}\n"
|
||||
f"─────────────────────────────────\n"
|
||||
)
|
||||
# Joint angles (compact)
|
||||
for i, leg in enumerate(("FL", "FR", "RL", "RR")):
|
||||
q = state.joint_pos[i*4:i*4+3]
|
||||
w = state.joint_vel[i*4+3]
|
||||
txt += f"{leg}: [{q[0]:+.2f} {q[1]:+.2f} {q[2]:+.2f}] w={w:+.1f}\n"
|
||||
|
||||
self.status_text.delete("1.0", tk.END)
|
||||
self.status_text.insert(tk.END, txt)
|
||||
|
||||
def tick(self):
|
||||
"""Process GUI events. Returns False if window closed."""
|
||||
if self._closed:
|
||||
return False
|
||||
try:
|
||||
self.root.update_idletasks()
|
||||
self.root.update()
|
||||
return True
|
||||
except tk.TclError:
|
||||
self._closed = True
|
||||
return False
|
||||
|
||||
|
||||
# Need numpy for degrees conversion in update_status
|
||||
import numpy as np
|
||||
@@ -0,0 +1,389 @@
|
||||
"""Sweep wheel-mode IK postures for differential-drive tracking.
|
||||
|
||||
The sweep is intentionally small and reproducible:
|
||||
1. Generate ab=0 leg postures in the requested height range.
|
||||
2. Keep candidates with good static geometry from rl_friendly_opt.
|
||||
3. Simulate forward, yaw, and arc commands in MuJoCo.
|
||||
4. Rank by attitude, x-speed tracking, yaw-rate tracking, and wheel contact.
|
||||
|
||||
Usage:
|
||||
uv run python mujoco_sim/ik_diff_sweep.py --quick
|
||||
uv run python mujoco_sim/ik_diff_sweep.py --height-min 0.15 --height-max 0.42
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
|
||||
THIS_DIR = Path(__file__).resolve().parent
|
||||
if str(THIS_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(THIS_DIR))
|
||||
|
||||
from config import LEG_JOINTS, LEG_NAMES, SCENE_XML, WHEEL_JOINT, WHEEL_RADIUS # noqa: E402
|
||||
from rl_friendly_opt import get_all, rl_cost # noqa: E402
|
||||
from robot import Robot # noqa: E402
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Candidate:
|
||||
height: float
|
||||
ab: float
|
||||
hip: float
|
||||
knee: float
|
||||
static_cost: float
|
||||
r_hip_x: float
|
||||
cond: float
|
||||
max_tau: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Trial:
|
||||
name: str
|
||||
vx: float
|
||||
yaw_rate: float
|
||||
duration: float
|
||||
|
||||
|
||||
def wrap_pi(x: float) -> float:
|
||||
return (x + math.pi) % (2.0 * math.pi) - math.pi
|
||||
|
||||
|
||||
def body_track(model: mujoco.MjModel, data: mujoco.MjData) -> float:
|
||||
wheel_bids = []
|
||||
for leg in LEG_NAMES:
|
||||
bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link")
|
||||
wheel_bids.append(bid)
|
||||
y = data.xipos[wheel_bids, 1]
|
||||
return float(np.mean(y[[0, 2]]) - np.mean(y[[1, 3]]))
|
||||
|
||||
|
||||
def build_maps(robot: Robot) -> tuple[dict[str, int], dict[str, int], dict[str, int]]:
|
||||
act: dict[str, int] = {}
|
||||
qadr: dict[str, int] = {}
|
||||
vadr: dict[str, int] = {}
|
||||
for leg in LEG_NAMES:
|
||||
for jt in (*LEG_JOINTS, WHEEL_JOINT):
|
||||
name = f"{leg}_{jt}"
|
||||
act[name] = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
jid = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||
qadr[name] = robot.model.jnt_qposadr[jid]
|
||||
vadr[name] = robot.model.jnt_dofadr[jid]
|
||||
return act, qadr, vadr
|
||||
|
||||
|
||||
def set_posture(robot: Robot, cand: Candidate, act: dict[str, int], qadr: dict[str, int]) -> None:
|
||||
mujoco.mj_resetData(robot.model, robot.data)
|
||||
for leg in LEG_NAMES:
|
||||
side_ab = cand.ab if leg[1] == "l" else -cand.ab
|
||||
for jt, val in zip(LEG_JOINTS, (side_ab, cand.hip, cand.knee)):
|
||||
name = f"{leg}_{jt}"
|
||||
robot.data.qpos[qadr[name]] = val
|
||||
robot.data.ctrl[act[name]] = val
|
||||
robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
|
||||
robot.data.qpos[:3] = [0.0, 0.0, max(0.25, cand.height + 0.08)]
|
||||
robot.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
|
||||
robot.data.qvel[:] = 0.0
|
||||
mujoco.mj_forward(robot.model, robot.data)
|
||||
|
||||
|
||||
def wheel_targets(
|
||||
vx: float,
|
||||
yaw_rate: float,
|
||||
track: float,
|
||||
max_wheel: float,
|
||||
yaw_gain: float,
|
||||
wheel_model: str,
|
||||
linear_gain: float,
|
||||
direct_yaw_gain: float,
|
||||
wheel_signs: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
if wheel_model == "direct":
|
||||
left = linear_gain * vx - direct_yaw_gain * yaw_rate
|
||||
right = linear_gain * vx + direct_yaw_gain * yaw_rate
|
||||
else:
|
||||
left = (vx - yaw_gain * 0.5 * track * yaw_rate) / WHEEL_RADIUS
|
||||
right = (vx + yaw_gain * 0.5 * track * yaw_rate) / WHEEL_RADIUS
|
||||
raw = np.array([left, right, left, right], dtype=float)
|
||||
return np.clip(raw * wheel_signs, -max_wheel, max_wheel)
|
||||
|
||||
|
||||
def run_trial(robot: Robot, cand: Candidate, trial: Trial, args: argparse.Namespace) -> dict:
|
||||
act, qadr, vadr = build_maps(robot)
|
||||
set_posture(robot, cand, act, qadr)
|
||||
|
||||
ctrl_dt = args.control_dt
|
||||
sim_dt = robot.model.opt.timestep
|
||||
steps_per_ctrl = max(1, int(round(ctrl_dt / sim_dt)))
|
||||
track = body_track(robot.model, robot.data) if args.track_source == "model" else args.track_width
|
||||
wheel_signs = np.array(args.wheel_signs, dtype=float)
|
||||
wheel_cmd = wheel_targets(
|
||||
trial.vx,
|
||||
trial.yaw_rate,
|
||||
track,
|
||||
args.max_wheel_speed,
|
||||
args.yaw_gain,
|
||||
args.wheel_model,
|
||||
args.linear_gain,
|
||||
args.direct_yaw_gain,
|
||||
wheel_signs,
|
||||
)
|
||||
|
||||
for _ in range(int(round(args.settle / sim_dt))):
|
||||
for leg in LEG_NAMES:
|
||||
vals = (cand.ab if leg[1] == "l" else -cand.ab, cand.hip, cand.knee)
|
||||
for jt, val in zip(LEG_JOINTS, vals):
|
||||
robot.data.ctrl[act[f"{leg}_{jt}"]] = val
|
||||
robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
|
||||
robot.step()
|
||||
|
||||
state0 = robot.get_state()
|
||||
yaw0 = float(state0.rpy[2])
|
||||
x0 = float(state0.pos[0])
|
||||
|
||||
max_roll = 0.0
|
||||
max_pitch = 0.0
|
||||
max_tilt = 0.0
|
||||
max_wheel_air = -1e9
|
||||
wheel_err_sum = 0.0
|
||||
samples = 0
|
||||
max_leg_err = 0.0
|
||||
body_vx_sum = 0.0
|
||||
yaw_unwrapped = 0.0
|
||||
last_yaw = yaw0
|
||||
leg_target = np.array([cand.ab, cand.hip, cand.knee] * 4, dtype=float)
|
||||
|
||||
total_steps = int(round(trial.duration / sim_dt))
|
||||
cmd = np.zeros(4, dtype=float)
|
||||
max_delta = args.wheel_accel_limit * ctrl_dt
|
||||
wheel_body_ids = [
|
||||
mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link")
|
||||
for leg in LEG_NAMES
|
||||
]
|
||||
wheel_vadr = np.array([vadr[f"{leg}_{WHEEL_JOINT}"] for leg in LEG_NAMES], dtype=int)
|
||||
leg_qadr = np.array([qadr[f"{leg}_{jt}"] for leg in LEG_NAMES for jt in LEG_JOINTS], dtype=int)
|
||||
|
||||
for step in range(total_steps):
|
||||
if step % steps_per_ctrl == 0:
|
||||
cmd = cmd + np.clip(wheel_cmd - cmd, -max_delta, max_delta)
|
||||
for leg in LEG_NAMES:
|
||||
vals = (cand.ab if leg[1] == "l" else -cand.ab, cand.hip, cand.knee)
|
||||
for jt, val in zip(LEG_JOINTS, vals):
|
||||
robot.data.ctrl[act[f"{leg}_{jt}"]] = val
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = cmd[i]
|
||||
|
||||
robot.step()
|
||||
|
||||
if step % steps_per_ctrl == 0:
|
||||
st = robot.get_state()
|
||||
roll, pitch = float(st.rpy[0]), float(st.rpy[1])
|
||||
yaw_now = float(st.rpy[2])
|
||||
yaw_unwrapped += wrap_pi(yaw_now - last_yaw)
|
||||
last_yaw = yaw_now
|
||||
body_vx_sum += float(st.rot[:, 0].dot(st.lin_vel))
|
||||
max_roll = max(max_roll, abs(roll))
|
||||
max_pitch = max(max_pitch, abs(pitch))
|
||||
max_tilt = max(max_tilt, math.hypot(roll, pitch))
|
||||
wheel_air = robot.data.xipos[wheel_body_ids, 2] - WHEEL_RADIUS
|
||||
max_wheel_air = max(max_wheel_air, float(np.max(wheel_air)))
|
||||
wheel_err_sum += float(np.mean(np.abs(robot.data.qvel[wheel_vadr] - cmd)))
|
||||
max_leg_err = max(max_leg_err, float(np.max(np.abs(robot.data.qpos[leg_qadr] - leg_target))))
|
||||
samples += 1
|
||||
|
||||
st = robot.get_state()
|
||||
elapsed = max(1e-6, float(st.time - state0.time))
|
||||
world_x_rate = (float(st.pos[0]) - x0) / elapsed
|
||||
x_rate = body_vx_sum / max(1, samples)
|
||||
yaw_rate = yaw_unwrapped / max(1e-6, samples * steps_per_ctrl * sim_dt)
|
||||
x_err = abs(x_rate - trial.vx)
|
||||
yaw_err = abs(yaw_rate - trial.yaw_rate)
|
||||
return {
|
||||
"trial": trial.name,
|
||||
"x_rate": x_rate,
|
||||
"world_x_rate": world_x_rate,
|
||||
"yaw_rate": yaw_rate,
|
||||
"x_err": x_err,
|
||||
"yaw_err": yaw_err,
|
||||
"max_roll_deg": math.degrees(max_roll),
|
||||
"max_pitch_deg": math.degrees(max_pitch),
|
||||
"max_tilt_deg": math.degrees(max_tilt),
|
||||
"max_wheel_air_m": max_wheel_air,
|
||||
"mean_wheel_err": wheel_err_sum / max(1, samples),
|
||||
"max_leg_err": max_leg_err,
|
||||
"track": track,
|
||||
"wheel_cmd": [float(x) for x in wheel_cmd],
|
||||
}
|
||||
|
||||
|
||||
def generate_candidates(args: argparse.Namespace) -> list[Candidate]:
|
||||
if args.fixed_hip is not None or args.fixed_knee is not None:
|
||||
if args.fixed_hip is None or args.fixed_knee is None:
|
||||
raise SystemExit("--fixed-hip and --fixed-knee must be provided together")
|
||||
ab = float(args.fixed_ab)
|
||||
hip = float(args.fixed_hip)
|
||||
knee = float(args.fixed_knee)
|
||||
r = get_all(ab, hip, knee)
|
||||
return [
|
||||
Candidate(
|
||||
height=float(r["z"]),
|
||||
ab=ab,
|
||||
hip=hip,
|
||||
knee=knee,
|
||||
static_cost=float(rl_cost(r)),
|
||||
r_hip_x=float(r["r_hip_x_mag"]),
|
||||
cond=float(r["cond"]),
|
||||
max_tau=float(r["max_tau"]),
|
||||
)
|
||||
]
|
||||
|
||||
cands: list[Candidate] = []
|
||||
h_targets = np.arange(args.height_min, args.height_max + 0.5 * args.height_step, args.height_step)
|
||||
ab_values = [0.0] if args.ab_max <= 1e-9 else np.arange(0.0, args.ab_max + 1e-9, args.ab_step)
|
||||
hip_values = np.arange(args.hip_min, args.hip_max + 0.5 * args.hip_step, args.hip_step)
|
||||
knee_values = np.arange(args.knee_min, args.knee_max + 0.5 * args.knee_step, args.knee_step)
|
||||
for ht in h_targets:
|
||||
bucket: list[Candidate] = []
|
||||
for ab in ab_values:
|
||||
for hip in hip_values:
|
||||
for knee in knee_values:
|
||||
r = get_all(float(ab), float(hip), float(knee))
|
||||
if float(r["z"]) < args.height_min or float(r["z"]) > args.height_max:
|
||||
continue
|
||||
if abs(float(r["z"]) - float(ht)) > args.height_tol:
|
||||
continue
|
||||
if r["wz"] >= r["kz"]:
|
||||
continue
|
||||
if abs(r["r_hip_x_mag"]) > args.max_wheel_x:
|
||||
continue
|
||||
cost = float(rl_cost(r))
|
||||
bucket.append(
|
||||
Candidate(
|
||||
height=float(r["z"]),
|
||||
ab=float(ab),
|
||||
hip=float(hip),
|
||||
knee=float(knee),
|
||||
static_cost=cost,
|
||||
r_hip_x=float(r["r_hip_x_mag"]),
|
||||
cond=float(r["cond"]),
|
||||
max_tau=float(r["max_tau"]),
|
||||
)
|
||||
)
|
||||
bucket.sort(key=lambda c: c.static_cost)
|
||||
cands.extend(bucket[: args.top_per_height])
|
||||
return cands
|
||||
|
||||
|
||||
def score_result(cand: Candidate, trials: list[dict]) -> float:
|
||||
score = 0.08 * cand.static_cost
|
||||
for t in trials:
|
||||
score += 8.0 * t["x_err"]
|
||||
score += 10.0 * t["yaw_err"]
|
||||
score += 0.08 * t["max_tilt_deg"]
|
||||
score += 0.03 * max(0.0, t["max_pitch_deg"] - 8.0) ** 2
|
||||
score += 20.0 * max(0.0, t["max_wheel_air_m"] - 0.015)
|
||||
score += 1.5 * t["mean_wheel_err"]
|
||||
score += 2.0 * max(0.0, t["max_leg_err"] - 0.35)
|
||||
return float(score)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--height-min", type=float, default=0.15)
|
||||
parser.add_argument("--height-max", type=float, default=0.42)
|
||||
parser.add_argument("--height-step", type=float, default=0.01)
|
||||
parser.add_argument("--height-tol", type=float, default=0.004)
|
||||
parser.add_argument("--top-per-height", type=int, default=1)
|
||||
parser.add_argument("--ab-max", type=float, default=0.0)
|
||||
parser.add_argument("--ab-step", type=float, default=0.04)
|
||||
parser.add_argument("--hip-min", type=float, default=0.25)
|
||||
parser.add_argument("--hip-max", type=float, default=1.05)
|
||||
parser.add_argument("--hip-step", type=float, default=0.025)
|
||||
parser.add_argument("--knee-min", type=float, default=-2.65)
|
||||
parser.add_argument("--knee-max", type=float, default=-0.85)
|
||||
parser.add_argument("--knee-step", type=float, default=0.025)
|
||||
parser.add_argument("--max-wheel-x", type=float, default=0.09)
|
||||
parser.add_argument("--fixed-ab", type=float, default=0.0)
|
||||
parser.add_argument("--fixed-hip", type=float, default=None)
|
||||
parser.add_argument("--fixed-knee", type=float, default=None)
|
||||
parser.add_argument("--duration", type=float, default=4.0)
|
||||
parser.add_argument("--settle", type=float, default=1.5)
|
||||
parser.add_argument("--control-dt", type=float, default=0.02)
|
||||
parser.add_argument("--vx", type=float, default=0.6)
|
||||
parser.add_argument("--yaw", type=float, default=0.3)
|
||||
parser.add_argument("--arc-yaw", type=float, default=0.15)
|
||||
parser.add_argument("--yaw-gain", type=float, default=1.0)
|
||||
parser.add_argument("--wheel-model", choices=("diff", "direct"), default="diff")
|
||||
parser.add_argument("--linear-gain", type=float, default=12.5)
|
||||
parser.add_argument("--direct-yaw-gain", type=float, default=8.0)
|
||||
parser.add_argument("--max-wheel-speed", type=float, default=12.0)
|
||||
parser.add_argument("--wheel-accel-limit", type=float, default=35.0)
|
||||
parser.add_argument("--track-source", choices=("model", "fixed"), default="model")
|
||||
parser.add_argument("--track-width", type=float, default=0.394)
|
||||
parser.add_argument(
|
||||
"--wheel-signs",
|
||||
type=float,
|
||||
nargs=4,
|
||||
default=[1.0, 1.0, 1.0, 1.0],
|
||||
metavar=("FL", "FR", "RL", "RR"),
|
||||
help="Per-wheel velocity sign multipliers in joint order.",
|
||||
)
|
||||
parser.add_argument("--quick", action="store_true")
|
||||
parser.add_argument("--json", type=Path, default=None)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.quick:
|
||||
args.height_step = 0.02
|
||||
args.hip_step = 0.05
|
||||
args.knee_step = 0.05
|
||||
|
||||
candidates = generate_candidates(args)
|
||||
if not candidates:
|
||||
raise SystemExit("No candidates found")
|
||||
|
||||
trials = [
|
||||
Trial("forward", args.vx, 0.0, args.duration),
|
||||
Trial("yaw", 0.0, args.yaw, args.duration),
|
||||
Trial("arc", args.vx, args.arc_yaw, args.duration),
|
||||
]
|
||||
robot = Robot(SCENE_XML)
|
||||
rows = []
|
||||
for i, cand in enumerate(candidates, 1):
|
||||
trial_rows = [run_trial(robot, cand, t, args) for t in trials]
|
||||
rows.append({"candidate": cand.__dict__, "trials": trial_rows, "score": score_result(cand, trial_rows)})
|
||||
if i % 10 == 0:
|
||||
print(f"tested {i}/{len(candidates)}")
|
||||
|
||||
rows.sort(key=lambda r: r["score"])
|
||||
if args.json:
|
||||
args.json.write_text(json.dumps(rows, indent=2), encoding="utf-8")
|
||||
|
||||
print("Top IK postures for differential drive tracking")
|
||||
print("rank score height ab hip knee static xhip cond tau | fwd_x yaw_wz arc_x arc_wz max_tilt max_pitch")
|
||||
for rank, row in enumerate(rows[:10], 1):
|
||||
c = row["candidate"]
|
||||
by = {t["trial"]: t for t in row["trials"]}
|
||||
max_tilt = max(t["max_tilt_deg"] for t in row["trials"])
|
||||
max_pitch = max(t["max_pitch_deg"] for t in row["trials"])
|
||||
print(
|
||||
f"{rank:>2} {row['score']:>7.2f} {c['height']:.3f} {c['ab']:.2f} {c['hip']:.3f} {c['knee']:.3f} "
|
||||
f"{c['static_cost']:.1f} {c['r_hip_x']:.3f} {c['cond']:.2f} {c['max_tau']:.2f} | "
|
||||
f"{by['forward']['x_rate']:.3f} {by['yaw']['yaw_rate']:.3f} {by['arc']['x_rate']:.3f} {by['arc']['yaw_rate']:.3f} "
|
||||
f"{max_tilt:.1f} {max_pitch:.1f}"
|
||||
)
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,242 @@
|
||||
"""Convex MPC solver for wheeled-legged robot.
|
||||
|
||||
Centroidal dynamics: single rigid body model with 4 contact forces.
|
||||
State: x = [pos(3), rpy(3), vel(3), omega(3)] = 12
|
||||
Input: u = [f1(3), f2(3), f3(3), f4(3)] = 12
|
||||
Friction pyramid constraints on each foot.
|
||||
|
||||
Reference: MIT Cheetah 3 Convex MPC (Di Carlo et al.)
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy import sparse
|
||||
from scipy.linalg import block_diag
|
||||
import osqp
|
||||
|
||||
from config import ROBOT_MASS, LEG_NAMES
|
||||
|
||||
# MPC parameters
|
||||
MPC_HORIZON = 10 # prediction steps
|
||||
MPC_DT = 0.02 # 50 Hz MPC update
|
||||
MU = 0.6 # friction coefficient
|
||||
FZ_MAX = 200.0 # max vertical force per leg
|
||||
FZ_MIN = 10.0 # min vertical force (stance)
|
||||
NX = 12 # state dim
|
||||
NU = 12 # input dim (4 legs × 3D force)
|
||||
|
||||
# Cost weights: [pos_x, pos_y, pos_z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz]
|
||||
Q_WEIGHTS = np.array([2.0, 2.0, 50.0, 50.0, 50.0, 10.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0])
|
||||
R_WEIGHTS = np.array([1e-6] * 12)
|
||||
|
||||
|
||||
def _skew(v):
|
||||
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
|
||||
|
||||
|
||||
class ConvexMPC:
|
||||
"""Convex MPC: solves QP for optimal ground reaction forces."""
|
||||
|
||||
def __init__(self, mass=ROBOT_MASS, inertia=None):
|
||||
self.mass = mass
|
||||
# Approximate body inertia (diagonal, world-aligned)
|
||||
if inertia is None:
|
||||
self.I_body = np.diag([0.07, 0.26, 0.24])
|
||||
else:
|
||||
self.I_body = np.array(inertia).reshape(3, 3)
|
||||
|
||||
self.N = MPC_HORIZON
|
||||
self.dt = MPC_DT
|
||||
self.Q = np.diag(Q_WEIGHTS)
|
||||
self.R = np.diag(R_WEIGHTS)
|
||||
self.gravity = np.array([0, 0, -9.81])
|
||||
|
||||
self._last_forces = np.zeros(NU)
|
||||
|
||||
def solve(self, x0, x_ref, foot_positions, contact_schedule):
|
||||
"""Solve MPC QP.
|
||||
|
||||
Args:
|
||||
x0: (12,) current state [pos, rpy, vel, omega]
|
||||
x_ref: (12, N) reference trajectory over horizon
|
||||
foot_positions: (4, 3) foot positions in world frame (relative to CoM)
|
||||
contact_schedule: (4, N) binary contact table (1=stance)
|
||||
|
||||
Returns:
|
||||
forces: (12,) optimal forces for current timestep [f1x,f1y,f1z,...,f4x,f4y,f4z]
|
||||
"""
|
||||
N = self.N
|
||||
|
||||
# Build dynamics matrices
|
||||
Ad, Bd_list, gd = self._discretize_dynamics(x0, foot_positions)
|
||||
|
||||
# Build QP: min 0.5 z'Hz + f'z s.t. lb <= Az <= ub, lbx <= z <= ubx
|
||||
# Decision variables: z = [x1,...,xN, u0,...,uN-1]
|
||||
nvars = N * NX + N * NU
|
||||
|
||||
# --- Hessian ---
|
||||
H_diag = np.concatenate([np.tile(2 * Q_WEIGHTS, N), np.tile(2 * R_WEIGHTS, N)])
|
||||
H = sparse.diags(H_diag, format='csc')
|
||||
|
||||
# --- Gradient ---
|
||||
g = np.zeros(nvars)
|
||||
for k in range(N):
|
||||
g[k*NX:(k+1)*NX] = -2 * self.Q @ x_ref[:, k]
|
||||
|
||||
# --- Dynamics equality constraints ---
|
||||
# x_{k+1} = Ad @ x_k + Bd_k @ u_k + gd
|
||||
# Rewrite: x_{k+1} - Ad @ x_k - Bd_k @ u_k = gd (for k>0)
|
||||
# x_1 - Bd_0 @ u_0 = Ad @ x0 + gd (for k=0)
|
||||
n_eq = N * NX
|
||||
A_eq = np.zeros((n_eq, nvars))
|
||||
b_eq = np.zeros(n_eq)
|
||||
|
||||
# k=0: x_1 = Ad @ x0 + Bd_0 @ u_0 + gd
|
||||
A_eq[0:NX, 0:NX] = np.eye(NX) # x_1
|
||||
A_eq[0:NX, N*NX:N*NX+NU] = -Bd_list[0] # -Bd_0 @ u_0
|
||||
b_eq[0:NX] = Ad @ x0 + gd
|
||||
|
||||
for k in range(1, N):
|
||||
row = k * NX
|
||||
# x_{k+1}
|
||||
A_eq[row:row+NX, k*NX:(k+1)*NX] = np.eye(NX)
|
||||
# -Ad @ x_k
|
||||
A_eq[row:row+NX, (k-1)*NX:k*NX] = -Ad
|
||||
# -Bd_k @ u_k
|
||||
A_eq[row:row+NX, N*NX+k*NU:N*NX+(k+1)*NU] = -Bd_list[k]
|
||||
b_eq[row:row+NX] = gd
|
||||
|
||||
# --- Friction pyramid inequality constraints ---
|
||||
# For each stance leg at each timestep: 4 faces
|
||||
# fx - mu*fz <= 0, -fx - mu*fz <= 0, fy - mu*fz <= 0, -fy - mu*fz <= 0
|
||||
n_ineq_max = 4 * 4 * N
|
||||
A_ineq = np.zeros((n_ineq_max, nvars))
|
||||
u_ineq = np.zeros(n_ineq_max)
|
||||
|
||||
row = 0
|
||||
for k in range(N):
|
||||
u_base = N * NX + k * NU
|
||||
for leg in range(4):
|
||||
if contact_schedule[leg, k] == 1:
|
||||
fx_idx = u_base + leg * 3
|
||||
fy_idx = u_base + leg * 3 + 1
|
||||
fz_idx = u_base + leg * 3 + 2
|
||||
|
||||
# Friction pyramid: stance leg
|
||||
A_ineq[row, fx_idx] = 1.0
|
||||
A_ineq[row, fz_idx] = -MU
|
||||
row += 1
|
||||
A_ineq[row, fx_idx] = -1.0
|
||||
A_ineq[row, fz_idx] = -MU
|
||||
row += 1
|
||||
A_ineq[row, fy_idx] = 1.0
|
||||
A_ineq[row, fz_idx] = -MU
|
||||
row += 1
|
||||
A_ineq[row, fy_idx] = -1.0
|
||||
A_ineq[row, fz_idx] = -MU
|
||||
row += 1
|
||||
|
||||
A_ineq = A_ineq[:row]
|
||||
u_ineq = u_ineq[:row]
|
||||
|
||||
# Stack constraints
|
||||
A_full = np.vstack([A_eq, A_ineq])
|
||||
l_full = np.concatenate([b_eq, -np.inf * np.ones(row)])
|
||||
u_full = np.concatenate([b_eq, u_ineq])
|
||||
|
||||
# --- Box constraints on forces (as identity rows in A) ---
|
||||
A_box = np.zeros((N * NU, nvars))
|
||||
l_box = -np.inf * np.ones(N * NU)
|
||||
u_box = np.inf * np.ones(N * NU)
|
||||
|
||||
for k in range(N):
|
||||
u_base = N * NX + k * NU
|
||||
for leg in range(4):
|
||||
idx = u_base + leg * 3
|
||||
box_row = k * NU + leg * 3
|
||||
# Identity rows for fx, fy, fz
|
||||
for j in range(3):
|
||||
A_box[box_row + j, idx + j] = 1.0
|
||||
|
||||
if contact_schedule[leg, k] == 1:
|
||||
# Stance: fz bounded
|
||||
l_box[box_row + 2] = FZ_MIN
|
||||
u_box[box_row + 2] = FZ_MAX
|
||||
else:
|
||||
# Swing: all forces = 0
|
||||
l_box[box_row:box_row+3] = 0.0
|
||||
u_box[box_row:box_row+3] = 0.0
|
||||
|
||||
# Final constraint matrix
|
||||
A_full = np.vstack([A_full, A_box])
|
||||
l_full = np.concatenate([l_full, l_box])
|
||||
u_full = np.concatenate([u_full, u_box])
|
||||
|
||||
# --- Solve with OSQP ---
|
||||
A_sparse = sparse.csc_matrix(A_full)
|
||||
H_sparse = sparse.triu(H, format='csc')
|
||||
|
||||
solver = osqp.OSQP()
|
||||
solver.setup(H_sparse, g, A_sparse, l_full, u_full,
|
||||
eps_abs=1e-4, eps_rel=1e-4,
|
||||
max_iter=500, polish=True, verbose=False,
|
||||
warm_start=True)
|
||||
|
||||
# Warm start with previous solution
|
||||
if self._last_forces is not None:
|
||||
x_warm = np.zeros(nvars)
|
||||
x_warm[N*NX:N*NX+NU] = self._last_forces
|
||||
solver.warm_start(x=x_warm)
|
||||
|
||||
result = solver.solve()
|
||||
|
||||
if result.info.status == 'solved' or result.info.status == 'solved_inaccurate':
|
||||
# Extract first timestep forces
|
||||
forces = result.x[N*NX:N*NX+NU]
|
||||
self._last_forces = forces.copy()
|
||||
else:
|
||||
forces = self._last_forces
|
||||
|
||||
return forces
|
||||
|
||||
def _discretize_dynamics(self, x0, foot_positions):
|
||||
"""Build discrete-time centroidal dynamics.
|
||||
|
||||
State: [pos, rpy, vel, omega] (12)
|
||||
Continuous: dx/dt = Ac @ x + Bc @ u + gc
|
||||
Discrete: x_{k+1} = Ad @ x + Bd @ u + gd
|
||||
"""
|
||||
m = self.mass
|
||||
I_inv = np.linalg.inv(self.I_body)
|
||||
dt = self.dt
|
||||
yaw = x0[5]
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
|
||||
# Rotation for rpy rate ≈ R_z^T @ omega
|
||||
R_zT = np.array([[cy, sy, 0], [-sy, cy, 0], [0, 0, 1]])
|
||||
|
||||
# Ac (12×12)
|
||||
Ac = np.zeros((NX, NX))
|
||||
Ac[0:3, 6:9] = np.eye(3) # pos_dot = vel
|
||||
Ac[3:6, 9:12] = R_zT # rpy_dot ≈ R_z^T @ omega
|
||||
|
||||
# Ad = I + Ac*dt (first-order)
|
||||
Ad = np.eye(NX) + Ac * dt
|
||||
|
||||
# Bc varies per timestep (foot positions change contact point)
|
||||
Bd_list = []
|
||||
for k in range(self.N):
|
||||
Bc = np.zeros((NX, NU))
|
||||
for leg in range(4):
|
||||
r = foot_positions[leg]
|
||||
# vel_dot += f/m
|
||||
Bc[6:9, leg*3:(leg+1)*3] = np.eye(3) / m
|
||||
# omega_dot += I^{-1} @ (r × f)
|
||||
Bc[9:12, leg*3:(leg+1)*3] = I_inv @ _skew(r)
|
||||
Bd = Bc * dt
|
||||
Bd_list.append(Bd)
|
||||
|
||||
# Gravity contribution
|
||||
gd = np.zeros(NX)
|
||||
gd[6:9] = self.gravity * dt # vel += g*dt
|
||||
|
||||
return Ad, Bd_list, gd
|
||||
@@ -0,0 +1,261 @@
|
||||
"""MPC controller integration for wheeled-legged robot.
|
||||
|
||||
Integrates: gait scheduler + reference trajectory + ConvexMPC solver +
|
||||
swing leg control + stance force mapping + wheel drive.
|
||||
|
||||
Architecture (following go2-convex-mpc):
|
||||
- MPC runs at ~50 Hz (every MPC_DECIMATION control steps)
|
||||
- Swing/stance leg controller runs at control rate (50 Hz)
|
||||
- Wheel drive: stance legs use differential drive, swing legs coast
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from robot import Robot, RobotState
|
||||
from dynamics import Dynamics
|
||||
from mpc import ConvexMPC, MPC_DT
|
||||
from config import (
|
||||
LEG_NAMES, WHEEL_RADIUS, WHEEL_TRACK, WHEEL_VEL_MAX,
|
||||
CTRL_DT, GAIT_FREQ, GAIT_DUTY, SWING_HEIGHT, PHASE_OFFSETS,
|
||||
DEFAULT_JOINT_ANGLES, ROBOT_MASS,
|
||||
)
|
||||
|
||||
# MPC update decimation (relative to control loop)
|
||||
MPC_DECIMATION = max(1, int(MPC_DT / CTRL_DT)) # 1 step at 50Hz
|
||||
|
||||
|
||||
class MPCController:
|
||||
"""Convex MPC locomotion controller for wheeled-legged robot."""
|
||||
|
||||
def __init__(self, robot: Robot):
|
||||
self.robot = robot
|
||||
self.dynamics = Dynamics()
|
||||
self.mpc = ConvexMPC(mass=ROBOT_MASS)
|
||||
|
||||
# User commands
|
||||
self.vel_x = 0.0
|
||||
self.vel_y = 0.0
|
||||
self.yaw_rate = 0.0
|
||||
self.height = 0.35 # actual standing height with default joint angles
|
||||
|
||||
# Gait state - start at phase 0 with all legs in stance (duty=0.6)
|
||||
self._gait_phase = 0.0
|
||||
self._step_count = 0
|
||||
self._initialized = False
|
||||
|
||||
# MPC solution cache - initialize with gravity compensation
|
||||
self._mpc_forces = np.zeros(12)
|
||||
self._init_gravity_comp()
|
||||
|
||||
# Swing trajectory state
|
||||
self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
|
||||
self._swing_start_time = {leg: 0.0 for leg in LEG_NAMES}
|
||||
self._last_contact = {leg: True for leg in LEG_NAMES}
|
||||
|
||||
# Smoothed commands
|
||||
self._smooth_vx = 0.0
|
||||
self._smooth_vy = 0.0
|
||||
self._smooth_yaw = 0.0
|
||||
|
||||
def _init_gravity_comp(self):
|
||||
"""Pre-fill MPC forces with static gravity compensation."""
|
||||
fz_per_leg = ROBOT_MASS * 9.81 / 4.0
|
||||
for i in range(4):
|
||||
self._mpc_forces[i*3 + 2] = fz_per_leg
|
||||
|
||||
def compute(self, state: RobotState, dt: float):
|
||||
"""Main MPC control loop.
|
||||
|
||||
Uses MIT motor protocol: tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff
|
||||
where tau_ff comes from MPC force mapping via Jacobian transpose.
|
||||
|
||||
Returns:
|
||||
tau_legs: (12,) feedforward torques for MIT mode
|
||||
wheel_targets: (4,) wheel velocity targets
|
||||
"""
|
||||
# Smooth commands
|
||||
alpha = min(dt * 3.0, 1.0)
|
||||
self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
|
||||
self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
|
||||
self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
|
||||
|
||||
# Update Pinocchio
|
||||
q_pin, dq_pin = self.robot.get_qpos_qvel_for_pinocchio()
|
||||
self.dynamics.update(q_pin, dq_pin)
|
||||
|
||||
# Initialize foot positions on first call
|
||||
if not self._initialized:
|
||||
for leg in LEG_NAMES:
|
||||
self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
|
||||
self._initialized = True
|
||||
|
||||
# Decide if we should trot or just stand
|
||||
moving = (abs(self._smooth_vx) > 0.02 or
|
||||
abs(self._smooth_vy) > 0.02 or
|
||||
abs(self._smooth_yaw) > 0.05)
|
||||
|
||||
if moving:
|
||||
self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
|
||||
else:
|
||||
self._gait_phase = 0.0 # all legs in stance
|
||||
|
||||
# Contact schedule
|
||||
contacts = {}
|
||||
for leg in LEG_NAMES:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
contacts[leg] = phase < GAIT_DUTY
|
||||
|
||||
# Get foot positions relative to CoM
|
||||
foot_positions = np.zeros((4, 3))
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
foot_positions[i] = self.dynamics.get_foot_pos(leg) - state.pos
|
||||
|
||||
# --- Run MPC at lower rate ---
|
||||
if self._step_count % MPC_DECIMATION == 0:
|
||||
x0 = self._build_state_vector(state)
|
||||
x_ref = self._build_reference(state)
|
||||
contact_table = self._build_contact_table()
|
||||
self._mpc_forces = self.mpc.solve(x0, x_ref, foot_positions, contact_table)
|
||||
|
||||
self._step_count += 1
|
||||
|
||||
# --- Compute feedforward torques and desired joint positions ---
|
||||
tau_ff = np.zeros(12)
|
||||
q_des = np.zeros(12)
|
||||
dq_des = np.zeros(12)
|
||||
kp = np.zeros(12)
|
||||
kd = np.zeros(12)
|
||||
wheel_targets = np.zeros(4)
|
||||
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
if contacts[leg]:
|
||||
# Stance: MPC force → feedforward torque, PD holds posture
|
||||
f_leg = self._mpc_forces[i*3:(i+1)*3]
|
||||
J = self.dynamics.get_foot_jacobian_leg(leg)
|
||||
tau_ff[i*3:(i+1)*3] = J.T @ (-f_leg)
|
||||
|
||||
# PD target: default standing angles (posture hold)
|
||||
q_des[i*3] = DEFAULT_JOINT_ANGLES["hip_abduction"]
|
||||
q_des[i*3+1] = DEFAULT_JOINT_ANGLES["hip_pitch"]
|
||||
q_des[i*3+2] = DEFAULT_JOINT_ANGLES["knee"]
|
||||
kp[i*3:(i+1)*3] = [40.0, 40.0, 40.0]
|
||||
kd[i*3:(i+1)*3] = [3.0, 3.0, 3.0]
|
||||
|
||||
# Record foot position
|
||||
self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
|
||||
self._last_contact[leg] = True
|
||||
|
||||
# Wheel drive
|
||||
wheel_targets[i] = self._wheel_cmd(leg)
|
||||
else:
|
||||
# Swing: IK target position, strong PD, no feedforward
|
||||
if self._last_contact[leg]:
|
||||
self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
|
||||
self._swing_start_time[leg] = state.time
|
||||
self._last_contact[leg] = False
|
||||
|
||||
q_ik = self._swing_leg_ik(leg, state, q_pin)
|
||||
q_des[i*3:(i+1)*3] = q_ik
|
||||
kp[i*3:(i+1)*3] = [60.0, 60.0, 60.0] # strong PD for swing
|
||||
kd[i*3:(i+1)*3] = [3.0, 3.0, 3.0]
|
||||
# tau_ff stays 0 for swing
|
||||
|
||||
wheel_targets[i] = 0.0
|
||||
|
||||
# Use MIT protocol via robot interface
|
||||
self.robot.set_ctrl_mit(q_des, dq_des, kp, kd, tau_ff, wheel_targets)
|
||||
# Return dummy (actual ctrl is set directly above)
|
||||
return None, None
|
||||
|
||||
def _build_state_vector(self, state: RobotState):
|
||||
"""Build MPC state: [pos, rpy, vel, omega]."""
|
||||
return np.concatenate([state.pos, state.rpy, state.lin_vel, state.ang_vel])
|
||||
|
||||
def _build_reference(self, state: RobotState):
|
||||
"""Build reference trajectory over MPC horizon."""
|
||||
N = self.mpc.N
|
||||
x_ref = np.zeros((12, N))
|
||||
|
||||
yaw = state.rpy[2]
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
R_z = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
|
||||
vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
|
||||
|
||||
for k in range(N):
|
||||
t = (k + 1) * self.mpc.dt
|
||||
# Position: integrate from current
|
||||
x_ref[0, k] = state.pos[0] + vel_world[0] * t
|
||||
x_ref[1, k] = state.pos[1] + vel_world[1] * t
|
||||
x_ref[2, k] = self.height
|
||||
# RPY: keep roll/pitch zero, integrate yaw
|
||||
x_ref[3, k] = 0.0
|
||||
x_ref[4, k] = 0.0
|
||||
x_ref[5, k] = yaw + self._smooth_yaw * t
|
||||
# Velocity
|
||||
x_ref[6, k] = vel_world[0]
|
||||
x_ref[7, k] = vel_world[1]
|
||||
x_ref[8, k] = 0.0
|
||||
# Angular velocity
|
||||
x_ref[9, k] = 0.0
|
||||
x_ref[10, k] = 0.0
|
||||
x_ref[11, k] = self._smooth_yaw
|
||||
|
||||
return x_ref
|
||||
|
||||
def _build_contact_table(self):
|
||||
"""Build contact schedule over MPC horizon."""
|
||||
N = self.mpc.N
|
||||
table = np.zeros((4, N), dtype=int)
|
||||
for k in range(N):
|
||||
future_phase = (self._gait_phase + (k + 1) * self.mpc.dt * GAIT_FREQ) % 1.0
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
leg_phase = (future_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
table[i, k] = 1 if leg_phase < GAIT_DUTY else 0
|
||||
return table
|
||||
|
||||
def _swing_leg_ik(self, leg: str, state: RobotState, q_pin: np.ndarray):
|
||||
"""Swing leg: compute IK target joint angles for trajectory."""
|
||||
swing_phase = self._get_swing_phase(leg)
|
||||
|
||||
p_start = self._swing_start_foot[leg]
|
||||
p_end = self._compute_touchdown(leg, state)
|
||||
|
||||
s = swing_phase
|
||||
s_mj = 10*s**3 - 15*s**4 + 6*s**5
|
||||
|
||||
pos_des = p_start + (p_end - p_start) * s_mj
|
||||
# Z lift
|
||||
z_lift = 64.0 * s**3 * (1.0 - s)**3
|
||||
pos_des[2] = p_start[2] + SWING_HEIGHT * z_lift
|
||||
|
||||
# IK to get joint angles
|
||||
q_ik = self.dynamics.inverse_kinematics(leg, pos_des, q_pin)
|
||||
return q_ik
|
||||
|
||||
def _get_swing_phase(self, leg: str) -> float:
|
||||
phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
|
||||
if phase < GAIT_DUTY:
|
||||
return 0.0
|
||||
return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
|
||||
|
||||
def _compute_touchdown(self, leg: str, state: RobotState) -> np.ndarray:
|
||||
"""Raibert heuristic for touchdown position."""
|
||||
td = self._swing_start_foot[leg].copy()
|
||||
t_stance = GAIT_DUTY / GAIT_FREQ
|
||||
|
||||
yaw = state.rpy[2]
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
R_z = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
|
||||
cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
|
||||
|
||||
td[0] += cmd_vel_world[0] * t_stance * 0.5
|
||||
td[1] += cmd_vel_world[1] * t_stance * 0.5
|
||||
td[2] = WHEEL_RADIUS
|
||||
return td
|
||||
|
||||
def _wheel_cmd(self, leg: str) -> float:
|
||||
"""Differential drive for a single wheel."""
|
||||
if leg[1] == "l":
|
||||
v = (self._smooth_vx - 0.5 * WHEEL_TRACK * self._smooth_yaw) / WHEEL_RADIUS
|
||||
else:
|
||||
v = (self._smooth_vx + 0.5 * WHEEL_TRACK * self._smooth_yaw) / WHEEL_RADIUS
|
||||
return np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
|
||||
@@ -0,0 +1,367 @@
|
||||
"""Posture optimizer for wheeled-leg standing/crawl height table.
|
||||
|
||||
The table is not a pure "minimum average torque" table. For crawl and low-bar
|
||||
traversal, the wheel center should not be far from the hip/leg in sagittal X,
|
||||
otherwise the robot is no longer really using the wheel as the support/drive
|
||||
point. This is a soft guardrail, not a strict x=0 constraint. The score combines:
|
||||
|
||||
1. wheel center X offset from hip
|
||||
2. peak single-motor holding torque
|
||||
3. RMS torque, used as a proxy for I^2R heating
|
||||
|
||||
Usage:
|
||||
python posture_optimizer.py # MuJoCo sweep
|
||||
python posture_optimizer.py --quick # coarse MuJoCo sweep
|
||||
python posture_optimizer.py --analyze # analytical-only sweep
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
try:
|
||||
import mujoco
|
||||
except ImportError:
|
||||
mujoco = None
|
||||
|
||||
SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
|
||||
WHEEL_RADIUS = 0.10
|
||||
HIP_Z_OFFSET = 0.054
|
||||
L1, L2 = 0.25, 0.20
|
||||
ROBOT_MASS = 12.3
|
||||
G = 9.81
|
||||
F_PER_LEG = ROBOT_MASS * G / 4.0
|
||||
MAX_TORQUE = 17.0
|
||||
|
||||
LEG_NAMES = ("fl", "fr", "rl", "rr")
|
||||
LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
|
||||
|
||||
# 0.15m is not a good default table target for ab=0.0. 0.17m is the practical
|
||||
# default crawl height, while lower crawl can be evaluated with abduction.
|
||||
KNEE_MIN = -2.65
|
||||
HEIGHT_MIN = 0.17
|
||||
HEIGHT_MAX = 0.46
|
||||
SOFT_WHEEL_X_OFFSET = 0.05
|
||||
HARD_WHEEL_X_OFFSET = 0.08
|
||||
|
||||
_ACTUATOR_NAMES = [f"{leg}_{jt}" for leg in LEG_NAMES for jt in LEG_JOINTS]
|
||||
|
||||
|
||||
def compute_fk(hip, knee):
|
||||
"""Return wheel-center x offset and base height from (hip_pitch, knee)."""
|
||||
x = L1 * math.sin(hip) + L2 * math.sin(hip + knee)
|
||||
z = L1 * math.cos(hip) + L2 * math.cos(hip + knee)
|
||||
base_height = WHEEL_RADIUS + z - HIP_Z_OFFSET
|
||||
return x, base_height
|
||||
|
||||
|
||||
def posture_cost(x_foot, torques):
|
||||
"""Score one posture by support geometry, peak torque, and RMS torque."""
|
||||
tau = np.asarray(torques, dtype=float)
|
||||
peak_torque = float(np.max(np.abs(tau)))
|
||||
rms_torque = float(np.sqrt(np.mean(np.square(tau))))
|
||||
mean_i2r = float(np.mean(np.square(tau)))
|
||||
x_penalty = max(0.0, abs(x_foot) - SOFT_WHEEL_X_OFFSET)
|
||||
cost = (
|
||||
0.5 * (abs(x_foot) / SOFT_WHEEL_X_OFFSET) ** 2
|
||||
+ 8.0 * (x_penalty / max(1e-6, HARD_WHEEL_X_OFFSET - SOFT_WHEEL_X_OFFSET)) ** 2
|
||||
+ 3.0 * (peak_torque / MAX_TORQUE) ** 2
|
||||
+ (rms_torque / MAX_TORQUE) ** 2
|
||||
)
|
||||
return cost, peak_torque, rms_torque, mean_i2r
|
||||
|
||||
|
||||
def analyze_analytical():
|
||||
"""Analytical sweep using static GRF moments."""
|
||||
hip_range = np.arange(0.0, 1.6, 0.002)
|
||||
knee_range = np.arange(KNEE_MIN, -0.4, 0.002)
|
||||
|
||||
results = []
|
||||
for hip in hip_range:
|
||||
for knee in knee_range:
|
||||
x_foot, height = compute_fk(hip, knee)
|
||||
if height < HEIGHT_MIN or height > HEIGHT_MAX:
|
||||
continue
|
||||
|
||||
tau_hip = F_PER_LEG * x_foot
|
||||
x_knee_to_foot = L2 * math.sin(hip + knee)
|
||||
tau_knee = F_PER_LEG * x_knee_to_foot
|
||||
tau_abduction = 0.0
|
||||
cost, peak, rms, mean_i2r = posture_cost(
|
||||
x_foot, (tau_abduction, tau_hip, tau_knee))
|
||||
|
||||
results.append({
|
||||
"hip": float(hip),
|
||||
"knee": float(knee),
|
||||
"height": float(height),
|
||||
"x_foot": float(x_foot),
|
||||
"tau_abd": tau_abduction,
|
||||
"tau_hip": float(tau_hip),
|
||||
"tau_knee": float(tau_knee),
|
||||
"tau_peak": peak,
|
||||
"tau_rms": rms,
|
||||
"mean_i2r": mean_i2r,
|
||||
"cost": cost,
|
||||
})
|
||||
|
||||
return results
|
||||
|
||||
|
||||
def run_mujoco_sweep(quick=False):
|
||||
"""MuJoCo sweep measuring actual actuator forces at steady state."""
|
||||
if mujoco is None:
|
||||
raise RuntimeError("mujoco is not installed; use --analyze for analytical mode")
|
||||
|
||||
model = mujoco.MjModel.from_xml_path(str(SCENE_XML))
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
act_ids = {}
|
||||
for name in _ACTUATOR_NAMES:
|
||||
act_ids[name] = mujoco.mj_name2id(
|
||||
model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
|
||||
for i in range(model.nu):
|
||||
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
|
||||
model.actuator_biastype[i] = 1
|
||||
model.actuator_gaintype[i] = 0
|
||||
model.actuator_forcelimited[i] = 0
|
||||
if "wheel" not in name:
|
||||
model.actuator_gainprm[i, 0] = 60.0
|
||||
model.actuator_biasprm[i, 0] = 0.0
|
||||
model.actuator_biasprm[i, 1] = -60.0
|
||||
model.actuator_biasprm[i, 2] = -3.0
|
||||
model.actuator_ctrlrange[i] = [-3.14, 3.14]
|
||||
else:
|
||||
model.actuator_gainprm[i, 0] = 2.0
|
||||
model.actuator_biasprm[i, 0] = 0.0
|
||||
model.actuator_biasprm[i, 1] = 0.0
|
||||
model.actuator_biasprm[i, 2] = -2.0
|
||||
model.actuator_ctrlrange[i] = [-20.0, 20.0]
|
||||
|
||||
jnt_ids = {}
|
||||
for name in _ACTUATOR_NAMES:
|
||||
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||
jnt_ids[name] = model.jnt_qposadr[jid]
|
||||
|
||||
if quick:
|
||||
hip_range = np.arange(0.3, 1.5, 0.10)
|
||||
knee_range = np.arange(KNEE_MIN, -0.6, 0.10)
|
||||
else:
|
||||
hip_range = np.arange(0.0, 1.6, 0.04)
|
||||
knee_range = np.arange(KNEE_MIN, -0.4, 0.04)
|
||||
|
||||
results = []
|
||||
total = 0
|
||||
valid = 0
|
||||
|
||||
for hip in hip_range:
|
||||
for knee in knee_range:
|
||||
total += 1
|
||||
x_foot, height = compute_fk(hip, knee)
|
||||
if height < HEIGHT_MIN or height > HEIGHT_MAX:
|
||||
continue
|
||||
|
||||
mujoco.mj_resetData(model, data)
|
||||
|
||||
for leg in LEG_NAMES:
|
||||
for jt, val in zip(LEG_JOINTS, (0.0, hip, knee)):
|
||||
name = f"{leg}_{jt}"
|
||||
data.qpos[jnt_ids[name]] = val
|
||||
|
||||
data.qpos[2] = height
|
||||
data.qpos[3] = 1.0
|
||||
data.qpos[4:7] = 0.0
|
||||
|
||||
for leg in LEG_NAMES:
|
||||
for jt, val in zip(LEG_JOINTS, (0.0, hip, knee)):
|
||||
name = f"{leg}_{jt}"
|
||||
data.ctrl[act_ids[name]] = val
|
||||
name_w = f"{leg}_wheel_joint"
|
||||
if name_w in act_ids:
|
||||
data.ctrl[act_ids[name_w]] = 0.0
|
||||
|
||||
mujoco.mj_forward(model, data)
|
||||
|
||||
for _ in range(500):
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
roll, pitch, _ = _get_rpy(data, model)
|
||||
if abs(roll) > 0.8 or abs(pitch) > 0.8:
|
||||
continue
|
||||
|
||||
torque_buf = []
|
||||
for _ in range(100):
|
||||
mujoco.mj_step(model, data)
|
||||
torque_buf.append([data.actuator_force[act_ids[name]]
|
||||
for name in _ACTUATOR_NAMES])
|
||||
tau_avg = np.array(torque_buf).mean(axis=0)
|
||||
|
||||
tau_hip_val = tau_avg[1]
|
||||
tau_knee_val = tau_avg[2]
|
||||
tau_abd_val = tau_avg[0]
|
||||
cost, peak, rms, mean_i2r = posture_cost(x_foot, tau_avg)
|
||||
|
||||
q_hip_actual = float(data.qpos[jnt_ids["fl_hip_pitch_joint"]])
|
||||
q_knee_actual = float(data.qpos[jnt_ids["fl_knee_joint"]])
|
||||
|
||||
valid += 1
|
||||
results.append({
|
||||
"hip": float(hip),
|
||||
"knee": float(knee),
|
||||
"height": float(f"{height:.4f}"),
|
||||
"x_foot": float(f"{x_foot:.4f}"),
|
||||
"tau_abd": float(f"{tau_abd_val:.4f}"),
|
||||
"tau_hip": float(f"{tau_hip_val:.4f}"),
|
||||
"tau_knee": float(f"{tau_knee_val:.4f}"),
|
||||
"tau_peak": float(f"{peak:.4f}"),
|
||||
"tau_rms": float(f"{rms:.4f}"),
|
||||
"mean_i2r": float(f"{mean_i2r:.4f}"),
|
||||
"cost": float(f"{cost:.4f}"),
|
||||
"q_hip_actual": float(f"{q_hip_actual:.4f}"),
|
||||
"q_knee_actual": float(f"{q_knee_actual:.4f}"),
|
||||
})
|
||||
|
||||
if valid % 20 == 0:
|
||||
print(f" [{valid}/{total}] hip={hip:.2f} knee={knee:.2f} "
|
||||
f"h={height:.3f} x={x_foot:+.4f} "
|
||||
f"peak={peak:.2f} rms={rms:.2f} cost={cost:.4f}")
|
||||
|
||||
return results
|
||||
|
||||
|
||||
def _get_rpy(data, model):
|
||||
"""Extract roll and pitch from MuJoCo data."""
|
||||
base_bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
|
||||
rot = data.xmat[base_bid].reshape(3, 3)
|
||||
roll = math.atan2(rot[2, 1], rot[2, 2])
|
||||
pitch = math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2))
|
||||
return roll, pitch, 0.0
|
||||
|
||||
|
||||
def print_top_results(results, n=10):
|
||||
"""Print the top-N results with lowest cost."""
|
||||
sorted_r = sorted(results, key=lambda r: r["cost"])
|
||||
|
||||
print(f"\n{'=' * 104}")
|
||||
print(f"TOP {n} CONFIGURATIONS (soft wheel-X + peak/RMS torque score)")
|
||||
print(f"{'=' * 104}")
|
||||
print(f"{'Rank':>4} {'hip':>6} {'knee':>7} {'height':>7} {'x_foot':>8} "
|
||||
f"{'tau_abd':>8} {'tau_hip':>8} {'tau_knee':>8} "
|
||||
f"{'peak':>8} {'rms':>8} {'cost':>9}")
|
||||
print(f"{'-' * 104}")
|
||||
|
||||
for i, r in enumerate(sorted_r[:n]):
|
||||
print(f"{i + 1:>4} {r['hip']:>6.3f} {r['knee']:>7.3f} "
|
||||
f"{r['height']:>7.3f} {r.get('x_foot', 0):>8.4f} "
|
||||
f"{r.get('tau_abd', 0):>8.3f} {r['tau_hip']:>8.3f} "
|
||||
f"{r['tau_knee']:>8.3f} {r.get('tau_peak', 0):>8.3f} "
|
||||
f"{r.get('tau_rms', 0):>8.3f} {r['cost']:>9.4f}")
|
||||
|
||||
best = sorted_r[0]
|
||||
print(f"\nBEST: hip={best['hip']:.3f} knee={best['knee']:.3f} "
|
||||
f"z={best['height']:.3f}m x={best.get('x_foot', 0):+.4f}m "
|
||||
f"peak={best.get('tau_peak', 0):.3f}Nm "
|
||||
f"rms={best.get('tau_rms', 0):.3f}Nm cost={best['cost']:.4f}\n")
|
||||
|
||||
return sorted_r
|
||||
|
||||
|
||||
def compute_height_table(results):
|
||||
"""Build height-to-angle lookup with soft wheel-X support guardrail."""
|
||||
sorted_r = sorted(
|
||||
(r for r in results if abs(r.get("x_foot", 999.0)) <= HARD_WHEEL_X_OFFSET),
|
||||
key=lambda r: r["height"],
|
||||
)
|
||||
if not sorted_r:
|
||||
raise RuntimeError("No candidates satisfy HARD_WHEEL_X_OFFSET")
|
||||
|
||||
h_range = np.arange(0.17, 0.46, 0.02)
|
||||
table_h, table_hip, table_knee = [], [], []
|
||||
|
||||
for h_target in h_range:
|
||||
candidates = [(r, abs(r["height"] - h_target)) for r in sorted_r]
|
||||
candidates.sort(key=lambda x: (x[1], x[0]["cost"]))
|
||||
best = candidates[0][0]
|
||||
table_h.append(best["height"])
|
||||
table_hip.append(best["hip"])
|
||||
table_knee.append(best["knee"])
|
||||
|
||||
return {
|
||||
"height": [round(h, 3) for h in table_h],
|
||||
"hip": [round(h, 3) for h in table_hip],
|
||||
"knee": [round(k, 3) for k in table_knee],
|
||||
}
|
||||
|
||||
|
||||
def export_calibrated_table(table):
|
||||
"""Print the new height table in copy-paste format."""
|
||||
print(f"\n{'=' * 80}")
|
||||
print("CALIBRATED HEIGHT TABLE (soft wheel-X support guardrail)")
|
||||
print(f"{'=' * 80}")
|
||||
print(f"_H = {table['height']}")
|
||||
print(f"_HIP = {table['hip']}")
|
||||
print(f"_KNEE = {table['knee']}")
|
||||
print(f"{'=' * 80}\n")
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Find wheeled-leg posture table")
|
||||
parser.add_argument("--quick", action="store_true", help="Coarse MuJoCo sweep")
|
||||
parser.add_argument("--analyze", action="store_true", help="Analytical only")
|
||||
parser.add_argument("--mujoco", action="store_true", default=True,
|
||||
help="Run MuJoCo simulation when available")
|
||||
args = parser.parse_args()
|
||||
|
||||
print("=" * 72)
|
||||
print("WHEELED-LEG POSTURE OPTIMIZER")
|
||||
print("=" * 72)
|
||||
print(f"Robot mass: {ROBOT_MASS} kg, F_per_leg: {F_PER_LEG:.1f} N")
|
||||
print(f"L1={L1}m, L2={L2}m, wheel_r={WHEEL_RADIUS}m")
|
||||
print(f"Height range: [{HEIGHT_MIN}, {HEIGHT_MAX}] m")
|
||||
print(f"Knee min hard limit: {KNEE_MIN} rad")
|
||||
print(f"Soft wheel X offset: {SOFT_WHEEL_X_OFFSET} m")
|
||||
print(f"Hard wheel X offset: {HARD_WHEEL_X_OFFSET} m\n")
|
||||
|
||||
t0 = time.time()
|
||||
if args.analyze or mujoco is None:
|
||||
print("[Analytical mode]")
|
||||
results = analyze_analytical()
|
||||
else:
|
||||
print("[MuJoCo simulation mode]")
|
||||
results = run_mujoco_sweep(quick=args.quick)
|
||||
|
||||
elapsed = time.time() - t0
|
||||
print(f"Evaluated {len(results)} valid configurations in {elapsed:.1f}s")
|
||||
if not results:
|
||||
print("No valid configurations found")
|
||||
return
|
||||
|
||||
best_results = print_top_results(results, n=15)
|
||||
|
||||
x_def, h_def = compute_fk(0.666, -1.546)
|
||||
print(f"Current config default: hip=0.666, knee=-1.546 "
|
||||
f"=> z={h_def:.3f}m, x={x_def:+.4f}m")
|
||||
|
||||
table = compute_height_table(best_results)
|
||||
export_calibrated_table(table)
|
||||
|
||||
best = best_results[0]
|
||||
print("=" * 72)
|
||||
print("RECOMMENDED DEFAULT")
|
||||
print("=" * 72)
|
||||
print(f"hip_abduction: 0.0")
|
||||
print(f"hip_pitch: {best['hip']:.3f}")
|
||||
print(f"knee: {best['knee']:.3f}")
|
||||
print(f"height: {best['height']:.3f} m")
|
||||
print(f"x_foot: {best.get('x_foot', 0):+.4f} m")
|
||||
print(f"tau_peak: {best.get('tau_peak', 0):.3f} Nm")
|
||||
print(f"tau_rms: {best.get('tau_rms', 0):.3f} Nm")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,189 @@
|
||||
"""Fast posture table helper based on wheelleg.xml link offsets.
|
||||
|
||||
This tool is useful because it uses the real FL leg offsets from MJCF instead
|
||||
of the simplified two-link geometry used by posture_optimizer.py. It is still a
|
||||
static single-leg approximation, so use it to choose candidate crawl/standing
|
||||
poses, then verify in MuJoCo and on the robot at low speed.
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
F_PER_LEG = 12.3 * 9.81 / 4.0
|
||||
KNEE_MIN = -2.65
|
||||
HIP_MIN = -2.58
|
||||
HIP_MAX = 2.58
|
||||
AB_MIN = -0.436
|
||||
AB_MAX = 0.611
|
||||
HARD_WHEEL_X_OFFSET = 0.08
|
||||
|
||||
|
||||
def rx(a):
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
return ((1, 0, 0), (0, c, -s), (0, s, c))
|
||||
|
||||
|
||||
def ry(a):
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
return ((c, 0, s), (0, 1, 0), (-s, 0, c))
|
||||
|
||||
|
||||
def mv(m, v):
|
||||
return [
|
||||
m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2],
|
||||
m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2],
|
||||
m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2],
|
||||
]
|
||||
|
||||
|
||||
def add(a, b):
|
||||
return [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
|
||||
|
||||
|
||||
def get_posture(q_ab, q_hip, q_knee):
|
||||
"""Return static FK/torque metrics for one FL leg.
|
||||
|
||||
Returns:
|
||||
base_z, tau_ab, tau_hip, tau_knee, i2r_total, knee_z, wheel_z, wheel_x_from_hip
|
||||
"""
|
||||
# FL offsets from mjcf/wheelleg.xml.
|
||||
t_ab = [0.32826, 0.066172, 0.053981]
|
||||
t_hip = [0.06389, -0.027344, 0.00010727]
|
||||
t_knee = [0.0, 0.1035, -0.25]
|
||||
t_wheel = [0.0, 0.014699, -0.20011]
|
||||
|
||||
p = mv(ry(q_knee), t_wheel)
|
||||
p = add(p, t_knee)
|
||||
p = mv(ry(q_hip), p)
|
||||
p = add(p, t_hip)
|
||||
p = mv(rx(q_ab), p)
|
||||
p = add(p, t_ab)
|
||||
|
||||
base_z = 0.10 - p[2]
|
||||
wheel_z = p[2]
|
||||
|
||||
knee_pos = mv(ry(q_hip), t_knee)
|
||||
knee_pos = mv(rx(q_ab), knee_pos)
|
||||
knee_z = knee_pos[2] + t_ab[2]
|
||||
|
||||
joint_knee = mv(ry(q_hip), add(t_knee, t_hip))
|
||||
joint_knee = mv(rx(q_ab), joint_knee)
|
||||
joint_knee = add(t_ab, joint_knee)
|
||||
|
||||
joint_hip = mv(rx(q_ab), t_hip)
|
||||
joint_hip = add(t_ab, joint_hip)
|
||||
|
||||
r_knee = [p[0] - joint_knee[0], p[1] - joint_knee[1], p[2] - joint_knee[2]]
|
||||
r_hip = [p[0] - joint_hip[0], p[1] - joint_hip[1], p[2] - joint_hip[2]]
|
||||
r_ab = [p[0] - t_ab[0], p[1] - t_ab[1], p[2] - t_ab[2]]
|
||||
|
||||
tau_ab = r_ab[1] * F_PER_LEG
|
||||
tau_hip = -r_hip[0] * F_PER_LEG
|
||||
tau_knee = -r_knee[0] * F_PER_LEG
|
||||
i2r_total = tau_ab * tau_ab + tau_hip * tau_hip + tau_knee * tau_knee
|
||||
wheel_x_from_hip = p[0] - joint_hip[0]
|
||||
return base_z, tau_ab, tau_hip, tau_knee, i2r_total, knee_z, wheel_z, wheel_x_from_hip
|
||||
|
||||
|
||||
def score_candidate(z, z_target, tau_ab, tau_hip, tau_knee, wheel_x, x_target=0.0):
|
||||
peak = max(abs(tau_ab), abs(tau_hip), abs(tau_knee))
|
||||
rms = math.sqrt((tau_ab * tau_ab + tau_hip * tau_hip + tau_knee * tau_knee) / 3.0)
|
||||
x_err = wheel_x - x_target
|
||||
x_over = max(0.0, abs(wheel_x) - 0.05)
|
||||
return (
|
||||
3000.0 * (z - z_target) ** 2
|
||||
+ 2.5 * (peak / 17.0) ** 2
|
||||
+ (rms / 17.0) ** 2
|
||||
+ 0.4 * (x_err / 0.05) ** 2
|
||||
+ 6.0 * (x_over / 0.03) ** 2
|
||||
), peak, rms
|
||||
|
||||
|
||||
def find_best(z_target, ab_range=(0.0, 0.0), step=0.002, x_target=0.0, hard_wheel_x_offset=HARD_WHEEL_X_OFFSET):
|
||||
"""Find one static posture near target height without violating hard limits."""
|
||||
best = None
|
||||
ab0, ab1 = ab_range
|
||||
n_ab = max(1, int(round((ab1 - ab0) / step)) + 1)
|
||||
n_hip = int(round((1.6 - 0.0) / step)) + 1
|
||||
n_knee = int(round((-0.4 - KNEE_MIN) / step)) + 1
|
||||
|
||||
for ia in range(n_ab):
|
||||
ab = ab0 + ia * step
|
||||
if ab < AB_MIN or ab > AB_MAX:
|
||||
continue
|
||||
for ih in range(n_hip):
|
||||
hip = ih * step
|
||||
if hip < HIP_MIN or hip > HIP_MAX:
|
||||
continue
|
||||
for ik in range(n_knee):
|
||||
knee = KNEE_MIN + ik * step
|
||||
z, ta, th, tk, i2r, kz, wz, wx = get_posture(ab, hip, knee)
|
||||
if abs(z - z_target) > 0.0015:
|
||||
continue
|
||||
if abs(wx) > hard_wheel_x_offset:
|
||||
continue
|
||||
if wz >= kz:
|
||||
continue
|
||||
cost, peak, rms = score_candidate(z, z_target, ta, th, tk, wx, x_target=x_target)
|
||||
cand = (cost, ab, hip, knee, z, ta, th, tk, peak, rms, i2r, wx)
|
||||
if best is None or cand[0] < best[0]:
|
||||
best = cand
|
||||
return best
|
||||
|
||||
|
||||
def print_table(
|
||||
z_targets,
|
||||
name,
|
||||
ab_range=(0.0, 0.0),
|
||||
step=0.002,
|
||||
x_target=0.0,
|
||||
hard_wheel_x_offset=HARD_WHEEL_X_OFFSET,
|
||||
):
|
||||
print(f"\n{'=' * 96}")
|
||||
print(name)
|
||||
print(f"{'=' * 96}")
|
||||
print(f"{'z':>6} {'ab':>6} {'hip':>7} {'knee':>7} "
|
||||
f"{'tau_ab':>8} {'tau_hip':>8} {'tau_knee':>9} "
|
||||
f"{'peak':>8} {'rms':>8} {'x_hip':>8}")
|
||||
print("-" * 96)
|
||||
for zt in z_targets:
|
||||
best = find_best(
|
||||
zt,
|
||||
ab_range=ab_range,
|
||||
step=step,
|
||||
x_target=x_target,
|
||||
hard_wheel_x_offset=hard_wheel_x_offset,
|
||||
)
|
||||
if best is None:
|
||||
print(f"{zt:>6.3f} no valid config")
|
||||
continue
|
||||
_, ab, hip, knee, z, ta, th, tk, peak, rms, _, wx = best
|
||||
print(f"{z:>6.3f} {ab:>6.3f} {hip:>7.3f} {knee:>7.3f} "
|
||||
f"{ta:>8.3f} {th:>8.3f} {tk:>9.3f} "
|
||||
f"{peak:>8.3f} {rms:>8.3f} {wx:>8.4f}")
|
||||
|
||||
|
||||
def print_crawl_default():
|
||||
best = find_best(0.17, ab_range=(0.0, 0.0), step=0.002)
|
||||
if best is None:
|
||||
return
|
||||
_, ab, hip, knee, z, ta, th, tk, peak, rms, _, wx = best
|
||||
print("\nSuggested runtime crawl_default_dof_pos:")
|
||||
print(
|
||||
f"[{ab:.3f}, {hip:.3f}, {knee:.3f}, "
|
||||
f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
|
||||
f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
|
||||
f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
|
||||
"0.0, 0.0, 0.0, 0.0]"
|
||||
)
|
||||
print(f"# z={z:.3f}, peak={peak:.3f}Nm, rms={rms:.3f}Nm, wheel_x_from_hip={wx:+.4f}m")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print_table([round(0.36 + 0.01 * i, 2) for i in range(10)],
|
||||
"STANDING candidates from MJCF FL geometry", step=0.004)
|
||||
print_table([round(0.17 + 0.01 * i, 2) for i in range(9)],
|
||||
"CRAWL candidates from MJCF FL geometry", step=0.002)
|
||||
print_table([round(0.10 + 0.01 * i, 2) for i in range(6)],
|
||||
"LOW CRAWL candidates, fixed FL abduction = +0.2",
|
||||
ab_range=(0.2, 0.2), step=0.002, hard_wheel_x_offset=0.45)
|
||||
print_crawl_default()
|
||||
@@ -0,0 +1,200 @@
|
||||
"""RlFriendlyPostureOpt — 结合电机发热 + RL友好度约束的站/爬姿优化
|
||||
|
||||
RL友好度约束（从实机经验总结，静力学可计算）：
|
||||
1. 髋关节力臂 ≥ 0.08m — 不让髋闲置（动态响应差）
|
||||
2. 三电机不均衡 ≤ 1.3x — 不让单电机先超载
|
||||
3. 运动学条件数 κ ≤ 3.5 — 不让有效传动比过高（放大控制噪声）
|
||||
|
||||
使用方法：
|
||||
uv run python mujoco_sim/rl_friendly_opt.py # 打印推荐
|
||||
uv run python -c "from mujoco_sim.rl_friendly_opt import get_all; print(get_all(0, 0.8, -1.22))"
|
||||
|
||||
基于 mjcf/wheelleg.xml 的 FL 腿运动学。
|
||||
"""
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 运动学常数（来自 wheelleg.xml FL 腿）
|
||||
# ---------------------------------------------------------------------------
|
||||
T_AB = [0.32826, 0.066172, 0.053981]
|
||||
T_HIP = [0.06389, -0.027344, 0.00010727]
|
||||
T_KNEE = [0.0, 0.1035, -0.25]
|
||||
T_WHEEL = [0.0, 0.014699, -0.20011]
|
||||
F_PER_LEG = 12.3 * 9.81 / 4.0
|
||||
KNEE_MIN, KNEE_MAX = -2.65, 2.65
|
||||
HIP_MIN, HIP_MAX = -2.58, 2.58
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 工具函数
|
||||
# ---------------------------------------------------------------------------
|
||||
def rx(a):
|
||||
c = math.cos(a); s = math.sin(a)
|
||||
return ((1, 0, 0), (0, c, -s), (0, s, c))
|
||||
|
||||
def ry(a):
|
||||
c = math.cos(a); s = math.sin(a)
|
||||
return ((c, 0, s), (0, 1, 0), (-s, 0, c))
|
||||
|
||||
def mv(m, v):
|
||||
return [m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2],
|
||||
m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2],
|
||||
m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]]
|
||||
|
||||
def add(a, b): return [a[0]+b[0], a[1]+b[1], a[2]+b[2]]
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 主函数
|
||||
# ---------------------------------------------------------------------------
|
||||
def get_all(q_ab, q_hip, q_knee):
|
||||
"""FK + 力矩 + 运动学指标。
|
||||
|
||||
返回 dict:
|
||||
z, ab, hip, knee, tau_ab, tau_hip, tau_knee,
|
||||
max_tau, i2r, imbal, cond, r_hip_x_mag, calf_deg
|
||||
"""
|
||||
# --- FK ---
|
||||
p = mv(ry(q_knee), T_WHEEL)
|
||||
p = add(p, T_KNEE)
|
||||
p = mv(ry(q_hip), p)
|
||||
p = add(p, T_HIP)
|
||||
p = mv(rx(q_ab), p)
|
||||
wh = add(p, T_AB)
|
||||
base_z = 0.10 - wh[2]
|
||||
wheel_z = wh[2]
|
||||
|
||||
# 膝位置（用于 wheel-below-knee）
|
||||
pk = mv(ry(q_hip), T_KNEE)
|
||||
pk = mv(rx(q_ab), pk)
|
||||
kz = pk[2] + T_AB[2]
|
||||
|
||||
# 关节位置
|
||||
jk = mv(ry(q_hip), add(T_KNEE, T_HIP))
|
||||
jk = mv(rx(q_ab), jk)
|
||||
jk = add(T_AB, jk)
|
||||
jh = mv(rx(q_ab), T_HIP)
|
||||
jh = add(T_AB, jh)
|
||||
|
||||
# 力矩
|
||||
rk = [wh[0]-jk[0], wh[1]-jk[1], wh[2]-jk[2]]
|
||||
rh = [wh[0]-jh[0], wh[1]-jh[1], wh[2]-jh[2]]
|
||||
ra = [wh[0]-T_AB[0], wh[1]-T_AB[1], wh[2]-T_AB[2]]
|
||||
tau_ab = ra[1] * F_PER_LEG
|
||||
tau_hip = -rh[0] * F_PER_LEG
|
||||
tau_knee = -rk[0] * F_PER_LEG
|
||||
|
||||
# --- Jacobian（有限差分） ---
|
||||
eps = 1e-6
|
||||
def foot_rel_hip(h, k):
|
||||
fp = mv(ry(k), T_WHEEL)
|
||||
fp = add(fp, T_KNEE)
|
||||
fp = mv(ry(h), fp)
|
||||
return [fp[0] + T_AB[0] - jh[0], fp[2] + T_AB[2] - jh[2]]
|
||||
fp0 = foot_rel_hip(q_hip, q_knee)
|
||||
fph = foot_rel_hip(q_hip + eps, q_knee)
|
||||
fpk = foot_rel_hip(q_hip, q_knee + eps)
|
||||
J = np.array([
|
||||
[(fph[0]-fp0[0])/eps, (fpk[0]-fp0[0])/eps],
|
||||
[(fph[1]-fp0[1])/eps, (fpk[1]-fp0[1])/eps],
|
||||
])
|
||||
s = np.linalg.svd(J, compute_uv=False)
|
||||
cond = s[0] / s[-1] if s[-1] > 1e-10 else 999.0
|
||||
min_sv = s[-1]
|
||||
|
||||
# --- 小腿角度（相对铅垂线） ---
|
||||
calf_x = (-0.20011) * math.sin(q_knee)
|
||||
calf_z = (-0.20011) * math.cos(q_knee)
|
||||
cv_x = calf_x * math.cos(q_hip) + calf_z * math.sin(q_hip)
|
||||
cv_z = -calf_x * math.sin(q_hip) + calf_z * math.cos(q_hip)
|
||||
calf_deg = math.degrees(math.atan2(cv_x, -cv_z))
|
||||
|
||||
abs_taus = [abs(tau_ab), abs(tau_hip), abs(tau_knee)]
|
||||
return {
|
||||
'z': base_z, 'ab': q_ab, 'hip': q_hip, 'knee': q_knee,
|
||||
'tau_ab': tau_ab, 'tau_hip': tau_hip, 'tau_knee': tau_knee,
|
||||
'max_tau': max(abs_taus),
|
||||
'i2r': tau_ab**2 + tau_hip**2 + tau_knee**2,
|
||||
'imbal': max(abs_taus) / max(1e-10, min(abs_taus)),
|
||||
'cond': cond, 'min_sv': min_sv,
|
||||
'r_hip_x_mag': abs(rh[0]),
|
||||
'calf_deg': calf_deg,
|
||||
'kz': kz, 'wz': wheel_z,
|
||||
}
|
||||
|
||||
|
||||
def rl_cost(r):
|
||||
"""RL友好度综合成本（越小越好）。
|
||||
|
||||
约束来源：
|
||||
c1 — 瓶颈电机发热 τ²/τ_max² 主目标
|
||||
c2 — 电机不均衡 > 1.3x (imbal-1.3)² 单电机先超载
|
||||
c3 — 髋力臂 < 8cm (0.08 - r_hip) 髋闲置→动态响应差
|
||||
c4 — 有效传动比 κ > 3.5 (κ - 3.5) 高刚度→冲击传递大
|
||||
c5 — 腿的被动刚度 > 1.3x (stiff-1.3) 刚度比→冲击吸收（新！）
|
||||
"""
|
||||
c1 = (r['max_tau'] / 17.0) ** 2
|
||||
c2 = max(0.0, (r['imbal'] - 1.3) / 1.0) ** 2
|
||||
c3 = max(0.0, (0.08 - r['r_hip_x_mag'])) / 0.08
|
||||
c4 = max(0.0, (r['cond'] - 3.5)) / 5.0
|
||||
# stiffness ratio normalized to z=0.40 (σ_min≈0.115)
|
||||
stiff = (0.115 / r['min_sv']) ** 2
|
||||
c5 = max(0.0, (stiff - 1.3)) / 3.0
|
||||
return 100.0*c1 + 50.0*c2 + 80.0*c3 + 30.0*c4 + 40.0*c5
|
||||
|
||||
|
||||
def sweep_z(z_targets, name, ab_max=0.44, tol=0.004):
|
||||
"""遍历 z 扫描最优姿态。"""
|
||||
print(f"\n{'='*100}")
|
||||
print(f" {name}")
|
||||
print(f"{'='*100}")
|
||||
print(f"{'z_tgt':>6} {'z':>6} {'ab':>5} {'hip':>6} {'knee':>6} | "
|
||||
f"{'maxτ':>6} {'imbal':>6} {'r_hip':>6} {'κ':>5} | "
|
||||
f"{'c1热':>6} {'c2均':>6} {'c3髋':>6} {'c4奇':>6} {'cost':>7}")
|
||||
print("-"*100)
|
||||
results = []
|
||||
for zt in z_targets:
|
||||
best_cost = float('inf')
|
||||
best_r = None
|
||||
for ab in [round(i*0.02, 2) for i in range(int(ab_max/0.02)+1)]:
|
||||
for h in [round(i*0.01, 2) for i in range(260)]:
|
||||
for kn in [round(-2.65+i*0.01, 2) for i in range(256)]:
|
||||
if h + kn > -0.1: continue
|
||||
r = get_all(ab, h, kn)
|
||||
if abs(r['z'] - zt) > tol: continue
|
||||
if r['wz'] >= r['kz']: continue
|
||||
if r['tau_ab'] < 0 or r['tau_hip'] < 0: continue
|
||||
cost = rl_cost(r)
|
||||
if cost < best_cost:
|
||||
best_cost = cost; best_r = r
|
||||
if best_r:
|
||||
r = best_r
|
||||
c1s = 100.0*(r['max_tau']/17.0)**2
|
||||
c2s = 50.0*max(0.0,(r['imbal']-1.3))**2
|
||||
c3s = 80.0*max(0.0,(0.08-r['r_hip_x_mag']))/0.08
|
||||
c4s = 30.0*max(0.0,(r['cond']-3.5))/5.0
|
||||
print(f"{zt:>6.2f} {r['z']:>6.3f} {r['ab']:>5.2f} {r['hip']:>6.2f} {r['knee']:>6.2f} | "
|
||||
f"{r['max_tau']:>6.3f} {r['imbal']:>6.1f}x {r['r_hip_x_mag']:>6.3f} {r['cond']:>5.1f} | "
|
||||
f"{c1s:>6.1f} {c2s:>6.1f} {c3s:>6.1f} {c4s:>6.1f} {best_cost:>7.1f}")
|
||||
results.append((zt, r))
|
||||
else:
|
||||
print(f"{zt:>6.2f} — no valid")
|
||||
return results
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
stand = sweep_z([round(0.36+0.01*i, 2) for i in range(10)],
|
||||
"STANDING — heat + RL constraints (ab free)", 0.44)
|
||||
crawl = sweep_z([round(0.10+0.01*i, 2) for i in range(9)],
|
||||
"CRAWL — heat + RL constraints (ab_max=0.25)", 0.25)
|
||||
|
||||
print(f"\n{'='*100}")
|
||||
print(" RECOMMENDATION")
|
||||
print(f"{'='*100}")
|
||||
if stand:
|
||||
zt, r = min(stand, key=lambda x: rl_cost(x[1]))
|
||||
print(f" Standing: z={r['z']:.3f} ab={r['ab']:.2f} hip={r['hip']:.2f} knee={r['knee']:.2f}")
|
||||
print(f" maxτ={r['max_tau']:.3f} imbal={r['imbal']:.1f}x κ={r['cond']:.1f} r_hip={r['r_hip_x_mag']:.3f}m")
|
||||
if crawl:
|
||||
zt, r = min(crawl, key=lambda x: rl_cost(x[1]))
|
||||
print(f" Crawl: z={r['z']:.3f} ab={r['ab']:.2f} hip={r['hip']:.2f} knee={r['knee']:.2f}")
|
||||
print(f" maxτ={r['max_tau']:.3f} imbal={r['imbal']:.1f}x κ={r['cond']:.1f} r_hip={r['r_hip_x_mag']:.3f}m")
|
||||
@@ -0,0 +1,231 @@
|
||||
"""MuJoCo interface for the wheeled-legged robot.
|
||||
|
||||
Configures actuators as proper PD controllers at runtime:
|
||||
- Leg joints: force = kp*(ctrl - qpos) - kd*qvel, ctrl = target angle
|
||||
- Wheel joints: force = gain*(ctrl - qvel), ctrl = target velocity (rad/s)
|
||||
"""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import mujoco
|
||||
from dataclasses import dataclass
|
||||
from config import (SCENE_XML, LEG_NAMES, LEG_JOINTS, WHEEL_JOINT,
|
||||
DEFAULT_JOINT_ANGLES, WHEEL_RADIUS, WHEEL_TRACK,
|
||||
L_THIGH, L_CALF)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RobotState:
|
||||
"""Robot state from MuJoCo."""
|
||||
pos: np.ndarray # (3,) world position
|
||||
quat: np.ndarray # (4,) quaternion (w,x,y,z) MuJoCo convention
|
||||
rot: np.ndarray # (3,3) body→world rotation
|
||||
rpy: np.ndarray # (3,) roll, pitch, yaw
|
||||
lin_vel: np.ndarray # (3,) world frame linear velocity
|
||||
ang_vel: np.ndarray # (3,) body frame angular velocity
|
||||
joint_pos: np.ndarray # (16,) all joint positions [fl3+wheel, fr3+wheel, rl3+wheel, rr3+wheel]
|
||||
joint_vel: np.ndarray # (16,) all joint velocities
|
||||
time: float
|
||||
|
||||
|
||||
class Robot:
|
||||
"""MuJoCo simulation interface with proper PD actuator configuration."""
|
||||
|
||||
# Leg PD gains (tuned for 12.3kg robot)
|
||||
LEG_KP = 60.0
|
||||
LEG_KD = 3.0
|
||||
# Wheel velocity gain
|
||||
WHEEL_KP = 2.0
|
||||
|
||||
def __init__(self, xml_path=None):
|
||||
self.model = mujoco.MjModel.from_xml_path(str(xml_path or SCENE_XML))
|
||||
self.data = mujoco.MjData(self.model)
|
||||
|
||||
# Cache IDs
|
||||
self._base_bid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
|
||||
self._actuator_ids = {} # name → actuator index
|
||||
self._joint_qpos_adr = {} # name → qpos address
|
||||
self._joint_qvel_adr = {} # name → qvel address
|
||||
|
||||
# Build joint/actuator maps
|
||||
self._ctrl_order = []
|
||||
for leg in LEG_NAMES:
|
||||
for jt in (*LEG_JOINTS, WHEEL_JOINT):
|
||||
name = f"{leg}_{jt}"
|
||||
aid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||
self._actuator_ids[name] = aid
|
||||
self._joint_qpos_adr[name] = self.model.jnt_qposadr[jid]
|
||||
self._joint_qvel_adr[name] = self.model.jnt_dofadr[jid]
|
||||
self._ctrl_order.append(name)
|
||||
|
||||
# Configure actuators as proper PD controllers
|
||||
self._configure_actuators()
|
||||
|
||||
def _configure_actuators(self):
|
||||
"""Set actuators to proper PD mode.
|
||||
|
||||
Leg joints: force = kp*(ctrl - qpos) - kd*qvel
|
||||
Wheels: force = gain*(ctrl - qvel) (velocity tracking)
|
||||
"""
|
||||
for i in range(self.model.nu):
|
||||
name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
|
||||
self.model.actuator_biastype[i] = 1 # affine bias
|
||||
self.model.actuator_gaintype[i] = 0 # fixed gain
|
||||
self.model.actuator_forcelimited[i] = 0 # no force clamp (17Nm is in actuatorfrcrange)
|
||||
|
||||
if 'wheel' not in name:
|
||||
self.model.actuator_gainprm[i, 0] = self.LEG_KP
|
||||
self.model.actuator_biasprm[i, 0] = 0.0
|
||||
self.model.actuator_biasprm[i, 1] = -self.LEG_KP
|
||||
self.model.actuator_biasprm[i, 2] = -self.LEG_KD
|
||||
self.model.actuator_ctrlrange[i] = [-3.14, 3.14]
|
||||
else:
|
||||
self.model.actuator_gainprm[i, 0] = self.WHEEL_KP
|
||||
self.model.actuator_biasprm[i, 0] = 0.0
|
||||
self.model.actuator_biasprm[i, 1] = 0.0
|
||||
self.model.actuator_biasprm[i, 2] = -self.WHEEL_KP
|
||||
self.model.actuator_ctrlrange[i] = [-20.0, 20.0]
|
||||
|
||||
@property
|
||||
def dt(self):
|
||||
return self.model.opt.timestep
|
||||
|
||||
def reset(self):
|
||||
"""Reset to standing pose at correct height for default joint angles."""
|
||||
mujoco.mj_resetData(self.model, self.data)
|
||||
|
||||
# Set default leg angles
|
||||
for leg in LEG_NAMES:
|
||||
for jt, key in zip(LEG_JOINTS, ("hip_abduction", "hip_pitch", "knee")):
|
||||
name = f"{leg}_{jt}"
|
||||
adr = self._joint_qpos_adr[name]
|
||||
self.data.qpos[adr] = DEFAULT_JOINT_ANGLES[key]
|
||||
|
||||
# Compute correct base height from default angles using exact FK
|
||||
# z_base = wheel_radius + L1*cos(hip) + L2*cos(hip+knee) - hip_z_offset
|
||||
hip = DEFAULT_JOINT_ANGLES["hip_pitch"]
|
||||
knee = DEFAULT_JOINT_ANGLES["knee"]
|
||||
base_z = (WHEEL_RADIUS
|
||||
+ L_THIGH * math.cos(hip)
|
||||
+ L_CALF * math.cos(hip + knee)
|
||||
- 0.054)
|
||||
self.data.qpos[2] = base_z
|
||||
self.data.qpos[3] = 1.0 # quat w
|
||||
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
# Set ctrl to match initial pose (so PD doesn't jerk)
|
||||
for leg in LEG_NAMES:
|
||||
for jt, key in zip(LEG_JOINTS, ("hip_abduction", "hip_pitch", "knee")):
|
||||
name = f"{leg}_{jt}"
|
||||
self.data.ctrl[self._actuator_ids[name]] = DEFAULT_JOINT_ANGLES[key]
|
||||
# Wheels: zero velocity
|
||||
self.data.ctrl[self._actuator_ids[f"{leg}_{WHEEL_JOINT}"]] = 0.0
|
||||
|
||||
def get_state(self) -> RobotState:
|
||||
"""Extract robot state."""
|
||||
pos = self.data.xpos[self._base_bid].copy()
|
||||
quat = self.data.xquat[self._base_bid].copy() # (w,x,y,z)
|
||||
rot = self.data.xmat[self._base_bid].reshape(3, 3).copy()
|
||||
|
||||
rpy = np.array([
|
||||
np.arctan2(rot[2, 1], rot[2, 2]),
|
||||
np.arctan2(-rot[2, 0], np.sqrt(rot[2, 1]**2 + rot[2, 2]**2)),
|
||||
np.arctan2(rot[1, 0], rot[0, 0]),
|
||||
])
|
||||
|
||||
# Base velocity (world frame)
|
||||
lin_vel = self.data.qvel[0:3].copy()
|
||||
ang_vel = self.data.qvel[3:6].copy()
|
||||
|
||||
# Joint states (16 joints: 4 legs × 4 joints each)
|
||||
joint_pos = np.zeros(16)
|
||||
joint_vel = np.zeros(16)
|
||||
for i, name in enumerate(self._ctrl_order):
|
||||
joint_pos[i] = self.data.qpos[self._joint_qpos_adr[name]]
|
||||
joint_vel[i] = self.data.qvel[self._joint_qvel_adr[name]]
|
||||
|
||||
return RobotState(
|
||||
pos=pos, quat=quat, rot=rot, rpy=rpy,
|
||||
lin_vel=lin_vel, ang_vel=ang_vel,
|
||||
joint_pos=joint_pos, joint_vel=joint_vel,
|
||||
time=self.data.time,
|
||||
)
|
||||
|
||||
def set_ctrl(self, leg_targets: np.ndarray, wheel_targets: np.ndarray):
|
||||
"""Set actuator commands (position PD mode).
|
||||
|
||||
Args:
|
||||
leg_targets: (12,) target joint angles for legs [fl3, fr3, rl3, rr3]
|
||||
wheel_targets: (4,) target wheel velocities [fl, fr, rl, rr] in rad/s
|
||||
"""
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
for j, jt in enumerate(LEG_JOINTS):
|
||||
name = f"{leg}_{jt}"
|
||||
self.data.ctrl[self._actuator_ids[name]] = leg_targets[i * 3 + j]
|
||||
name = f"{leg}_{WHEEL_JOINT}"
|
||||
self.data.ctrl[self._actuator_ids[name]] = wheel_targets[i]
|
||||
|
||||
def set_ctrl_mit(self, q_des: np.ndarray, dq_des: np.ndarray,
|
||||
kp: np.ndarray, kd: np.ndarray, tau_ff: np.ndarray,
|
||||
wheel_targets: np.ndarray):
|
||||
"""MIT motor protocol: tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff.
|
||||
|
||||
Computes torque in software, sends to actuators in torque mode.
|
||||
Call enable_torque_mode() first.
|
||||
|
||||
Args:
|
||||
q_des: (12,) desired joint angles
|
||||
dq_des: (12,) desired joint velocities
|
||||
kp: (12,) position gains (0 for pure torque)
|
||||
kd: (12,) velocity gains
|
||||
tau_ff: (12,) feedforward torques
|
||||
wheel_targets: (4,) wheel velocity targets
|
||||
"""
|
||||
for i, leg in enumerate(LEG_NAMES):
|
||||
for j, jt in enumerate(LEG_JOINTS):
|
||||
name = f"{leg}_{jt}"
|
||||
aid = self._actuator_ids[name]
|
||||
idx = i * 3 + j
|
||||
q = self.data.qpos[self._joint_qpos_adr[name]]
|
||||
dq = self.data.qvel[self._joint_qvel_adr[name]]
|
||||
tau = (kp[idx] * (q_des[idx] - q)
|
||||
+ kd[idx] * (dq_des[idx] - dq)
|
||||
+ tau_ff[idx])
|
||||
self.data.ctrl[aid] = np.clip(tau, -17.0, 17.0)
|
||||
name = f"{leg}_{WHEEL_JOINT}"
|
||||
self.data.ctrl[self._actuator_ids[name]] = wheel_targets[i]
|
||||
|
||||
def enable_torque_mode(self):
|
||||
"""Switch leg actuators to direct torque mode (for MPC/MIT)."""
|
||||
for i in range(self.model.nu):
|
||||
name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
|
||||
if 'wheel' not in name:
|
||||
self.model.actuator_gainprm[i, 0] = 1.0
|
||||
self.model.actuator_biasprm[i, :3] = [0, 0, 0]
|
||||
self.model.actuator_biastype[i] = 0
|
||||
self.model.actuator_ctrlrange[i] = [-17.0, 17.0]
|
||||
|
||||
def enable_position_mode(self):
|
||||
"""Switch leg actuators back to position PD mode."""
|
||||
self._configure_actuators()
|
||||
|
||||
def step(self):
|
||||
"""Advance one simulation timestep."""
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
def get_qpos_qvel_for_pinocchio(self):
|
||||
"""Get full qpos/qvel for Pinocchio (reorder quaternion)."""
|
||||
qpos = self.data.qpos.copy()
|
||||
qvel = self.data.qvel.copy()
|
||||
# MuJoCo quat: (w,x,y,z) → Pinocchio: (x,y,z,w)
|
||||
w, x, y, z = qpos[3], qpos[4], qpos[5], qpos[6]
|
||||
q_pin = np.concatenate([qpos[0:3], [x, y, z, w], qpos[7:]])
|
||||
# MuJoCo vel is already [lin_world(3), ang_body(3), joints(16)]
|
||||
# Pinocchio wants [lin_body(3), ang_body(3), joints(16)]
|
||||
from scipy.spatial.transform import Rotation
|
||||
R = Rotation.from_quat([x, y, z, w]).as_matrix()
|
||||
v_body = R.T @ qvel[0:3]
|
||||
dq_pin = np.concatenate([v_body, qvel[3:]])
|
||||
return q_pin, dq_pin
|
||||
@@ -0,0 +1,111 @@
|
||||
"""Main entry point: wheeled-legged robot simulation.
|
||||
|
||||
Controls:
|
||||
Mode: wheel (default) - differential drive + posture hold
|
||||
trot - quadruped gait with wheel assist
|
||||
mpc - convex MPC locomotion (torque control)
|
||||
|
||||
Keyboard (in MuJoCo viewer):
|
||||
W/S: vel_x ±0.1
|
||||
A/D: yaw_rate ±0.2
|
||||
Q/E: height ±0.02
|
||||
1: wheel mode
|
||||
2: trot mode
|
||||
3: MPC mode
|
||||
4: prone toggle
|
||||
Z: reset commands
|
||||
"""
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
import mujoco.viewer as mjv
|
||||
|
||||
from robot import Robot
|
||||
from controller import Controller
|
||||
from gui import GUI
|
||||
from config import CTRL_DECIMATION
|
||||
|
||||
|
||||
def main():
|
||||
robot = Robot()
|
||||
robot.reset()
|
||||
ctrl = Controller(robot)
|
||||
gui = GUI(ctrl)
|
||||
|
||||
step = 0
|
||||
sim_steps_per_ctrl = CTRL_DECIMATION
|
||||
|
||||
def key_callback(keycode):
|
||||
"""Called from MuJoCo render thread - only modify ctrl directly, not tkinter."""
|
||||
try:
|
||||
c = chr(keycode).lower()
|
||||
except (ValueError, OverflowError):
|
||||
return
|
||||
if c == 'w':
|
||||
ctrl.vel_x = min(ctrl.vel_x + 0.1, 1.5)
|
||||
elif c == 's':
|
||||
ctrl.vel_x = max(ctrl.vel_x - 0.1, -1.5)
|
||||
elif c == 'a':
|
||||
ctrl.yaw_rate = min(ctrl.yaw_rate + 0.2, 2.0)
|
||||
elif c == 'd':
|
||||
ctrl.yaw_rate = max(ctrl.yaw_rate - 0.2, -2.0)
|
||||
elif c == 'q':
|
||||
ctrl.height = min(ctrl.height + 0.02, 0.45)
|
||||
elif c == 'e':
|
||||
ctrl.height = max(ctrl.height - 0.02, 0.17)
|
||||
elif c == '1':
|
||||
ctrl.mode = "wheel"; ctrl.prone = False
|
||||
elif c == '2':
|
||||
ctrl.mode = "trot"; ctrl.prone = False
|
||||
elif c == '3':
|
||||
ctrl.mode = "mpc"; ctrl.prone = False
|
||||
elif c == '4':
|
||||
ctrl.prone = not ctrl.prone
|
||||
elif c == 'z':
|
||||
ctrl.vel_x = 0.0; ctrl.vel_y = 0.0; ctrl.yaw_rate = 0.0
|
||||
|
||||
with mjv.launch_passive(robot.model, robot.data, key_callback=key_callback) as viewer:
|
||||
viewer.cam.distance = 2.5
|
||||
viewer.cam.elevation = -20
|
||||
viewer.cam.azimuth = 135
|
||||
|
||||
last_time = robot.data.time
|
||||
|
||||
while viewer.is_running() and not gui.closed:
|
||||
t_start = time.perf_counter()
|
||||
|
||||
# Detect viewer reset (Backspace) - time jumps back to 0
|
||||
if robot.data.time < last_time:
|
||||
robot.reset()
|
||||
last_time = robot.data.time
|
||||
|
||||
# Get state and compute control
|
||||
state = robot.get_state()
|
||||
leg_targets, wheel_targets = ctrl.compute(state, robot.dt * sim_steps_per_ctrl)
|
||||
|
||||
# Apply control and step simulation
|
||||
# MPC mode sets ctrl directly via set_ctrl_mit, skip set_ctrl
|
||||
if ctrl.mode != "mpc":
|
||||
robot.set_ctrl(leg_targets, wheel_targets)
|
||||
for _ in range(sim_steps_per_ctrl):
|
||||
robot.step()
|
||||
|
||||
viewer.sync()
|
||||
step += 1
|
||||
|
||||
# Update GUI every 25 steps (~10 Hz)
|
||||
if step % 25 == 0:
|
||||
state = robot.get_state()
|
||||
gui.update_status(state, step)
|
||||
if not gui.tick():
|
||||
break
|
||||
|
||||
# Real-time sync
|
||||
elapsed = time.perf_counter() - t_start
|
||||
target_dt = robot.dt * sim_steps_per_ctrl
|
||||
if elapsed < target_dt:
|
||||
time.sleep(target_dt - elapsed)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,528 @@
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
"""MuJoCo static posture optimizer for wheeled-leg standing defaults.
|
||||
|
||||
The old posture tools are mostly analytical. This script keeps the fast MJCF
|
||||
kinematics as a candidate generator, then evaluates the best candidates in
|
||||
MuJoCo with gravity and floor contact enabled.
|
||||
|
||||
The score is meant for RL default pose / real deployment:
|
||||
- low peak and RMS standing torque, so one hot motor is not hidden by average
|
||||
- wheel contact point close to the hip in X for wheel speed tracking
|
||||
- COM projection margin inside the four-wheel support rectangle
|
||||
- non-singular leg Jacobian for posture control authority
|
||||
- underbody and knee clearance for obstacle tolerance
|
||||
|
||||
Usage:
|
||||
uv run python mujoco_sim/static_posture_optimizer.py
|
||||
uv run python mujoco_sim/static_posture_optimizer.py --quick
|
||||
uv run python mujoco_sim/static_posture_optimizer.py --ab-max 0.08
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import mujoco
|
||||
import numpy as np
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(REPO_ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(REPO_ROOT))
|
||||
|
||||
from mujoco_sim.rl_friendly_opt import get_all # noqa: E402
|
||||
|
||||
SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
|
||||
|
||||
LEG_NAMES = ("fl", "fr", "rl", "rr")
|
||||
LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
|
||||
WHEEL_JOINT = "wheel_joint"
|
||||
LEG_ACTUATORS = [f"{leg}_{jt}" for leg in LEG_NAMES for jt in LEG_JOINTS]
|
||||
|
||||
ROBOT_MASS = 12.3
|
||||
G = 9.81
|
||||
MAX_TORQUE = 17.0
|
||||
WHEEL_RADIUS = 0.10
|
||||
|
||||
HIP_MIN, HIP_MAX = -2.58, 2.58
|
||||
KNEE_MIN, KNEE_MAX = -2.65, 2.65
|
||||
HIP_SCAN = (0.20, 1.15)
|
||||
KNEE_SCAN = (-1.90, -0.65)
|
||||
|
||||
SOFT_WHEEL_X = 0.045
|
||||
HARD_WHEEL_X = 0.085
|
||||
MIN_COM_MARGIN = 0.045
|
||||
MIN_KNEE_CLEARANCE = 0.105
|
||||
MIN_UNDERBODY_CLEARANCE = 0.33
|
||||
MAX_COND = 3.7
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Candidate:
|
||||
ab: float
|
||||
hip: float
|
||||
knee: float
|
||||
z_fk: float
|
||||
analytic_cost: float
|
||||
|
||||
|
||||
@dataclass
|
||||
class StaticResult:
|
||||
z_target: float
|
||||
z: float
|
||||
ab: float
|
||||
hip: float
|
||||
knee: float
|
||||
cost: float
|
||||
peak_tau: float
|
||||
rms_tau: float
|
||||
mean_i2r: float
|
||||
imbal: float
|
||||
wheel_x: float
|
||||
cond: float
|
||||
min_sv: float
|
||||
com_margin_x: float
|
||||
com_margin_y: float
|
||||
support_margin: float
|
||||
normal_cv: float
|
||||
body_clearance: float
|
||||
knee_clearance: float
|
||||
roll: float
|
||||
pitch: float
|
||||
height_err: float
|
||||
|
||||
|
||||
def _configure_actuators(model: mujoco.MjModel) -> None:
|
||||
"""Configure leg joints as position PD and wheels as zero-velocity motors."""
|
||||
for i in range(model.nu):
|
||||
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) or ""
|
||||
model.actuator_biastype[i] = 1
|
||||
model.actuator_gaintype[i] = 0
|
||||
model.actuator_forcelimited[i] = 0
|
||||
if "wheel" in name:
|
||||
model.actuator_gainprm[i, 0] = 2.0
|
||||
model.actuator_biasprm[i, 0] = 0.0
|
||||
model.actuator_biasprm[i, 1] = 0.0
|
||||
model.actuator_biasprm[i, 2] = -2.0
|
||||
model.actuator_ctrlrange[i] = [-20.0, 20.0]
|
||||
else:
|
||||
model.actuator_gainprm[i, 0] = 60.0
|
||||
model.actuator_biasprm[i, 0] = 0.0
|
||||
model.actuator_biasprm[i, 1] = -60.0
|
||||
model.actuator_biasprm[i, 2] = -3.0
|
||||
model.actuator_ctrlrange[i] = [-3.14, 3.14]
|
||||
|
||||
|
||||
def _ids(model: mujoco.MjModel):
|
||||
act = {}
|
||||
qadr = {}
|
||||
for leg in LEG_NAMES:
|
||||
for jt in (*LEG_JOINTS, WHEEL_JOINT):
|
||||
name = f"{leg}_{jt}"
|
||||
act[name] = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||
qadr[name] = model.jnt_qposadr[jid]
|
||||
bodies = {
|
||||
"base": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link"),
|
||||
**{
|
||||
f"{leg}_wheel": mujoco.mj_name2id(
|
||||
model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link"
|
||||
)
|
||||
for leg in LEG_NAMES
|
||||
},
|
||||
**{
|
||||
f"{leg}_knee": mujoco.mj_name2id(
|
||||
model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_knee_Link"
|
||||
)
|
||||
for leg in LEG_NAMES
|
||||
},
|
||||
}
|
||||
return act, qadr, bodies
|
||||
|
||||
|
||||
def _mirrored_ab(leg: str, ab: float) -> float:
|
||||
return ab if leg[1] == "l" else -ab
|
||||
|
||||
|
||||
def _set_pose(
|
||||
model: mujoco.MjModel,
|
||||
data: mujoco.MjData,
|
||||
act: dict[str, int],
|
||||
qadr: dict[str, int],
|
||||
cand: Candidate,
|
||||
) -> None:
|
||||
mujoco.mj_resetData(model, data)
|
||||
for leg in LEG_NAMES:
|
||||
vals = (_mirrored_ab(leg, cand.ab), cand.hip, cand.knee)
|
||||
for jt, val in zip(LEG_JOINTS, vals):
|
||||
name = f"{leg}_{jt}"
|
||||
data.qpos[qadr[name]] = val
|
||||
data.ctrl[act[name]] = val
|
||||
data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
|
||||
data.qpos[0:3] = [0.0, 0.0, cand.z_fk]
|
||||
data.qpos[3] = 1.0
|
||||
data.qpos[4:7] = 0.0
|
||||
mujoco.mj_forward(model, data)
|
||||
|
||||
|
||||
def _rpy(data: mujoco.MjData, bodies: dict[str, int]) -> tuple[float, float]:
|
||||
rot = data.xmat[bodies["base"]].reshape(3, 3)
|
||||
roll = math.atan2(rot[2, 1], rot[2, 2])
|
||||
pitch = math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2))
|
||||
return roll, pitch
|
||||
|
||||
|
||||
def _robot_com(model: mujoco.MjModel, data: mujoco.MjData) -> np.ndarray:
|
||||
masses = model.body_mass[1:]
|
||||
return (data.xipos[1:] * masses[:, None]).sum(axis=0) / masses.sum()
|
||||
|
||||
|
||||
def _support_metrics(
|
||||
model: mujoco.MjModel,
|
||||
data: mujoco.MjData,
|
||||
bodies: dict[str, int],
|
||||
) -> tuple[float, float, float, float]:
|
||||
wheel_xy = np.array([data.xpos[bodies[f"{leg}_wheel"]][:2] for leg in LEG_NAMES])
|
||||
com_xy = _robot_com(model, data)[:2]
|
||||
min_xy = wheel_xy.min(axis=0)
|
||||
max_xy = wheel_xy.max(axis=0)
|
||||
margin_low = com_xy - min_xy
|
||||
margin_high = max_xy - com_xy
|
||||
margin_x = float(min(margin_low[0], margin_high[0]))
|
||||
margin_y = float(min(margin_low[1], margin_high[1]))
|
||||
support_margin = float(min(margin_x, margin_y))
|
||||
|
||||
normal = []
|
||||
for leg in LEG_NAMES:
|
||||
bid = bodies[f"{leg}_wheel"]
|
||||
fz = 0.0
|
||||
for i in range(data.ncon):
|
||||
con = data.contact[i]
|
||||
b1 = model.geom_bodyid[con.geom1]
|
||||
b2 = model.geom_bodyid[con.geom2]
|
||||
if b1 == bid or b2 == bid:
|
||||
wrench = np.zeros(6)
|
||||
mujoco.mj_contactForce(model, data, i, wrench)
|
||||
fz += abs(float(wrench[0]))
|
||||
normal.append(fz)
|
||||
normal = np.asarray(normal, dtype=float)
|
||||
if normal.sum() < 1e-6:
|
||||
normal_cv = 9.99
|
||||
else:
|
||||
normal_cv = float(normal.std() / max(1e-6, normal.mean()))
|
||||
return margin_x, margin_y, support_margin, normal_cv
|
||||
|
||||
|
||||
def _clearance_metrics(data: mujoco.MjData, bodies: dict[str, int]) -> tuple[float, float]:
|
||||
base_z = float(data.xpos[bodies["base"]][2])
|
||||
# The collision box in wheelleg.xml is centered at z=0.054 with half-height 0.073.
|
||||
body_clearance = base_z + 0.054 - 0.073
|
||||
knee_z = min(float(data.xpos[bodies[f"{leg}_knee"]][2]) for leg in LEG_NAMES)
|
||||
wheel_z = min(float(data.xpos[bodies[f"{leg}_wheel"]][2]) for leg in LEG_NAMES)
|
||||
return body_clearance, knee_z - wheel_z
|
||||
|
||||
|
||||
def _static_cost(r: StaticResult) -> float:
|
||||
wheel_over = max(0.0, abs(r.wheel_x) - SOFT_WHEEL_X)
|
||||
support_short = max(0.0, MIN_COM_MARGIN - r.support_margin)
|
||||
body_short = max(0.0, MIN_UNDERBODY_CLEARANCE - r.body_clearance)
|
||||
knee_short = max(0.0, MIN_KNEE_CLEARANCE - r.knee_clearance)
|
||||
cond_over = max(0.0, r.cond - MAX_COND)
|
||||
tilt = math.hypot(r.roll, r.pitch)
|
||||
|
||||
return (
|
||||
6500.0 * r.height_err**2
|
||||
+ 2.6 * (r.peak_tau / MAX_TORQUE) ** 2
|
||||
+ 1.1 * (r.rms_tau / MAX_TORQUE) ** 2
|
||||
+ 1.8 * max(0.0, r.imbal - 1.6) ** 2
|
||||
+ 0.65 * (abs(r.wheel_x) / SOFT_WHEEL_X) ** 2
|
||||
+ 9.0 * (wheel_over / max(1e-6, HARD_WHEEL_X - SOFT_WHEEL_X)) ** 2
|
||||
+ 10.0 * (support_short / MIN_COM_MARGIN) ** 2
|
||||
+ 2.0 * (r.normal_cv / 0.35) ** 2
|
||||
+ 30.0 * (cond_over / 1.0) ** 2
|
||||
+ 4.0 * (body_short / 0.06) ** 2
|
||||
+ 2.0 * (knee_short / 0.04) ** 2
|
||||
+ 1.0 * (tilt / 0.05) ** 2
|
||||
)
|
||||
|
||||
|
||||
def _analytic_cost(r: dict, z_target: float) -> float:
|
||||
wheel_over = max(0.0, abs(r["r_hip_x_mag"]) - SOFT_WHEEL_X)
|
||||
cond_over = max(0.0, r["cond"] - MAX_COND)
|
||||
return (
|
||||
1400.0 * (r["z"] - z_target) ** 2
|
||||
+ 2.2 * (r["max_tau"] / MAX_TORQUE) ** 2
|
||||
+ 0.7 * (math.sqrt(r["i2r"] / 3.0) / MAX_TORQUE) ** 2
|
||||
+ 0.55 * (abs(r["r_hip_x_mag"]) / SOFT_WHEEL_X) ** 2
|
||||
+ 8.0 * (wheel_over / max(1e-6, HARD_WHEEL_X - SOFT_WHEEL_X)) ** 2
|
||||
+ 2.0 * (cond_over / 2.0) ** 2
|
||||
)
|
||||
|
||||
|
||||
def generate_candidates(
|
||||
z_target: float,
|
||||
step: float,
|
||||
ab_max: float,
|
||||
keep: int,
|
||||
z_tol: float,
|
||||
) -> list[Candidate]:
|
||||
candidates: list[Candidate] = []
|
||||
ab_values = np.arange(0.0, ab_max + 0.5 * step, step)
|
||||
hip_values = np.arange(HIP_SCAN[0], HIP_SCAN[1] + 0.5 * step, step)
|
||||
knee_values = np.arange(KNEE_SCAN[0], KNEE_SCAN[1] + 0.5 * step, step)
|
||||
for ab in ab_values:
|
||||
for hip in hip_values:
|
||||
if hip < HIP_MIN or hip > HIP_MAX:
|
||||
continue
|
||||
for knee in knee_values:
|
||||
if knee < KNEE_MIN or knee > KNEE_MAX:
|
||||
continue
|
||||
r = get_all(float(ab), float(hip), float(knee))
|
||||
if abs(r["z"] - z_target) > z_tol:
|
||||
continue
|
||||
if r["wz"] >= r["kz"]:
|
||||
continue
|
||||
if abs(r["r_hip_x_mag"]) > HARD_WHEEL_X:
|
||||
continue
|
||||
if r["max_tau"] > MAX_TORQUE * 1.15:
|
||||
continue
|
||||
candidates.append(
|
||||
Candidate(
|
||||
ab=float(ab),
|
||||
hip=float(hip),
|
||||
knee=float(knee),
|
||||
z_fk=float(r["z"]),
|
||||
analytic_cost=_analytic_cost(r, z_target),
|
||||
)
|
||||
)
|
||||
candidates.sort(key=lambda c: c.analytic_cost)
|
||||
return candidates[:keep]
|
||||
|
||||
|
||||
def evaluate_candidate(
|
||||
model: mujoco.MjModel,
|
||||
data: mujoco.MjData,
|
||||
act: dict[str, int],
|
||||
qadr: dict[str, int],
|
||||
bodies: dict[str, int],
|
||||
cand: Candidate,
|
||||
z_target: float,
|
||||
settle_steps: int,
|
||||
avg_steps: int,
|
||||
) -> StaticResult | None:
|
||||
_set_pose(model, data, act, qadr, cand)
|
||||
for _ in range(settle_steps):
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
roll, pitch = _rpy(data, bodies)
|
||||
if abs(roll) > 0.35 or abs(pitch) > 0.35:
|
||||
return None
|
||||
|
||||
tau_buf = []
|
||||
for _ in range(avg_steps):
|
||||
mujoco.mj_step(model, data)
|
||||
tau_buf.append([data.actuator_force[act[name]] for name in LEG_ACTUATORS])
|
||||
tau = np.asarray(tau_buf, dtype=float).mean(axis=0)
|
||||
tau_abs = np.abs(tau)
|
||||
peak = float(tau_abs.max())
|
||||
rms = float(np.sqrt(np.mean(tau * tau)))
|
||||
mean_i2r = float(np.mean(tau * tau))
|
||||
|
||||
sagittal_abs = []
|
||||
for name, value in zip(LEG_ACTUATORS, tau):
|
||||
if "hip_pitch" in name or "knee" in name:
|
||||
sagittal_abs.append(abs(float(value)))
|
||||
sagittal_abs = np.asarray(sagittal_abs, dtype=float)
|
||||
imbal = float(sagittal_abs.max() / max(1e-6, sagittal_abs.mean()))
|
||||
|
||||
fk = get_all(cand.ab, cand.hip, cand.knee)
|
||||
margin_x, margin_y, support_margin, normal_cv = _support_metrics(model, data, bodies)
|
||||
body_clearance, knee_clearance = _clearance_metrics(data, bodies)
|
||||
z = float(data.xpos[bodies["base"]][2])
|
||||
|
||||
result = StaticResult(
|
||||
z_target=z_target,
|
||||
z=z,
|
||||
ab=cand.ab,
|
||||
hip=cand.hip,
|
||||
knee=cand.knee,
|
||||
cost=0.0,
|
||||
peak_tau=peak,
|
||||
rms_tau=rms,
|
||||
mean_i2r=mean_i2r,
|
||||
imbal=imbal,
|
||||
wheel_x=float(fk["r_hip_x_mag"]),
|
||||
cond=float(fk["cond"]),
|
||||
min_sv=float(fk["min_sv"]),
|
||||
com_margin_x=margin_x,
|
||||
com_margin_y=margin_y,
|
||||
support_margin=support_margin,
|
||||
normal_cv=normal_cv,
|
||||
body_clearance=body_clearance,
|
||||
knee_clearance=knee_clearance,
|
||||
roll=roll,
|
||||
pitch=pitch,
|
||||
height_err=z - z_target,
|
||||
)
|
||||
result.cost = _static_cost(result)
|
||||
return result
|
||||
|
||||
|
||||
def optimize(
|
||||
z_targets: list[float],
|
||||
step: float,
|
||||
ab_max: float,
|
||||
keep: int,
|
||||
z_tol: float,
|
||||
settle_steps: int,
|
||||
avg_steps: int,
|
||||
) -> list[StaticResult]:
|
||||
model = mujoco.MjModel.from_xml_path(str(SCENE_XML))
|
||||
data = mujoco.MjData(model)
|
||||
_configure_actuators(model)
|
||||
act, qadr, bodies = _ids(model)
|
||||
|
||||
results: list[StaticResult] = []
|
||||
for zt in z_targets:
|
||||
candidates = generate_candidates(zt, step=step, ab_max=ab_max, keep=keep, z_tol=z_tol)
|
||||
best: StaticResult | None = None
|
||||
for cand in candidates:
|
||||
result = evaluate_candidate(
|
||||
model,
|
||||
data,
|
||||
act,
|
||||
qadr,
|
||||
bodies,
|
||||
cand,
|
||||
zt,
|
||||
settle_steps=settle_steps,
|
||||
avg_steps=avg_steps,
|
||||
)
|
||||
if result is None:
|
||||
continue
|
||||
if best is None or result.cost < best.cost:
|
||||
best = result
|
||||
if best is None:
|
||||
print(f"{zt:.2f}: no valid MuJoCo-static candidate from {len(candidates)} seeds")
|
||||
continue
|
||||
results.append(best)
|
||||
print_result(best)
|
||||
return results
|
||||
|
||||
|
||||
def print_header() -> None:
|
||||
print("\nMuJoCo static posture optimization")
|
||||
print("z_tgt z ab hip knee | peak rms imbal wheelX cond | comX comY clrB clrK | cost")
|
||||
print("-" * 111)
|
||||
|
||||
|
||||
def print_result(r: StaticResult) -> None:
|
||||
print(
|
||||
f"{r.z_target:5.2f} {r.z:6.3f} {r.ab:5.2f} {r.hip:6.3f} {r.knee:6.3f} | "
|
||||
f"{r.peak_tau:5.2f} {r.rms_tau:5.2f} {r.imbal:6.2f} "
|
||||
f"{r.wheel_x:6.3f} {r.cond:5.2f} | "
|
||||
f"{r.com_margin_x:5.3f} {r.com_margin_y:5.3f} "
|
||||
f"{r.body_clearance:5.3f} {r.knee_clearance:5.3f} | "
|
||||
f"{r.cost:6.2f}"
|
||||
)
|
||||
|
||||
|
||||
def print_tables(results: list[StaticResult]) -> None:
|
||||
if not results:
|
||||
return
|
||||
print("\nCopy-paste tables:")
|
||||
print("_H_TARGET = " + repr([round(r.z_target, 3) for r in results]))
|
||||
print("_Z_STATIC = " + repr([round(r.z, 3) for r in results]))
|
||||
print("_HIP = " + repr([round(r.hip, 3) for r in results]))
|
||||
print("_KNEE = " + repr([round(r.knee, 3) for r in results]))
|
||||
print("_ABD_LEFT = " + repr([round(r.ab, 3) for r in results]))
|
||||
print("_ABD_RIGHT = " + repr([round(-r.ab, 3) for r in results]))
|
||||
|
||||
best = min(results, key=lambda r: r.cost)
|
||||
print("\nRecommended default:")
|
||||
print(
|
||||
f"z={best.z_target:.2f}, ab={best.ab:.3f}, hip={best.hip:.3f}, "
|
||||
f"knee={best.knee:.3f}, peak={best.peak_tau:.2f}Nm, "
|
||||
f"rms={best.rms_tau:.2f}Nm, support_margin={best.support_margin:.3f}m"
|
||||
)
|
||||
pose = []
|
||||
for leg in LEG_NAMES:
|
||||
pose.extend([_mirrored_ab(leg, best.ab), best.hip, best.knee])
|
||||
pose.extend([0.0, 0.0, 0.0, 0.0])
|
||||
print("default_dof_pos = " + repr([round(v, 3) for v in pose]))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--z-min", type=float, default=0.36)
|
||||
parser.add_argument("--z-max", type=float, default=0.45)
|
||||
parser.add_argument("--z-step", type=float, default=0.01)
|
||||
parser.add_argument("--grid-step", type=float, default=0.01)
|
||||
parser.add_argument("--ab-max", type=float, default=0.0)
|
||||
parser.add_argument("--keep", type=int, default=45)
|
||||
parser.add_argument("--z-tol", type=float, default=0.035)
|
||||
parser.add_argument("--settle-steps", type=int, default=350)
|
||||
parser.add_argument("--avg-steps", type=int, default=80)
|
||||
parser.add_argument("--quick", action="store_true", help="Coarser and faster scan")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.quick:
|
||||
args.grid_step = max(args.grid_step, 0.02)
|
||||
args.keep = min(args.keep, 20)
|
||||
args.z_tol = max(args.z_tol, 0.040)
|
||||
args.settle_steps = min(args.settle_steps, 220)
|
||||
args.avg_steps = min(args.avg_steps, 40)
|
||||
|
||||
n = int(round((args.z_max - args.z_min) / args.z_step)) + 1
|
||||
z_targets = [round(args.z_min + i * args.z_step, 3) for i in range(n)]
|
||||
|
||||
print_header()
|
||||
results = optimize(
|
||||
z_targets,
|
||||
step=args.grid_step,
|
||||
ab_max=args.ab_max,
|
||||
keep=args.keep,
|
||||
z_tol=args.z_tol,
|
||||
settle_steps=args.settle_steps,
|
||||
avg_steps=args.avg_steps,
|
||||
)
|
||||
print_tables(results)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,58 @@
|
||||
# 后期 Sim2Sim 工具
|
||||
|
||||
本目录保存比赛训练架构之后形成的 MuJoCo 策略验证工具。`v0.8.0` 在早期 Sim2Sim 基础上增加 ONNX 策略加载、IK 参数扫描、纯 IK 绕桩验证和批量路线检查；训练任务与 MJCF 不在本阶段修改。
|
||||
|
||||
## 主要入口
|
||||
|
||||
- `nav_sim2sim.py`：Pygame 面板与 MuJoCo 多任务导航，Rough 策略优先加载根目录的 `model_6800.onnx`。
|
||||
- `sim2sim.py`：较轻量的键盘控制与策略回放入口，优先加载 `model_6800.onnx`，缺失时回退到早期 `model_rough.pt`。
|
||||
- `ik_slalom_sim2sim.py`：不依赖 RL 策略的 IK、差速轮、路径跟踪和绕桩测试。
|
||||
- `ik_compensation_sweep.py`：批量扫描 IK 补偿参数并输出排序结果。
|
||||
- `nav_route_sim2sim_check.py`：使用 ONNX 策略批量检查内置任务或外部航点路线。
|
||||
- `export_onnx.py`：将兼容的 PyTorch actor checkpoint 导出并核对为 ONNX。
|
||||
- `interface/mujoco_io.py`：MuJoCo 模型、传感器和执行器接口。
|
||||
- `policy/policy_runner.py`：PT/ONNX 策略加载与历史观测缓存。
|
||||
|
||||
## 环境
|
||||
|
||||
主训练环境继续由根目录的 `uv.lock` 管理。后期 Sim2Sim 新增的 Pygame 与 ONNX Runtime 单独记录在 `sim2sim/requirements.txt`，运行时叠加，避免重新解析时改变已归档的 MuJoCo nightly 版本：
|
||||
|
||||
```powershell
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_sim2sim.py
|
||||
```
|
||||
|
||||
训练工程提供 MuJoCo、NumPy、PyTorch、Matplotlib 和 `pynput`；专用 requirements 显式补充 Pygame 与 ONNX Runtime。下面其他命令同样使用 `--with-requirements .\sim2sim\requirements.txt`。
|
||||
|
||||
## 常用命令
|
||||
|
||||
```powershell
|
||||
# 比赛 Rough 策略交互回放
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_sim2sim.py
|
||||
|
||||
# 轻量策略回放
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\sim2sim.py
|
||||
|
||||
# 纯 IK 绕桩验证
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\ik_slalom_sim2sim.py --test slalom
|
||||
|
||||
# IK 补偿参数扫描
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\ik_compensation_sweep.py --top 12
|
||||
|
||||
# 使用内置绕桩任务做批量 Sim2Sim 路线检查
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_route_sim2sim_check.py `
|
||||
--terrain-xml .\sim2sim\terrain\scene_terrain.xml `
|
||||
--mission slalom `
|
||||
--onnx .\model_6800.onnx
|
||||
|
||||
# 导出早期参考 PT 权重；也可用 --pt-path 指定其他 checkpoint
|
||||
uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\export_onnx.py
|
||||
```
|
||||
|
||||
## 模型与边界
|
||||
|
||||
- `../model_6800.onnx` 是 `last_not_slalom_1050` 最终真机工程使用的比赛 Rough 策略，SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
|
||||
- `../model_rough.pt` 是较早阶段的参考 checkpoint，两者不是同一版本的权重。
|
||||
- Crawl 模型未在本阶段归档；需要 Crawl 策略的入口会查找 `model_crawl.onnx` 或 `model_crawl.pt`。
|
||||
- `nav_route_sim2sim_check.py` 依赖 `../tools/nav_tools/route_safety_check.py` 的航点和避障几何定义；默认使用 `points_20260715_120154.json` 与 `1hao.xml`。
|
||||
|
||||
运行时生成的日志、临时 XML、`route_check_runs/` 和批量实验输出不纳入版本库。人工打点形成的路线快照保存在 `../tools/nav_tools/points/`，大量重复仿真轨迹仍不复制。
|
||||
@@ -0,0 +1,110 @@
|
||||
import argparse
|
||||
import torch
|
||||
import torch.nn as nn
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
import onnxruntime as ort
|
||||
|
||||
|
||||
PROJECT_ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
class PolicyMLP(nn.Module):
|
||||
def __init__(self, obs_dim=53, action_dim=16):
|
||||
super().__init__()
|
||||
self.register_buffer("obs_mean", torch.zeros(obs_dim))
|
||||
self.register_buffer("obs_std", torch.ones(obs_dim))
|
||||
self.net = nn.Sequential(
|
||||
nn.Linear(obs_dim, 512), nn.ELU(),
|
||||
nn.Linear(512, 256), nn.ELU(),
|
||||
nn.Linear(256, 128), nn.ELU(),
|
||||
nn.Linear(128, action_dim),
|
||||
)
|
||||
|
||||
def forward(self, x):
|
||||
x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6)
|
||||
return self.net(x)
|
||||
|
||||
def load_policy(model_path, device):
|
||||
ckpt = torch.load(model_path, map_location=device, weights_only=False)
|
||||
state_dict = ckpt["actor_state_dict"]
|
||||
weight_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
|
||||
obs_dim = state_dict[weight_key].shape[1]
|
||||
|
||||
output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight"
|
||||
action_dim = state_dict[output_key].shape[0]
|
||||
|
||||
model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim)
|
||||
my_sd = {}
|
||||
for k, v in state_dict.items():
|
||||
if k.startswith("mlp."):
|
||||
my_sd[k.replace("mlp.", "net.")] = v
|
||||
elif k.startswith("net."):
|
||||
my_sd[k] = v
|
||||
elif k == "obs_normalizer._mean":
|
||||
my_sd["obs_mean"] = v.squeeze()
|
||||
elif k == "obs_normalizer._var":
|
||||
my_sd["obs_std"] = torch.sqrt(v.squeeze() + 1e-5)
|
||||
|
||||
model.load_state_dict(my_sd, strict=False)
|
||||
model.eval()
|
||||
model.to(device)
|
||||
return model, obs_dim
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument(
|
||||
"--pt-path",
|
||||
"--pt_path",
|
||||
dest="pt_path",
|
||||
type=Path,
|
||||
default=PROJECT_ROOT / "model_rough.pt",
|
||||
help="PyTorch checkpoint to export (default: ../model_rough.pt).",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
pt_path = args.pt_path.expanduser().resolve()
|
||||
if not pt_path.exists():
|
||||
print(f"File not found: {pt_path}")
|
||||
return
|
||||
|
||||
device = torch.device("cpu")
|
||||
print(f"Loading {pt_path}...")
|
||||
model, obs_dim = load_policy(pt_path, device)
|
||||
|
||||
onnx_path = pt_path.with_suffix(".onnx")
|
||||
|
||||
dummy_input = torch.randn(1, obs_dim, device=device)
|
||||
|
||||
print(f"Exporting to {onnx_path}...")
|
||||
torch.onnx.export(
|
||||
model,
|
||||
dummy_input,
|
||||
str(onnx_path),
|
||||
export_params=True,
|
||||
opset_version=14,
|
||||
do_constant_folding=True,
|
||||
input_names=["obs"],
|
||||
output_names=["action"],
|
||||
dynamic_axes={"obs": {0: "batch_size"}, "action": {0: "batch_size"}}
|
||||
)
|
||||
|
||||
print("Verifying ONNX export...")
|
||||
try:
|
||||
session = ort.InferenceSession(str(onnx_path))
|
||||
with torch.no_grad():
|
||||
pt_out = model(dummy_input).numpy()
|
||||
onnx_out = session.run(["action"], {"obs": dummy_input.numpy()})[0]
|
||||
|
||||
max_diff = np.max(np.abs(pt_out - onnx_out))
|
||||
mean_diff = np.mean(np.abs(pt_out - onnx_out))
|
||||
print(f"ONNX vs PyTorch - max_diff: {max_diff:.6f}, mean_diff: {mean_diff:.6f}")
|
||||
|
||||
if max_diff < 1e-4:
|
||||
print("ONNX export verified OK.")
|
||||
else:
|
||||
print("WARNING: ONNX export has significant divergence from PyTorch model.")
|
||||
except ImportError:
|
||||
print("onnxruntime not installed. Skipping verification. Install with: pip install onnxruntime")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,238 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Sweep IK compensation parameters in the standalone sim2sim scene."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import importlib.util
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
THIS_DIR = Path(__file__).resolve().parent
|
||||
PROJECT_ROOT = THIS_DIR.parent
|
||||
if str(THIS_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(THIS_DIR))
|
||||
|
||||
SIM_PATH = THIS_DIR / "ik_slalom_sim2sim.py"
|
||||
spec = importlib.util.spec_from_file_location("ik_slalom_sim2sim", SIM_PATH)
|
||||
if spec is None or spec.loader is None:
|
||||
raise RuntimeError(f"Cannot load {SIM_PATH}")
|
||||
sim = importlib.util.module_from_spec(spec)
|
||||
sys.modules["ik_slalom_sim2sim"] = sim
|
||||
spec.loader.exec_module(sim)
|
||||
|
||||
|
||||
TRIALS = [
|
||||
{"name": "forward", "speed": 1.0, "yaw": 0.0, "target_vx": 1.0, "target_yaw": 0.0},
|
||||
{"name": "yaw", "speed": 0.0, "yaw": 1.0, "target_vx": 0.0, "target_yaw": 1.0},
|
||||
{"name": "arc", "speed": 1.0, "yaw": 1.0, "target_vx": 1.0, "target_yaw": 1.0},
|
||||
]
|
||||
|
||||
|
||||
def parse_float_list(text: str) -> list[float]:
|
||||
return [float(x.strip()) for x in text.split(",") if x.strip()]
|
||||
|
||||
|
||||
def parse_bool_list(text: str) -> list[bool]:
|
||||
out: list[bool] = []
|
||||
for item in text.split(","):
|
||||
key = item.strip().lower()
|
||||
if not key:
|
||||
continue
|
||||
if key in {"1", "true", "on", "yes"}:
|
||||
out.append(True)
|
||||
elif key in {"0", "false", "off", "no"}:
|
||||
out.append(False)
|
||||
else:
|
||||
raise argparse.ArgumentTypeError(f"Invalid bool item: {item}")
|
||||
return out
|
||||
|
||||
|
||||
def make_sim_args(args: argparse.Namespace, trial: dict[str, float | str], cfg: dict[str, Any]) -> argparse.Namespace:
|
||||
argv = [
|
||||
"ik_slalom_sim2sim.py",
|
||||
"--test",
|
||||
str(trial["name"]),
|
||||
"--duration",
|
||||
str(args.duration),
|
||||
"--settle",
|
||||
str(args.settle),
|
||||
"--speed",
|
||||
str(trial["speed"]),
|
||||
"--yaw-rate",
|
||||
str(trial["yaw"]),
|
||||
"--posture",
|
||||
"custom",
|
||||
"--custom-abduction",
|
||||
str(args.custom_abduction),
|
||||
"--custom-hip",
|
||||
str(args.custom_hip),
|
||||
"--custom-knee",
|
||||
str(args.custom_knee),
|
||||
"--wheel-model",
|
||||
"direct",
|
||||
"--linear-wheel-gain",
|
||||
str(args.linear_wheel_gain),
|
||||
"--direct-yaw-wheel-gain",
|
||||
str(args.direct_yaw_wheel_gain),
|
||||
"--max-wheel-speed",
|
||||
str(args.max_wheel_speed),
|
||||
"--wheel-accel-limit",
|
||||
str(args.wheel_accel_limit),
|
||||
"--yaw-rate-kp",
|
||||
str(cfg["yaw_rate_kp"]),
|
||||
"--encoder-posture-kp",
|
||||
str(cfg["encoder_posture_kp"]),
|
||||
"--encoder-posture-max",
|
||||
str(cfg["encoder_posture_max"]),
|
||||
"--roll-comp-gain",
|
||||
str(cfg["roll_comp_gain"]),
|
||||
"--pitch-comp-gain",
|
||||
str(cfg["pitch_comp_gain"]),
|
||||
"--no-realtime",
|
||||
]
|
||||
argv.append("--imu-posture" if cfg["imu_posture"] else "--no-imu-posture")
|
||||
argv.append("--encoder-guard" if cfg["encoder_guard"] else "--no-encoder-guard")
|
||||
argv.append("--imu-guard" if cfg["imu_guard"] else "--no-imu-guard")
|
||||
old_argv = sys.argv
|
||||
try:
|
||||
sys.argv = argv
|
||||
return sim.parse_args()
|
||||
finally:
|
||||
sys.argv = old_argv
|
||||
|
||||
|
||||
def score_trial(out: dict[str, Any], trial: dict[str, float | str]) -> dict[str, float]:
|
||||
vx = float(out["mean_body_vx_mps"])
|
||||
yaw = float(out["mean_yaw_rate_rad_s"])
|
||||
vx_err = abs(vx - float(trial["target_vx"]))
|
||||
yaw_err = abs(yaw - float(trial["target_yaw"]))
|
||||
return {
|
||||
"vx": vx,
|
||||
"yaw": yaw,
|
||||
"imu_gyro_z": float(out["mean_imu_gyro_z_rad_s"]),
|
||||
"vx_err": vx_err,
|
||||
"yaw_err": yaw_err,
|
||||
"err": vx_err + yaw_err,
|
||||
}
|
||||
|
||||
|
||||
def run_sweep(args: argparse.Namespace) -> list[dict[str, Any]]:
|
||||
rows: list[dict[str, Any]] = []
|
||||
for imu_posture in args.imu_posture_values:
|
||||
for encoder_guard in args.encoder_guard_values:
|
||||
for imu_guard in args.imu_guard_values:
|
||||
for encoder_posture_kp in args.encoder_posture_kps:
|
||||
for encoder_posture_max in args.encoder_posture_maxs:
|
||||
for yaw_rate_kp in args.yaw_rate_kps:
|
||||
for roll_comp_gain in args.roll_comp_gains:
|
||||
for pitch_comp_gain in args.pitch_comp_gains:
|
||||
cfg = {
|
||||
"imu_posture": imu_posture,
|
||||
"encoder_guard": encoder_guard,
|
||||
"imu_guard": imu_guard,
|
||||
"encoder_posture_kp": encoder_posture_kp,
|
||||
"encoder_posture_max": encoder_posture_max,
|
||||
"yaw_rate_kp": yaw_rate_kp,
|
||||
"roll_comp_gain": roll_comp_gain,
|
||||
"pitch_comp_gain": pitch_comp_gain,
|
||||
}
|
||||
detail: list[dict[str, Any]] = []
|
||||
speed_error = 0.0
|
||||
max_tilt = 0.0
|
||||
max_leg = 0.0
|
||||
mean_wheel_err = 0.0
|
||||
stable_all = True
|
||||
for trial in TRIALS:
|
||||
sim_args = make_sim_args(args, trial, cfg)
|
||||
out = sim.run_one(str(trial["name"]), sim_args)
|
||||
trial_score = score_trial(out, trial)
|
||||
trial_score["test"] = str(trial["name"])
|
||||
detail.append(trial_score)
|
||||
speed_error += trial_score["err"]
|
||||
max_tilt = max(max_tilt, float(out["max_tilt_deg"]))
|
||||
max_leg = max(max_leg, float(out["max_leg_encoder_error_rad"]))
|
||||
mean_wheel_err += float(out["mean_wheel_speed_error_rad_s"])
|
||||
stable_all = stable_all and bool(out["stable"])
|
||||
|
||||
score = (
|
||||
speed_error
|
||||
+ args.tilt_weight * max_tilt
|
||||
+ args.leg_error_weight * max_leg
|
||||
+ args.wheel_error_weight * (mean_wheel_err / len(TRIALS))
|
||||
)
|
||||
row = {
|
||||
**cfg,
|
||||
"score": round(score, 6),
|
||||
"speed_error_sum": round(speed_error, 6),
|
||||
"max_tilt_deg": round(max_tilt, 5),
|
||||
"max_leg_encoder_error_rad": round(max_leg, 6),
|
||||
"mean_wheel_speed_error_rad_s": round(mean_wheel_err / len(TRIALS), 6),
|
||||
"stable_all": stable_all,
|
||||
"detail": detail,
|
||||
}
|
||||
rows.append(row)
|
||||
print(
|
||||
"DONE "
|
||||
+ json.dumps(
|
||||
{k: v for k, v in row.items() if k != "detail"},
|
||||
ensure_ascii=False,
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
rows.sort(key=lambda r: float(r["score"]))
|
||||
return rows
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--duration", type=float, default=3.0)
|
||||
parser.add_argument("--settle", type=float, default=1.5)
|
||||
parser.add_argument("--custom-abduction", type=float, default=0.2)
|
||||
parser.add_argument("--custom-hip", type=float, default=1.697)
|
||||
parser.add_argument("--custom-knee", type=float, default=-2.650)
|
||||
parser.add_argument("--linear-wheel-gain", type=float, default=12.5)
|
||||
parser.add_argument("--direct-yaw-wheel-gain", type=float, default=8.0)
|
||||
parser.add_argument("--max-wheel-speed", type=float, default=12.0)
|
||||
parser.add_argument("--wheel-accel-limit", type=float, default=35.0)
|
||||
parser.add_argument("--imu-posture-values", type=parse_bool_list, default=[True, False])
|
||||
parser.add_argument("--encoder-guard-values", type=parse_bool_list, default=[True])
|
||||
parser.add_argument("--imu-guard-values", type=parse_bool_list, default=[True])
|
||||
parser.add_argument("--encoder-posture-kps", type=parse_float_list, default=[0.0, 0.05, 0.15, 0.30])
|
||||
parser.add_argument("--encoder-posture-maxs", type=parse_float_list, default=[0.03])
|
||||
parser.add_argument("--yaw-rate-kps", type=parse_float_list, default=[0.0, 0.4, 0.8])
|
||||
parser.add_argument("--roll-comp-gains", type=parse_float_list, default=[0.35])
|
||||
parser.add_argument("--pitch-comp-gains", type=parse_float_list, default=[0.35])
|
||||
parser.add_argument("--tilt-weight", type=float, default=0.02)
|
||||
parser.add_argument("--leg-error-weight", type=float, default=0.5)
|
||||
parser.add_argument("--wheel-error-weight", type=float, default=0.0)
|
||||
parser.add_argument("--top", type=int, default=12)
|
||||
parser.add_argument("--json", type=Path, default=None)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
rows = run_sweep(args)
|
||||
if args.json:
|
||||
args.json.write_text(json.dumps(rows, indent=2), encoding="utf-8")
|
||||
|
||||
print("\nTop compensation parameter sets")
|
||||
print("rank score speed_err tilt leg_err wheel_err imu enc_kp yaw_kp enc_guard imu_guard")
|
||||
for i, row in enumerate(rows[: args.top], 1):
|
||||
print(
|
||||
f"{i:2d} {row['score']:7.4f} {row['speed_error_sum']:7.4f} "
|
||||
f"{row['max_tilt_deg']:5.2f} {row['max_leg_encoder_error_rad']:7.4f} "
|
||||
f"{row['mean_wheel_speed_error_rad_s']:7.4f} "
|
||||
f"{int(row['imu_posture'])} {row['encoder_posture_kp']:6.3f} "
|
||||
f"{row['yaw_rate_kp']:6.3f} {int(row['encoder_guard'])} {int(row['imu_guard'])}"
|
||||
)
|
||||
for d in row["detail"]:
|
||||
print(f" {d['test']:<7} vx={d['vx']:+.3f} yaw={d['yaw']:+.3f} err={d['err']:.3f}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
File diff suppressed because it is too large
Load Diff
@@ -87,9 +87,14 @@ class MuJoCoIO:
|
||||
return out_xml_path
|
||||
|
||||
def _rebuild_actuators(self, spec):
|
||||
actuators_to_delete = list(spec.actuators)
|
||||
for act in actuators_to_delete:
|
||||
spec.delete(act)
|
||||
if hasattr(spec, "delete"):
|
||||
actuators_to_delete = list(spec.actuators)
|
||||
for act in actuators_to_delete:
|
||||
spec.delete(act)
|
||||
else:
|
||||
actuators_to_delete = list(spec.actuators)
|
||||
for act in actuators_to_delete:
|
||||
act.delete()
|
||||
|
||||
# Keep sim2sim aligned with the training robot config and sim2real runtime:
|
||||
# leg position PD = (50.0, 1.5), wheel velocity damping = 1.0.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -324,9 +324,11 @@ def main():
|
||||
terrain_dir = Path(__file__).parent / "terrain"
|
||||
terrain_xml = terrain_dir / "scene_terrain.xml"
|
||||
robot_xml = project_root / "mjcf" / "wheelleg.xml"
|
||||
rough_onnx = project_root / "model_6800.onnx"
|
||||
crawl_onnx = project_root / "model_crawl.onnx"
|
||||
policy_path = {
|
||||
"rough": project_root / "model_rough.pt",
|
||||
"crawl": project_root / "model_crawl.pt"
|
||||
"rough": rough_onnx if rough_onnx.exists() else project_root / "model_rough.pt",
|
||||
"crawl": crawl_onnx if crawl_onnx.exists() else project_root / "model_crawl.pt"
|
||||
}
|
||||
|
||||
# 1. 解析 XML 地图障碍物，实现 100% 可视化精准对应
|
||||
|
||||
@@ -3,7 +3,10 @@ import torch.nn as nn
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
from pathlib import Path
|
||||
from pynput import keyboard
|
||||
try:
|
||||
from pynput import keyboard
|
||||
except ImportError:
|
||||
keyboard = None
|
||||
|
||||
# ============================================================
|
||||
# Policy Model
|
||||
@@ -26,6 +29,28 @@ class PolicyMLP(nn.Module):
|
||||
|
||||
|
||||
def load_policy(model_path, device):
|
||||
if str(model_path).endswith('.onnx'):
|
||||
import onnxruntime as ort
|
||||
session = ort.InferenceSession(str(model_path))
|
||||
class OnnxWrapper:
|
||||
def __init__(self, session):
|
||||
self.session = session
|
||||
self.obs_dim = session.get_inputs()[0].shape[1]
|
||||
if isinstance(self.obs_dim, str):
|
||||
self.obs_dim = 53
|
||||
class MockMean:
|
||||
def __init__(self, d):
|
||||
self.d = d
|
||||
def numel(self):
|
||||
return self.d
|
||||
self.obs_mean = MockMean(self.obs_dim)
|
||||
|
||||
def __call__(self, x):
|
||||
inputs = {self.session.get_inputs()[0].name: x.cpu().numpy()}
|
||||
out = self.session.run(None, inputs)[0]
|
||||
return torch.tensor(out, device=x.device)
|
||||
return OnnxWrapper(session)
|
||||
|
||||
ckpt = torch.load(model_path, map_location=device, weights_only=False)
|
||||
state_dict = ckpt["actor_state_dict"]
|
||||
|
||||
@@ -129,9 +154,13 @@ class PolicyRunner:
|
||||
], dtype=np.float32)
|
||||
|
||||
# Background keyboard listener for seamless switcher keys ('1' and '2')
|
||||
self.listener = keyboard.Listener(on_press=self._on_press)
|
||||
self.listener.start()
|
||||
print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
|
||||
self.listener = None
|
||||
if keyboard is not None:
|
||||
self.listener = keyboard.Listener(on_press=self._on_press)
|
||||
self.listener.start()
|
||||
print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
|
||||
else:
|
||||
print("[PolicyRunner] pynput not installed; background keyboard switcher disabled.")
|
||||
|
||||
def _on_press(self, key):
|
||||
try:
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
# Additional runtime dependencies for the post-training Sim2Sim tools.
|
||||
onnxruntime>=1.19.0
|
||||
pygame>=2.6.1
|
||||
@@ -41,6 +41,19 @@ class PolicyMLP(nn.Module):
|
||||
|
||||
|
||||
def load_policy(model_path, device):
|
||||
if str(model_path).endswith('.onnx'):
|
||||
import onnxruntime as ort
|
||||
session = ort.InferenceSession(str(model_path))
|
||||
class OnnxWrapper:
|
||||
def __init__(self, session):
|
||||
self.session = session
|
||||
def __call__(self, x):
|
||||
inputs = {self.session.get_inputs()[0].name: x.cpu().numpy()}
|
||||
out = self.session.run(None, inputs)[0]
|
||||
return torch.tensor(out, device=x.device)
|
||||
return OnnxWrapper(session)
|
||||
|
||||
|
||||
ckpt = torch.load(model_path, map_location=device, weights_only=False)
|
||||
state_dict = ckpt["actor_state_dict"]
|
||||
model = PolicyMLP()
|
||||
@@ -131,7 +144,9 @@ def main():
|
||||
terrain_dir = Path(__file__).parent / "terrain"
|
||||
terrain_xml = terrain_dir / "scene_terrain.xml"
|
||||
robot_xml = Path(__file__).parent.parent / "mjcf" / "wheelleg.xml"
|
||||
policy_path = Path(__file__).parent.parent / "model_1700.pt"
|
||||
policy_path = Path(__file__).parent.parent / "model_6800.onnx"
|
||||
if not policy_path.exists():
|
||||
policy_path = Path(__file__).parent.parent / "model_rough.pt"
|
||||
hfield_dir = terrain_dir
|
||||
|
||||
temp_xml = project_root / "mjcf" / "sim2sim_temp.xml"
|
||||
|
||||
@@ -52,7 +52,11 @@ from ..mdp.only_positive_rewards import enable_only_positive_rewards
|
||||
from ..mdp.rewards import (
|
||||
track_linear_velocity,
|
||||
track_linear_velocity_l1,
|
||||
track_linear_velocity_x,
|
||||
track_linear_velocity_y,
|
||||
track_angular_velocity,
|
||||
track_angular_velocity_z,
|
||||
stair_lateral_yaw_drift_l2,
|
||||
base_height_l2,
|
||||
safe_base_lin_vel,
|
||||
safe_foot_contact,
|
||||
@@ -79,8 +83,34 @@ from ..mdp.rewards import (
|
||||
ang_vel_xy_l2,
|
||||
undesired_contacts,
|
||||
contact_forces,
|
||||
tracking_lin_vel_error,
|
||||
tracking_yaw_vel_error,
|
||||
tracking_lin_vel_x_error,
|
||||
tracking_lin_vel_y_error,
|
||||
tracking_lin_vel_along_command_error,
|
||||
actual_lin_vel_orthogonal_command_mean,
|
||||
command_lin_vel_mean,
|
||||
command_yaw_vel_abs_mean,
|
||||
actual_lin_vel_mean,
|
||||
tracking_lin_vel_error_band_mean,
|
||||
tracking_lin_vel_axis_error_band_mean,
|
||||
command_band_active,
|
||||
wheel_raw_action_abs_mean,
|
||||
wheel_target_vel_abs_mean,
|
||||
wheel_actual_vel_abs_mean,
|
||||
wheel_target_actual_vel_error_mean,
|
||||
wheel_actual_to_target_vel_ratio_mean,
|
||||
wheel_target_actual_sign_agreement,
|
||||
upright_metric,
|
||||
base_ground_contact_metric,
|
||||
)
|
||||
from ..mdp.curriculums import (
|
||||
command_axis_levels_vel,
|
||||
command_levels_adaptive,
|
||||
terrain_levels_obstacle_release,
|
||||
terrain_levels_ramp_strict,
|
||||
terrain_levels_vel_strict,
|
||||
)
|
||||
from ..mdp.curriculums import terrain_levels_vel_strict
|
||||
from ..mdp.commands import UniformThresholdVelocityCommandCfg
|
||||
|
||||
# Constant Definitions
|
||||
@@ -367,13 +397,18 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
terrain_generator=TerrainGeneratorCfg(
|
||||
size=(8.0, 8.0), border_width=20.0, num_rows=10, num_cols=20, curriculum=True,
|
||||
sub_terrains={
|
||||
"flat": BoxFlatTerrainCfg(proportion=0.05, size=(8.0, 8.0)),
|
||||
"pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
|
||||
"pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.45, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
|
||||
"random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.3), size=(8.0, 8.0)),
|
||||
"flat": BoxFlatTerrainCfg(proportion=0.15, size=(8.0, 8.0)),
|
||||
"pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
|
||||
"pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.35, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
|
||||
"random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.20), size=(8.0, 8.0)),
|
||||
"random_rough": HfRandomUniformTerrainCfg(proportion=0.01, noise_range=(0.0, 0.06), noise_step=0.01, horizontal_scale=0.20, downsampled_scale=0.20, border_width=0.25, base_thickness_ratio=100.0, size=(8.0, 8.0)),
|
||||
"perlin_noise": HfPerlinNoiseTerrainCfg(proportion=0.01, height_range=(0.0, 0.06), octaves=2, persistence=0.4, lacunarity=2.0, horizontal_scale=0.20, resolution=0.20, border_width=0.50, base_thickness_ratio=100.0, size=(8.0, 8.0)),
|
||||
"rc_wall": RCWallTerrainCfg(proportion=0.15, wall_height_range=(0.0, 0.45), size=(8.0, 8.0)),
|
||||
"rc_wall": RCWallTerrainCfg(
|
||||
proportion=0.15,
|
||||
wall_height_range=(0.10, 0.35),
|
||||
wall_centers_x=(2.1, 3.2, 4.3, 5.4, 6.5),
|
||||
size=(8.0, 8.0),
|
||||
),
|
||||
"sloped_terrain": HfPyramidSlopedTerrainCfg(proportion=0.01, slope_range=(0.052, 0.325), platform_width=2.0, border_width=0.25, base_thickness_ratio=100.0, horizontal_scale=0.20, size=(8.0, 8.0)),
|
||||
},
|
||||
),
|
||||
@@ -382,15 +417,65 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
|
||||
# Keep the custom terrain set, but align command/curriculum behavior with go2w rough.
|
||||
cfg.curriculum.pop("command_vel", None)
|
||||
cfg.curriculum["terrain_levels"] = CurriculumTermCfg(func=velocity_mdp.terrain_levels_vel, params={"command_name": "twist"})
|
||||
cfg.curriculum["terrain_levels"] = CurriculumTermCfg(
|
||||
func=terrain_levels_obstacle_release,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"initial_terrain_names": ("flat", "random_rough", "perlin_noise", "sloped_terrain", "pyramid_stairs"),
|
||||
"release_schedule": (
|
||||
(200 * 24, ("random_grid",)),
|
||||
(500 * 24, ("pyramid_stairs_inv",)),
|
||||
(700 * 24, ("rc_wall",)),
|
||||
),
|
||||
},
|
||||
)
|
||||
cfg.curriculum["command_x_levels"] = CurriculumTermCfg(
|
||||
func=command_levels_adaptive,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"reward_term_name": "track_lin_vel_x_exp",
|
||||
"axis": "x",
|
||||
"initial_range": (-0.5, 0.5),
|
||||
"delta_command": 0.05,
|
||||
"target_ratio": 0.8,
|
||||
"ema_alpha": 0.5,
|
||||
},
|
||||
)
|
||||
cfg.curriculum["command_y_levels"] = CurriculumTermCfg(
|
||||
func=command_levels_adaptive,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"reward_term_name": "track_lin_vel_y_exp",
|
||||
"axis": "y",
|
||||
"initial_range": (-0.5, 0.5),
|
||||
"delta_command": 0.05,
|
||||
"target_ratio": 0.8,
|
||||
"ema_alpha": 0.5,
|
||||
},
|
||||
)
|
||||
cfg.curriculum["command_yaw_levels"] = CurriculumTermCfg(
|
||||
func=command_levels_adaptive,
|
||||
params={
|
||||
"command_name": "twist",
|
||||
"reward_term_name": "track_ang_vel_z_exp",
|
||||
"axis": "yaw",
|
||||
"initial_range": (-0.5, 0.5),
|
||||
"delta_command": 0.05,
|
||||
"target_ratio": 0.8,
|
||||
"ema_alpha": 0.5,
|
||||
},
|
||||
)
|
||||
|
||||
cfg.commands["twist"].heading_command = True
|
||||
cfg.commands["twist"].rel_heading_envs = 1.0
|
||||
cfg.commands["twist"].heading_control_stiffness = 0.5
|
||||
cfg.commands["twist"].ranges.heading = (-math.pi, math.pi)
|
||||
cfg.commands["twist"].rel_standing_envs = 0.02
|
||||
cfg.commands["twist"].rel_forward_envs = 0.30
|
||||
cfg.commands["twist"].rel_lateral_envs = 0.20
|
||||
cfg.commands["twist"].rel_yaw_envs = 0.20
|
||||
cfg.commands["twist"].ranges.lin_vel_x = (-1.0, 1.0)
|
||||
cfg.commands["twist"].ranges.lin_vel_y = (-0.6, 0.6)
|
||||
cfg.commands["twist"].ranges.lin_vel_y = (-1.0, 1.0)
|
||||
cfg.commands["twist"].ranges.ang_vel_z = (-1.0, 1.0)
|
||||
|
||||
# ------------------
|
||||
@@ -402,7 +487,7 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
cfg.events["reset_base"] = EventTermCfg(
|
||||
func=envs_mdp.reset_root_state_uniform, mode="reset",
|
||||
params={
|
||||
"pose_range": {"z": (0.40, 0.45), "yaw": (-math.pi, math.pi)},
|
||||
"pose_range": {"z": (0.42, 0.42), "yaw": (-math.pi, math.pi)},
|
||||
"velocity_range": {"x": (-0.2, 0.2), "y": (-0.1, 0.1), "yaw": (-0.2, 0.2)},
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
@@ -416,15 +501,32 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
# ------------------
|
||||
# Rewards Integration
|
||||
# ------------------
|
||||
cfg.rewards["track_lin_vel"] = RewardTermCfg(
|
||||
func=track_linear_velocity,
|
||||
weight=3.0,
|
||||
params={"std": 0.5, "command_name": "twist"}
|
||||
cfg.rewards.pop("track_lin_vel", None)
|
||||
cfg.rewards.pop("track_ang_vel", None)
|
||||
cfg.rewards["track_lin_vel_x_exp"] = RewardTermCfg(
|
||||
func=track_linear_velocity_x,
|
||||
weight=1.0,
|
||||
params={"std": 0.25, "command_name": "twist"},
|
||||
)
|
||||
cfg.rewards["track_ang_vel"] = RewardTermCfg(
|
||||
func=track_angular_velocity,
|
||||
weight=1.5,
|
||||
params={"std": 0.5, "command_name": "twist"}
|
||||
cfg.rewards["track_lin_vel_y_exp"] = RewardTermCfg(
|
||||
func=track_linear_velocity_y,
|
||||
weight=1.0,
|
||||
params={"std": 0.25, "command_name": "twist"},
|
||||
)
|
||||
cfg.rewards["track_ang_vel_z_exp"] = RewardTermCfg(
|
||||
func=track_angular_velocity_z,
|
||||
weight=1.0,
|
||||
params={"std": 0.25, "command_name": "twist"},
|
||||
)
|
||||
cfg.rewards["stair_lateral_yaw_drift"] = RewardTermCfg(
|
||||
func=stair_lateral_yaw_drift_l2,
|
||||
weight=-1.0,
|
||||
params={
|
||||
"terrain_names": ("pyramid_stairs", "pyramid_stairs_inv", "random_grid"),
|
||||
"y_scale": 1.0,
|
||||
"yaw_scale": 1.0,
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
|
||||
cfg.rewards["lin_vel_z"] = RewardTermCfg(func=lin_vel_z_l2, weight=-2.0)
|
||||
@@ -455,8 +557,8 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
weight=-0.05,
|
||||
params={
|
||||
"mirror_joints": [
|
||||
["fl_(hip_abduction|hip_pitch|knee)_joint", "rr_(hip_abduction|hip_pitch|knee)_joint"],
|
||||
["fr_(hip_abduction|hip_pitch|knee)_joint", "rl_(hip_abduction|hip_pitch|knee)_joint"]
|
||||
["fl_(hip_pitch|knee)_joint", "rr_(hip_pitch|knee)_joint"],
|
||||
["fr_(hip_pitch|knee)_joint", "rl_(hip_pitch|knee)_joint"]
|
||||
],
|
||||
"asset_cfg": SceneEntityCfg("robot")
|
||||
}
|
||||
@@ -465,33 +567,61 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
# 移除 variable_posture 及其产生的静止奖励陷阱，换用极轻微的偏离惩罚
|
||||
cfg.rewards.pop("stand_still", None)
|
||||
cfg.rewards["stand_still"] = RewardTermCfg(func=stand_still, weight=-2.0, params={"command_name": "twist", "command_threshold": 0.1})
|
||||
|
||||
|
||||
cfg.rewards.pop("hip_deviation", None)
|
||||
cfg.rewards.pop("variable_posture", None)
|
||||
|
||||
|
||||
cfg.rewards.pop("joint_deviation_l2", None)
|
||||
cfg.rewards["joint_pos_penalty"] = RewardTermCfg(
|
||||
func=joint_pos_penalty,
|
||||
|
||||
# 针对 ab 关节施加较严厉的惩罚，防止在 yaw 时乱撇腿
|
||||
cfg.rewards["joint_pos_penalty_ab"] = RewardTermCfg(
|
||||
func=joint_pos_penalty,
|
||||
weight=-1.0,
|
||||
params={
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint", ".*_hip_pitch_joint", ".*_knee_joint")),
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint",)),
|
||||
"command_name": "twist"
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
# 针对 pitch 和 knee 关节施加较宽松的惩罚，保留跨越障碍的抬腿自由度
|
||||
cfg.rewards["joint_pos_penalty_sagittal"] = RewardTermCfg(
|
||||
func=joint_pos_penalty,
|
||||
weight=-0.3,
|
||||
params={
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_pitch_joint", ".*_knee_joint")),
|
||||
"command_name": "twist"
|
||||
}
|
||||
)
|
||||
|
||||
# 🌟 强力约束同侧外展关节平行对称，消除转向时的前后剪刀式摆动
|
||||
cfg.rewards["abduction_mirror"] = RewardTermCfg(
|
||||
func=joint_mirror,
|
||||
weight=-0.5, # 施加合理惩罚，限制前后腿同侧外展关节反向运动
|
||||
params={
|
||||
"mirror_joints": [
|
||||
["fl_hip_abduction_joint", "rl_hip_abduction_joint"],
|
||||
["fr_hip_abduction_joint", "rr_hip_abduction_joint"]
|
||||
],
|
||||
"asset_cfg": SceneEntityCfg("robot")
|
||||
}
|
||||
)
|
||||
|
||||
cfg.rewards["feet_contact_without_cmd"] = RewardTermCfg(
|
||||
func=feet_contact_without_cmd,
|
||||
weight=0.1,
|
||||
func=feet_contact_without_cmd,
|
||||
weight=0.1,
|
||||
params={"command_name": "twist", "sensor_name": "feet_ground_contact"}
|
||||
)
|
||||
cfg.rewards["feet_air_time"].weight = 0.0
|
||||
cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=1.0)
|
||||
cfg.rewards["feet_air_time"].weight = 0.15
|
||||
cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=0.5)
|
||||
|
||||
cfg.rewards["base_height_l2"].weight = 0.0
|
||||
cfg.rewards["base_height_l2"].params["target_height"] = 0.40
|
||||
cfg.rewards["base_height_l2"].params["target_height"] = 0.42
|
||||
cfg.rewards["base_height_l2"].params["sensor_cfg"] = SceneEntityCfg("height_scanner")
|
||||
|
||||
# 恢复机身碰撞惩罚为-1.0，逼迫机器人高抬腿跨越障碍，防止拖地
|
||||
@@ -515,6 +645,79 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
|
||||
cfg.scene.terrain.num_envs = 2048
|
||||
cfg.scene.terrain.env_spacing = 2.5
|
||||
|
||||
cfg.metrics.update(
|
||||
{
|
||||
"tracking_lin_vel_error": MetricsTermCfg(func=tracking_lin_vel_error, params={"command_name": "twist"}),
|
||||
"tracking_lin_vel_x_error": MetricsTermCfg(func=tracking_lin_vel_x_error, params={"command_name": "twist"}),
|
||||
"tracking_lin_vel_y_error": MetricsTermCfg(func=tracking_lin_vel_y_error, params={"command_name": "twist"}),
|
||||
"tracking_lin_vel_along_cmd_error": MetricsTermCfg(
|
||||
func=tracking_lin_vel_along_command_error, params={"command_name": "twist"}
|
||||
),
|
||||
"actual_lin_vel_orthogonal_cmd": MetricsTermCfg(
|
||||
func=actual_lin_vel_orthogonal_command_mean, params={"command_name": "twist"}
|
||||
),
|
||||
"tracking_yaw_vel_error": MetricsTermCfg(func=tracking_yaw_vel_error, params={"command_name": "twist"}),
|
||||
"cmd_lin_vel": MetricsTermCfg(func=command_lin_vel_mean, params={"command_name": "twist"}),
|
||||
"cmd_yaw_vel": MetricsTermCfg(func=command_yaw_vel_abs_mean, params={"command_name": "twist"}),
|
||||
"actual_lin_vel": MetricsTermCfg(func=actual_lin_vel_mean),
|
||||
"tracking_lin_vel_error_cmd_0_03": MetricsTermCfg(
|
||||
func=tracking_lin_vel_error_band_mean,
|
||||
params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
|
||||
),
|
||||
"tracking_lin_vel_error_cmd_03_07": MetricsTermCfg(
|
||||
func=tracking_lin_vel_error_band_mean,
|
||||
params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
|
||||
),
|
||||
"tracking_lin_vel_error_cmd_07_up": MetricsTermCfg(
|
||||
func=tracking_lin_vel_error_band_mean,
|
||||
params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
|
||||
),
|
||||
"tracking_lin_vel_x_error_cmd_0_03": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 0, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
|
||||
),
|
||||
"tracking_lin_vel_x_error_cmd_03_07": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 0, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
|
||||
),
|
||||
"tracking_lin_vel_x_error_cmd_07_up": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 0, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
|
||||
),
|
||||
"tracking_lin_vel_y_error_cmd_0_03": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 1, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
|
||||
),
|
||||
"tracking_lin_vel_y_error_cmd_03_07": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 1, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
|
||||
),
|
||||
"tracking_lin_vel_y_error_cmd_07_up": MetricsTermCfg(
|
||||
func=tracking_lin_vel_axis_error_band_mean,
|
||||
params={"axis": 1, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
|
||||
),
|
||||
"cmd_band_0_03": MetricsTermCfg(
|
||||
func=command_band_active, params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3}
|
||||
),
|
||||
"cmd_band_03_07": MetricsTermCfg(
|
||||
func=command_band_active, params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7}
|
||||
),
|
||||
"cmd_band_07_up": MetricsTermCfg(
|
||||
func=command_band_active, params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0}
|
||||
),
|
||||
"wheel_raw_action_abs": MetricsTermCfg(func=wheel_raw_action_abs_mean),
|
||||
"wheel_target_vel_abs": MetricsTermCfg(func=wheel_target_vel_abs_mean),
|
||||
"wheel_actual_vel_abs": MetricsTermCfg(func=wheel_actual_vel_abs_mean),
|
||||
"wheel_target_actual_vel_error": MetricsTermCfg(func=wheel_target_actual_vel_error_mean),
|
||||
"wheel_actual_to_target_vel_ratio": MetricsTermCfg(func=wheel_actual_to_target_vel_ratio_mean),
|
||||
"wheel_target_actual_sign_agreement": MetricsTermCfg(func=wheel_target_actual_sign_agreement),
|
||||
"upright": MetricsTermCfg(func=upright_metric),
|
||||
"base_ground_contact_rate": MetricsTermCfg(
|
||||
func=base_ground_contact_metric, params={"sensor_name": "base_ground_contact"}
|
||||
),
|
||||
}
|
||||
)
|
||||
|
||||
if play:
|
||||
cfg.episode_length_s = int(1e9)
|
||||
cfg.observations["actor"].enable_corruption = False
|
||||
|
||||
@@ -33,9 +33,9 @@ def rough_ppo_runner_cfg() -> RslRlOnPolicyRunnerCfg:
|
||||
value_loss_coef=1.0,
|
||||
use_clipped_value_loss=True,
|
||||
clip_param=0.2,
|
||||
entropy_coef=0.001,#第一轮为0.003
|
||||
num_learning_epochs=5,
|
||||
num_mini_batches=4,
|
||||
entropy_coef=0.003,#第一轮为0.003，第二轮0.001，第三轮0.0008
|
||||
num_learning_epochs=5,#2048为5 4096为3
|
||||
num_mini_batches=4,#2048为4 4096为8
|
||||
learning_rate=8.0e-4,
|
||||
schedule="adaptive",
|
||||
gamma=0.99,
|
||||
|
||||
@@ -21,14 +21,20 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
|
||||
def __init__(self, cfg: UniformThresholdVelocityCommandCfg, env: ManagerBasedRlEnv):
|
||||
super().__init__(cfg, env)
|
||||
self.is_lateral_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
|
||||
self.is_yaw_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
|
||||
self.was_climbing = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
|
||||
# 缓存地形类型的索引（台阶、反向台阶、垂直短墙），实现高容错动态查找
|
||||
self._climbing_indices = []
|
||||
self._flat_index = -1
|
||||
terrain = getattr(self._env.scene, "terrain", None)
|
||||
if terrain is not None and getattr(terrain.cfg, "terrain_generator", None) is not None:
|
||||
sub_terrain_names = list(terrain.cfg.terrain_generator.sub_terrains.keys())
|
||||
for name in ["pyramid_stairs", "pyramid_stairs_inv", "rc_wall"]:
|
||||
if name in sub_terrain_names:
|
||||
self._climbing_indices.append(sub_terrain_names.index(name))
|
||||
if "flat" in sub_terrain_names:
|
||||
self._flat_index = sub_terrain_names.index("flat")
|
||||
|
||||
def _resample_command(self, env_ids: torch.Tensor) -> None:
|
||||
# 1. 调用基类的标准采样
|
||||
@@ -42,6 +48,41 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
self.vel_command_b[small_cmd_ids, :] = 0.0
|
||||
self.vel_command_w[small_cmd_ids, :] = 0.0
|
||||
|
||||
# 重置并采样单 Y（只横移）与单 Z（只原地转向）指令分布
|
||||
self.is_lateral_env[env_ids] = False
|
||||
self.is_yaw_env[env_ids] = False
|
||||
|
||||
# 对没有设为前进且没有静止的激活环境进行单轴采样
|
||||
active_non_fwd_mask = (~self.is_forward_env[env_ids]) & (~self.is_standing_env[env_ids])
|
||||
active_non_fwd_ids = env_ids[active_non_fwd_mask]
|
||||
|
||||
if len(active_non_fwd_ids) > 0:
|
||||
r = torch.empty(len(active_non_fwd_ids), device=self.device)
|
||||
# 采样单 Y 占比
|
||||
self.is_lateral_env[active_non_fwd_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_lateral_envs
|
||||
lat_ids = active_non_fwd_ids[self.is_lateral_env[active_non_fwd_ids]]
|
||||
if len(lat_ids) > 0:
|
||||
self.vel_command_b[lat_ids, 0] = 0.0
|
||||
y_signs = self.vel_command_b[lat_ids, 1].sign()
|
||||
y_signs[y_signs == 0] = 1.0
|
||||
self.vel_command_b[lat_ids, 1] = y_signs * self.vel_command_b[lat_ids, 1].abs().clamp(min=0.3)
|
||||
self.vel_command_b[lat_ids, 2] = 0.0
|
||||
|
||||
# 对不是 forward 也不是 lateral 的环境采样单 Z 占比
|
||||
non_lat_mask = ~self.is_lateral_env[active_non_fwd_ids]
|
||||
non_lat_ids = active_non_fwd_ids[non_lat_mask]
|
||||
|
||||
if len(non_lat_ids) > 0:
|
||||
r_yaw = torch.empty(len(non_lat_ids), device=self.device)
|
||||
self.is_yaw_env[non_lat_ids] = r_yaw.uniform_(0.0, 1.0) <= self.cfg.rel_yaw_envs
|
||||
yaw_ids = non_lat_ids[self.is_yaw_env[non_lat_ids]]
|
||||
if len(yaw_ids) > 0:
|
||||
self.vel_command_b[yaw_ids, 0] = 0.0
|
||||
self.vel_command_b[yaw_ids, 1] = 0.0
|
||||
z_signs = self.vel_command_b[yaw_ids, 2].sign()
|
||||
z_signs[z_signs == 0] = 1.0
|
||||
self.vel_command_b[yaw_ids, 2] = z_signs * self.vel_command_b[yaw_ids, 2].abs().clamp(min=0.3)
|
||||
|
||||
# 3. 地形自适应重采样限制：若在爬行地形，强制纯前进方向且速度 >= 0.3 m/s
|
||||
terrain = getattr(self._env.scene, "terrain", None)
|
||||
terrain_types = getattr(terrain, "terrain_types", None)
|
||||
@@ -58,6 +99,37 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
self.vel_command_b[climbing_env_ids, 2] = 0.0
|
||||
self.is_heading_env[climbing_env_ids] = True
|
||||
|
||||
# 4. 在平地（flat）地形上生成 35% 纯 Y 和 35% 纯 Z 指令，排除静止环境
|
||||
if terrain_types is not None and self._flat_index != -1:
|
||||
is_flat = terrain_types == self._flat_index
|
||||
flat_env_ids = env_ids[is_flat[env_ids]]
|
||||
if len(flat_env_ids) > 0:
|
||||
active_flat_mask = ~self.is_standing_env[flat_env_ids]
|
||||
active_flat_ids = flat_env_ids[active_flat_mask]
|
||||
if len(active_flat_ids) > 0:
|
||||
r = torch.empty(len(active_flat_ids), device=self.device).uniform_(0.0, 1.0)
|
||||
y_only_mask = r < 0.35
|
||||
z_only_mask = (r >= 0.35) & (r < 0.70)
|
||||
|
||||
y_only_ids = active_flat_ids[y_only_mask]
|
||||
if len(y_only_ids) > 0:
|
||||
self.vel_command_b[y_only_ids, 0] = 0.0 # x = 0
|
||||
self.vel_command_b[y_only_ids, 2] = 0.0 # yaw = 0
|
||||
if self.cfg.heading_command:
|
||||
self.heading_target[y_only_ids] = self.robot.data.heading_w[y_only_ids]
|
||||
self.is_heading_env[y_only_ids] = True
|
||||
|
||||
z_only_ids = active_flat_ids[z_only_mask]
|
||||
if len(z_only_ids) > 0:
|
||||
self.vel_command_b[z_only_ids, 0] = 0.0 # x = 0
|
||||
self.vel_command_b[z_only_ids, 1] = 0.0 # y = 0
|
||||
if self.cfg.heading_command:
|
||||
yaw_delta = torch.empty(len(z_only_ids), device=self.device).uniform_(
|
||||
-self.cfg.yaw_only_heading_range, self.cfg.yaw_only_heading_range
|
||||
)
|
||||
self.heading_target[z_only_ids] = self.robot.data.heading_w[z_only_ids] + yaw_delta
|
||||
self.is_heading_env[z_only_ids] = True
|
||||
|
||||
def _update_command(self) -> None:
|
||||
# 调用基类的每步更新
|
||||
super()._update_command()
|
||||
@@ -69,7 +141,14 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
is_climbing = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
|
||||
for idx in self._climbing_indices:
|
||||
is_climbing |= (terrain_types == idx)
|
||||
|
||||
|
||||
# 【出台阶实时重采样】：检查从爬坡状态刚刚离开环境的机器人，进行即时指令重采样
|
||||
left_climbing_mask = self.was_climbing & ~is_climbing
|
||||
if left_climbing_mask.any():
|
||||
left_climbing_ids = torch.where(left_climbing_mask)[0]
|
||||
self._resample_command(left_climbing_ids)
|
||||
|
||||
# 【入台阶实时截断】：对于正在爬楼梯/翻越短墙的环境，强行截断其横向横移指令，并进行温和偏航对齐
|
||||
climbing_env_ids = is_climbing.nonzero(as_tuple=False).flatten()
|
||||
if len(climbing_env_ids) > 0:
|
||||
self.vel_command_b[climbing_env_ids, 1] = 0.0
|
||||
@@ -79,6 +158,7 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
min=-0.3,
|
||||
max=0.3
|
||||
)
|
||||
self.was_climbing = is_climbing
|
||||
|
||||
# 5. 高速侧向解耦（适用于平地/斜坡等混合路面）：当前进速度 >= 0.8 m/s 时，清空侧向指令，防止高速甩尾甩飞
|
||||
high_speed_mask = self.vel_command_b[:, 0].abs() >= 0.8
|
||||
@@ -90,3 +170,6 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
|
||||
@dataclass(kw_only=True)
|
||||
class UniformThresholdVelocityCommandCfg(UniformVelocityCommandCfg):
|
||||
class_type: type = UniformThresholdVelocityCommand
|
||||
yaw_only_heading_range: float = 1.0
|
||||
rel_lateral_envs: float = 0.0
|
||||
rel_yaw_envs: float = 0.0
|
||||
|
||||
@@ -99,6 +99,165 @@ class adaptive_command_vel:
|
||||
}
|
||||
|
||||
|
||||
class command_axis_levels_vel:
|
||||
"""Linearly expand one command axis over training steps."""
|
||||
|
||||
def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
|
||||
from mjlab.tasks.velocity.mdp import UniformVelocityCommandCfg
|
||||
|
||||
p = cfg.params
|
||||
self._command_name: str = p.get("command_name", "twist")
|
||||
self._reward_name: str = p["reward_term_name"]
|
||||
self._axis: str = p["axis"]
|
||||
self._range_multiplier: tuple[float, float] = tuple(p.get("range_multiplier", (0.4, 1.0)))
|
||||
self._initial_range_param: tuple[float, float] | None = (
|
||||
tuple(p["initial_range"]) if "initial_range" in p else None
|
||||
)
|
||||
self._ema_alpha: float = p.get("ema_alpha", 0.5)
|
||||
self._warmup_steps: int = p.get("warmup_steps", 0)
|
||||
self._ramp_steps: int = p.get("ramp_steps", 800 * 24)
|
||||
|
||||
command_term = env.command_manager.get_term(self._command_name)
|
||||
self._cfg = cast(UniformVelocityCommandCfg, command_term.cfg)
|
||||
self._reward_idx = list(env.reward_manager._term_names).index(self._reward_name)
|
||||
self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
|
||||
self._running_mean = 0.0
|
||||
|
||||
self._full_range = self._get_range()
|
||||
self._min_range = self._initial_range_param or self._scaled_range(self._range_multiplier[0])
|
||||
self._set_range(self._min_range)
|
||||
|
||||
def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
|
||||
if len(env_ids) > 0:
|
||||
episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
|
||||
mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
|
||||
self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
|
||||
self._set_range(self._current_max_range(env.common_step_counter))
|
||||
|
||||
lo, hi = self._get_range()
|
||||
return {
|
||||
f"{self._axis}_range_min": torch.tensor(lo),
|
||||
f"{self._axis}_range_max": torch.tensor(hi),
|
||||
f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
|
||||
f"{self._axis}_warmup_active": torch.tensor(float(env.common_step_counter < self._warmup_steps)),
|
||||
f"{self._axis}_range_cap": torch.tensor(self._current_multiplier(env.common_step_counter)),
|
||||
}
|
||||
|
||||
def _get_range(self) -> tuple[float, float]:
|
||||
if self._axis == "x":
|
||||
return tuple(self._cfg.ranges.lin_vel_x)
|
||||
if self._axis == "y":
|
||||
return tuple(self._cfg.ranges.lin_vel_y)
|
||||
if self._axis == "yaw":
|
||||
return tuple(self._cfg.ranges.ang_vel_z)
|
||||
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
|
||||
|
||||
def _set_range(self, value: tuple[float, float]) -> None:
|
||||
if self._axis == "x":
|
||||
self._cfg.ranges.lin_vel_x = value
|
||||
elif self._axis == "y":
|
||||
self._cfg.ranges.lin_vel_y = value
|
||||
elif self._axis == "yaw":
|
||||
self._cfg.ranges.ang_vel_z = value
|
||||
else:
|
||||
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
|
||||
|
||||
def _scaled_range(self, multiplier: float) -> tuple[float, float]:
|
||||
lo, hi = self._full_range
|
||||
return (lo * multiplier, hi * multiplier)
|
||||
|
||||
def _current_multiplier(self, step: int) -> float:
|
||||
start, end = self._range_multiplier
|
||||
if self._ramp_steps <= 0:
|
||||
return end
|
||||
progress = max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
|
||||
return start + (end - start) * progress
|
||||
|
||||
def _current_max_range(self, step: int) -> tuple[float, float]:
|
||||
if self._initial_range_param is None:
|
||||
return self._scaled_range(self._current_multiplier(step))
|
||||
progress = self._current_progress(step)
|
||||
lo = self._min_range[0] + (self._full_range[0] - self._min_range[0]) * progress
|
||||
hi = self._min_range[1] + (self._full_range[1] - self._min_range[1]) * progress
|
||||
return (lo, hi)
|
||||
|
||||
def _current_progress(self, step: int) -> float:
|
||||
if self._ramp_steps <= 0:
|
||||
return 1.0
|
||||
return max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
|
||||
|
||||
|
||||
class command_levels_adaptive:
|
||||
"""Adaptive command range curriculum based on average tracking performance."""
|
||||
|
||||
def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
|
||||
p = cfg.params
|
||||
self._command_name: str = p.get("command_name", "twist")
|
||||
self._reward_name: str = p["reward_term_name"]
|
||||
self._axis: str = p["axis"]
|
||||
self._delta_command: float = p.get("delta_command", 0.05)
|
||||
self._target_ratio: float = p.get("target_ratio", 0.8)
|
||||
self._ema_alpha: float = p.get("ema_alpha", 0.5)
|
||||
|
||||
command_term = env.command_manager.get_term(self._command_name)
|
||||
self._cfg = command_term.cfg
|
||||
self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
|
||||
self._running_mean = 0.0
|
||||
|
||||
# Read the full range configured in the environment configuration
|
||||
self._full_range = self._get_range()
|
||||
|
||||
# Set the command range to initial range at the start
|
||||
self._initial_range = list(p["initial_range"]) # e.g. [-0.5, 0.5]
|
||||
self._current_range = list(self._initial_range)
|
||||
self._set_range(self._current_range)
|
||||
|
||||
def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
|
||||
if len(env_ids) > 0:
|
||||
episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
|
||||
mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
|
||||
self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
|
||||
|
||||
# Check performance at the end of every episode (or every max_episode_length_s)
|
||||
episode_length_steps = int(env.cfg.episode_length_s / env.step_dt)
|
||||
if env.common_step_counter > 0 and env.common_step_counter % episode_length_steps == 0:
|
||||
# If performance exceeds target ratio (e.g., 0.8), widen the range
|
||||
if self._running_mean > self._target_ratio:
|
||||
# Widen the range
|
||||
lo, hi = self._current_range
|
||||
new_lo = max(self._full_range[0], lo - self._delta_command)
|
||||
new_hi = min(self._full_range[1], hi + self._delta_command)
|
||||
self._current_range = [new_lo, new_hi]
|
||||
self._set_range(self._current_range)
|
||||
|
||||
lo, hi = self._current_range
|
||||
return {
|
||||
f"{self._axis}_range_min": torch.tensor(lo),
|
||||
f"{self._axis}_range_max": torch.tensor(hi),
|
||||
f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
|
||||
f"{self._axis}_target_ratio": torch.tensor(self._target_ratio),
|
||||
}
|
||||
|
||||
def _get_range(self) -> tuple[float, float]:
|
||||
if self._axis == "x":
|
||||
return tuple(self._cfg.ranges.lin_vel_x)
|
||||
if self._axis == "y":
|
||||
return tuple(self._cfg.ranges.lin_vel_y)
|
||||
if self._axis == "yaw":
|
||||
return tuple(self._cfg.ranges.ang_vel_z)
|
||||
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
|
||||
|
||||
def _set_range(self, value: list[float]) -> None:
|
||||
if self._axis == "x":
|
||||
self._cfg.ranges.lin_vel_x = tuple(value)
|
||||
elif self._axis == "y":
|
||||
self._cfg.ranges.lin_vel_y = tuple(value)
|
||||
elif self._axis == "yaw":
|
||||
self._cfg.ranges.ang_vel_z = tuple(value)
|
||||
else:
|
||||
raise ValueError(f"Unknown command curriculum axis: {self._axis}")
|
||||
|
||||
|
||||
def terrain_levels_vel_strict(
|
||||
env: ManagerBasedRlEnv,
|
||||
env_ids: torch.Tensor,
|
||||
@@ -163,3 +322,221 @@ def terrain_levels_vel_strict(
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def terrain_levels_ramp_strict(
|
||||
env: ManagerBasedRlEnv,
|
||||
env_ids: torch.Tensor,
|
||||
command_name: str,
|
||||
ramp_steps: int = 50 * 24,
|
||||
move_up_expected_distance_ratio: float = 0.60,
|
||||
move_down_distance_ratio: float = 0.50,
|
||||
min_command_speed: float = 0.15,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> dict[str, torch.Tensor]:
|
||||
"""Terrain curriculum active from start, with strict promotion and time-ramped max level."""
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
terrain = env.scene.terrain
|
||||
assert terrain is not None
|
||||
terrain_generator = terrain.cfg.terrain_generator
|
||||
assert terrain_generator is not None
|
||||
assert terrain.terrain_origins is not None
|
||||
assert terrain.env_origins is not None
|
||||
|
||||
command = env.command_manager.get_command(command_name)
|
||||
assert command is not None
|
||||
|
||||
distance = torch.norm(
|
||||
asset.data.root_link_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2],
|
||||
dim=1,
|
||||
)
|
||||
cmd_speed = torch.norm(command[env_ids, :2], dim=1)
|
||||
active_command = cmd_speed >= min_command_speed
|
||||
|
||||
expected_distance = cmd_speed * env.max_episode_length_s
|
||||
move_up = (distance > expected_distance * move_up_expected_distance_ratio) & active_command
|
||||
move_down = (
|
||||
(distance < expected_distance * move_down_distance_ratio)
|
||||
& active_command
|
||||
& ~move_up
|
||||
)
|
||||
|
||||
terrain.terrain_levels[env_ids] += 1 * move_up - 1 * move_down
|
||||
|
||||
max_level = max(int(terrain.max_terrain_level) - 1, 0)
|
||||
if ramp_steps <= 0:
|
||||
level_cap = max_level
|
||||
else:
|
||||
progress = max(0.0, min(1.0, env.common_step_counter / ramp_steps))
|
||||
level_cap = int(round(progress * max_level))
|
||||
terrain.terrain_levels[env_ids] = torch.clamp(
|
||||
terrain.terrain_levels[env_ids],
|
||||
min=0,
|
||||
max=min(level_cap, max_level),
|
||||
)
|
||||
|
||||
terrain.env_origins[env_ids] = terrain.terrain_origins[
|
||||
terrain.terrain_levels[env_ids], terrain.terrain_types[env_ids]
|
||||
]
|
||||
|
||||
levels = terrain.terrain_levels.float()
|
||||
result: dict[str, torch.Tensor] = {
|
||||
"mean": torch.mean(levels),
|
||||
"max": torch.max(levels),
|
||||
"level_cap": torch.tensor(float(level_cap), device=env.device),
|
||||
}
|
||||
|
||||
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
|
||||
num_cols = terrain.terrain_origins.shape[1]
|
||||
if num_cols == len(sub_terrain_names):
|
||||
types = terrain.terrain_types
|
||||
for i, name in enumerate(sub_terrain_names):
|
||||
mask = types == i
|
||||
if mask.any():
|
||||
result[name] = torch.mean(levels[mask])
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def terrain_levels_flat_warmup(
|
||||
env: ManagerBasedRlEnv,
|
||||
env_ids: torch.Tensor,
|
||||
command_name: str,
|
||||
warmup_steps: int = 4800,
|
||||
flat_terrain_name: str = "flat",
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> dict[str, torch.Tensor]:
|
||||
"""Keep all reset envs on flat level 0 before enabling terrain curriculum."""
|
||||
terrain = env.scene.terrain
|
||||
assert terrain is not None
|
||||
terrain_generator = terrain.cfg.terrain_generator
|
||||
assert terrain_generator is not None
|
||||
assert terrain.terrain_origins is not None
|
||||
assert terrain.env_origins is not None
|
||||
|
||||
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
|
||||
flat_type = sub_terrain_names.index(flat_terrain_name) if flat_terrain_name in sub_terrain_names else 0
|
||||
|
||||
if env.common_step_counter < warmup_steps:
|
||||
terrain.terrain_levels[env_ids] = 0
|
||||
terrain.terrain_types[env_ids] = flat_type
|
||||
terrain.env_origins[env_ids] = terrain.terrain_origins[0, flat_type]
|
||||
if not hasattr(terrain, "_flat_warmup_env_released"):
|
||||
terrain._flat_warmup_env_released = torch.zeros(
|
||||
env.num_envs, dtype=torch.bool, device=env.device
|
||||
)
|
||||
terrain._flat_warmup_env_released[env_ids] = False
|
||||
|
||||
levels = terrain.terrain_levels.float()
|
||||
result: dict[str, torch.Tensor] = {
|
||||
"mean": torch.mean(levels),
|
||||
"max": torch.max(levels),
|
||||
"warmup_active": torch.ones((), device=env.device),
|
||||
}
|
||||
for i, name in enumerate(sub_terrain_names):
|
||||
mask = terrain.terrain_types == i
|
||||
if mask.any():
|
||||
result[name] = torch.mean(levels[mask])
|
||||
return result
|
||||
|
||||
if not hasattr(terrain, "_flat_warmup_released"):
|
||||
terrain._flat_warmup_released = True
|
||||
terrain._flat_warmup_release_counts = 0
|
||||
if not hasattr(terrain, "_flat_warmup_env_released"):
|
||||
terrain._flat_warmup_env_released = torch.zeros(
|
||||
env.num_envs, dtype=torch.bool, device=env.device
|
||||
)
|
||||
|
||||
newly_released = env_ids[~terrain._flat_warmup_env_released[env_ids]]
|
||||
if len(newly_released) > 0:
|
||||
proportions = torch.tensor(
|
||||
[sub.proportion for sub in terrain_generator.sub_terrains.values()],
|
||||
device=env.device,
|
||||
dtype=torch.float,
|
||||
)
|
||||
proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
|
||||
terrain.terrain_types[newly_released] = torch.multinomial(
|
||||
proportions, len(newly_released), replacement=True
|
||||
)
|
||||
terrain.terrain_levels[newly_released] = 0
|
||||
terrain.env_origins[newly_released] = terrain.terrain_origins[
|
||||
terrain.terrain_levels[newly_released], terrain.terrain_types[newly_released]
|
||||
]
|
||||
terrain._flat_warmup_env_released[newly_released] = True
|
||||
terrain._flat_warmup_release_counts += len(newly_released)
|
||||
|
||||
result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
|
||||
result["warmup_active"] = torch.zeros((), device=env.device)
|
||||
result["released_envs"] = torch.tensor(float(getattr(terrain, "_flat_warmup_release_counts", 0)), device=env.device)
|
||||
return result
|
||||
|
||||
|
||||
def terrain_levels_obstacle_release(
|
||||
env: ManagerBasedRlEnv,
|
||||
env_ids: torch.Tensor,
|
||||
command_name: str,
|
||||
release_schedule: tuple[tuple[int, tuple[str, ...]], ...],
|
||||
initial_terrain_names: tuple[str, ...] = ("flat", "random_rough", "perlin_noise", "sloped_terrain"),
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> dict[str, torch.Tensor]:
|
||||
"""Release obstacle terrain types gradually while keeping standard level progression."""
|
||||
terrain = env.scene.terrain
|
||||
assert terrain is not None
|
||||
terrain_generator = terrain.cfg.terrain_generator
|
||||
assert terrain_generator is not None
|
||||
assert terrain.terrain_origins is not None
|
||||
assert terrain.env_origins is not None
|
||||
|
||||
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
|
||||
allowed_names = list(initial_terrain_names)
|
||||
for step, names in release_schedule:
|
||||
if env.common_step_counter >= step:
|
||||
allowed_names.extend(names)
|
||||
allowed_type_ids = [
|
||||
sub_terrain_names.index(name) for name in allowed_names if name in sub_terrain_names
|
||||
]
|
||||
if not allowed_type_ids:
|
||||
allowed_type_ids = [0]
|
||||
|
||||
if not hasattr(terrain, "_obstacle_release_env_allowed"):
|
||||
terrain._obstacle_release_env_allowed = torch.zeros(
|
||||
env.num_envs, dtype=torch.bool, device=env.device
|
||||
)
|
||||
terrain._last_allowed_count = len(allowed_type_ids)
|
||||
|
||||
# If new terrains were released, force all envs to eventually resample upon their next reset
|
||||
if len(allowed_type_ids) > terrain._last_allowed_count:
|
||||
terrain._obstacle_release_env_allowed.fill_(False)
|
||||
terrain._last_allowed_count = len(allowed_type_ids)
|
||||
|
||||
allowed_tensor = torch.tensor(allowed_type_ids, dtype=torch.long, device=env.device)
|
||||
current_allowed = torch.isin(terrain.terrain_types[env_ids], allowed_tensor)
|
||||
need_resample = env_ids[~terrain._obstacle_release_env_allowed[env_ids] | ~current_allowed]
|
||||
|
||||
if len(need_resample) > 0:
|
||||
proportions = torch.tensor(
|
||||
[terrain_generator.sub_terrains[sub_terrain_names[i]].proportion for i in allowed_type_ids],
|
||||
device=env.device,
|
||||
dtype=torch.float,
|
||||
)
|
||||
proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
|
||||
sampled = allowed_tensor[torch.multinomial(proportions, len(need_resample), replacement=True)]
|
||||
|
||||
# Check which envs actually changed terrain type
|
||||
changed_mask = terrain.terrain_types[need_resample] != sampled
|
||||
changed_envs = need_resample[changed_mask]
|
||||
|
||||
terrain.terrain_types[need_resample] = sampled
|
||||
# Only reset the level to 0 if the terrain type was actually changed
|
||||
if len(changed_envs) > 0:
|
||||
terrain.terrain_levels[changed_envs] = 0
|
||||
|
||||
terrain.env_origins[need_resample] = terrain.terrain_origins[
|
||||
terrain.terrain_levels[need_resample], terrain.terrain_types[need_resample]
|
||||
]
|
||||
terrain._obstacle_release_env_allowed[need_resample] = True
|
||||
|
||||
result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
|
||||
result["allowed_types"] = torch.tensor(float(len(allowed_type_ids)), device=env.device)
|
||||
for name in ("pyramid_stairs", "pyramid_stairs_inv", "random_grid", "rc_wall"):
|
||||
result[f"{name}_released"] = torch.tensor(float(name in allowed_names), device=env.device)
|
||||
return result
|
||||
|
||||
@@ -14,6 +14,11 @@ if TYPE_CHECKING:
|
||||
from mjlab.envs import ManagerBasedRlEnv
|
||||
|
||||
|
||||
def _finite(value: torch.Tensor, nan: float = 0.0, posinf: float = 0.0, neginf: float = 0.0) -> torch.Tensor:
|
||||
"""Keep diagnostic metrics finite when a terminating env has invalid physics state."""
|
||||
return torch.nan_to_num(value, nan=nan, posinf=posinf, neginf=neginf)
|
||||
|
||||
|
||||
def track_linear_velocity(
|
||||
env: ManagerBasedRlEnv,
|
||||
std: float,
|
||||
@@ -32,6 +37,69 @@ def track_linear_velocity(
|
||||
return reward
|
||||
|
||||
|
||||
def track_linear_velocity_x(
|
||||
env: ManagerBasedRlEnv,
|
||||
std: float,
|
||||
command_name: str,
|
||||
gravity_z_power: float | None = None,
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""LocoLeggedWheel-style independent x velocity tracking reward."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
command = env.command_manager.get_command(command_name)
|
||||
x_error = torch.square(command[:, 0] - asset.data.root_link_lin_vel_b[:, 0])
|
||||
reward = torch.exp(-x_error / std**2)
|
||||
if gravity_z_power is not None:
|
||||
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
|
||||
else:
|
||||
reward *= -asset.data.projected_gravity_b[:, 2]
|
||||
return reward
|
||||
|
||||
|
||||
def track_linear_velocity_y(
|
||||
env: ManagerBasedRlEnv,
|
||||
std: float,
|
||||
command_name: str,
|
||||
gravity_z_power: float | None = None,
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""LocoLeggedWheel-style independent y velocity tracking reward."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
command = env.command_manager.get_command(command_name)
|
||||
y_error = torch.square(command[:, 1] - asset.data.root_link_lin_vel_b[:, 1])
|
||||
reward = torch.exp(-y_error / std**2)
|
||||
if gravity_z_power is not None:
|
||||
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
|
||||
else:
|
||||
reward *= -asset.data.projected_gravity_b[:, 2]
|
||||
return reward
|
||||
|
||||
|
||||
def track_angular_velocity_z(
|
||||
env: ManagerBasedRlEnv,
|
||||
std: float,
|
||||
command_name: str,
|
||||
gravity_z_power: float | None = None,
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""LocoLeggedWheel-style independent yaw velocity tracking reward."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
command = env.command_manager.get_command(command_name)
|
||||
z_error = torch.square(command[:, 2] - asset.data.root_link_ang_vel_b[:, 2])
|
||||
reward = torch.exp(-z_error / std**2)
|
||||
if gravity_z_power is not None:
|
||||
reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
|
||||
else:
|
||||
reward *= -asset.data.projected_gravity_b[:, 2]
|
||||
return reward
|
||||
|
||||
|
||||
def track_angular_velocity(
|
||||
env: ManagerBasedRlEnv,
|
||||
std: float,
|
||||
@@ -50,6 +118,37 @@ def track_angular_velocity(
|
||||
return reward
|
||||
|
||||
|
||||
def stair_lateral_yaw_drift_l2(
|
||||
env: ManagerBasedRlEnv,
|
||||
terrain_names: tuple[str, ...] = ("pyramid_stairs", "pyramid_stairs_inv"),
|
||||
y_scale: float = 1.0,
|
||||
yaw_scale: float = 1.0,
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Penalize sideways velocity and yaw-rate drift only on stair terrains."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset: Entity = env.scene[asset_cfg.name]
|
||||
|
||||
terrain = getattr(env.scene, "terrain", None)
|
||||
terrain_types = getattr(terrain, "terrain_types", None)
|
||||
terrain_cfg = getattr(terrain, "cfg", None)
|
||||
terrain_generator = getattr(terrain_cfg, "terrain_generator", None)
|
||||
if terrain_types is None or terrain_generator is None:
|
||||
return torch.zeros(env.num_envs, device=env.device)
|
||||
|
||||
sub_terrain_names = list(terrain_generator.sub_terrains.keys())
|
||||
mask = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device)
|
||||
for name in terrain_names:
|
||||
if name in sub_terrain_names:
|
||||
mask |= terrain_types == sub_terrain_names.index(name)
|
||||
|
||||
y_vel = asset.data.root_link_lin_vel_b[:, 1]
|
||||
yaw_vel = asset.data.root_link_ang_vel_b[:, 2]
|
||||
penalty = y_scale * torch.square(y_vel) + yaw_scale * torch.square(yaw_vel)
|
||||
return _finite(torch.where(mask, penalty, torch.zeros_like(penalty)))
|
||||
|
||||
|
||||
def base_height_l2(
|
||||
env: ManagerBasedRlEnv,
|
||||
target_height: float = 0.36,
|
||||
@@ -809,3 +908,238 @@ def pitch_control_penalty(env, max_pitch_rad: float = 0.50, asset_cfg=None) -> t
|
||||
excessive_pitch = torch.clamp(torch.abs(g_x) - g_x_threshold, min=0.0)
|
||||
reward = torch.square(excessive_pitch)
|
||||
return reward
|
||||
|
||||
|
||||
def tracking_lin_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
|
||||
"""Current-step xy velocity tracking error."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
|
||||
|
||||
|
||||
def tracking_yaw_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
|
||||
"""Current-step yaw velocity tracking error."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return _finite(torch.abs(asset.data.root_link_ang_vel_b[:, 2] - cmd[:, 2]))
|
||||
|
||||
|
||||
def tracking_lin_vel_x_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
|
||||
"""Current-step x velocity tracking error."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 0] - cmd[:, 0]))
|
||||
|
||||
|
||||
def tracking_lin_vel_y_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
|
||||
"""Current-step y velocity tracking error."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 1] - cmd[:, 1]))
|
||||
|
||||
|
||||
def tracking_lin_vel_along_command_error(
|
||||
env,
|
||||
command_name: str = "twist",
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Error of velocity projected onto the commanded xy direction."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
cmd_xy = cmd[:, :2]
|
||||
cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
|
||||
direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
|
||||
actual_along = torch.sum(asset.data.root_link_lin_vel_b[:, :2] * direction, dim=1)
|
||||
return _finite(torch.abs(actual_along - cmd_speed))
|
||||
|
||||
|
||||
def actual_lin_vel_orthogonal_command_mean(
|
||||
env,
|
||||
command_name: str = "twist",
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Absolute velocity component perpendicular to the commanded xy direction."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
cmd_xy = cmd[:, :2]
|
||||
cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
|
||||
direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
|
||||
actual = asset.data.root_link_lin_vel_b[:, :2]
|
||||
actual_along = torch.sum(actual * direction, dim=1, keepdim=True) * direction
|
||||
orthogonal = actual - actual_along
|
||||
return _finite(torch.linalg.norm(orthogonal, dim=1))
|
||||
|
||||
|
||||
def command_lin_vel_mean(env, command_name: str = "twist") -> torch.Tensor:
|
||||
"""Current commanded xy speed magnitude."""
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return torch.linalg.norm(cmd[:, :2], dim=1)
|
||||
|
||||
|
||||
def command_yaw_vel_abs_mean(env, command_name: str = "twist") -> torch.Tensor:
|
||||
"""Current commanded yaw speed magnitude."""
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
return torch.abs(cmd[:, 2])
|
||||
|
||||
|
||||
def actual_lin_vel_mean(env, asset_cfg=None) -> torch.Tensor:
|
||||
"""Current actual xy speed magnitude."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2], dim=1))
|
||||
|
||||
|
||||
def tracking_lin_vel_error_band_mean(
|
||||
env,
|
||||
command_name: str = "twist",
|
||||
min_speed: float = 0.0,
|
||||
max_speed: float = 10.0,
|
||||
asset_cfg=None,
|
||||
) -> torch.Tensor:
|
||||
"""Broadcast the masked mean xy error for a command speed band."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
speed = torch.linalg.norm(cmd[:, :2], dim=1)
|
||||
err = _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
|
||||
active = torch.logical_and(speed >= min_speed, speed < max_speed)
|
||||
denom = active.float().sum().clamp_min(1.0)
|
||||
mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
|
||||
return torch.full_like(err, mean_err)
|
||||
|
||||
|
||||
def tracking_lin_vel_axis_error_band_mean(
|
||||
env,
|
||||
axis: int,
|
||||
command_name: str = "twist",
|
||||
min_speed: float = 0.0,
|
||||
max_speed: float = 10.0,
|
||||
asset_cfg=None,
|
||||
) -> torch.Tensor:
|
||||
"""Broadcast the masked mean x/y error for a command speed band."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
speed = torch.linalg.norm(cmd[:, :2], dim=1)
|
||||
err = _finite(torch.abs(asset.data.root_link_lin_vel_b[:, axis] - cmd[:, axis]))
|
||||
active = torch.logical_and(speed >= min_speed, speed < max_speed)
|
||||
denom = active.float().sum().clamp_min(1.0)
|
||||
mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
|
||||
return torch.full_like(err, mean_err)
|
||||
|
||||
|
||||
def command_band_active(
|
||||
env,
|
||||
command_name: str = "twist",
|
||||
min_speed: float = 0.0,
|
||||
max_speed: float = 10.0,
|
||||
) -> torch.Tensor:
|
||||
"""Fraction helper for command speed bands."""
|
||||
cmd = env.command_manager.get_command(command_name)
|
||||
speed = torch.linalg.norm(cmd[:, :2], dim=1)
|
||||
return torch.logical_and(speed >= min_speed, speed < max_speed).float()
|
||||
|
||||
|
||||
def wheel_raw_action_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
|
||||
"""Mean absolute raw wheel action."""
|
||||
action = env.action_manager.get_term(action_name).raw_action
|
||||
return torch.mean(torch.abs(action), dim=1)
|
||||
|
||||
|
||||
def wheel_target_vel_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
|
||||
"""Mean absolute processed wheel velocity target."""
|
||||
term = env.action_manager.get_term(action_name)
|
||||
target = getattr(term, "_processed_actions")
|
||||
return torch.mean(torch.abs(target), dim=1)
|
||||
|
||||
|
||||
def wheel_actual_vel_abs_mean(env, asset_cfg: SceneEntityCfg | None = None) -> torch.Tensor:
|
||||
"""Mean absolute actual wheel joint velocity."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
|
||||
asset = env.scene[asset_cfg.name]
|
||||
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
|
||||
return _finite(torch.mean(torch.abs(asset.data.joint_vel[:, joint_ids]), dim=1))
|
||||
|
||||
|
||||
def wheel_target_actual_vel_error_mean(
|
||||
env,
|
||||
action_name: str = "wheel_joint_vel",
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Mean absolute error between processed wheel target and actual wheel velocity."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
|
||||
term = env.action_manager.get_term(action_name)
|
||||
target = getattr(term, "_processed_actions")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
|
||||
actual = asset.data.joint_vel[:, joint_ids]
|
||||
return _finite(torch.mean(torch.abs(target - actual), dim=1))
|
||||
|
||||
|
||||
def wheel_actual_to_target_vel_ratio_mean(
|
||||
env,
|
||||
action_name: str = "wheel_joint_vel",
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Mean |actual wheel velocity| / |target wheel velocity|, clipped for readable logs."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
|
||||
term = env.action_manager.get_term(action_name)
|
||||
target = getattr(term, "_processed_actions")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
|
||||
actual = asset.data.joint_vel[:, joint_ids]
|
||||
ratio = torch.abs(actual) / torch.clamp(torch.abs(target), min=0.1)
|
||||
return _finite(torch.mean(torch.clamp(ratio, max=3.0), dim=1))
|
||||
|
||||
|
||||
def wheel_target_actual_sign_agreement(
|
||||
env,
|
||||
action_name: str = "wheel_joint_vel",
|
||||
asset_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Fraction of wheel targets and actual velocities with matching sign."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
|
||||
term = env.action_manager.get_term(action_name)
|
||||
target = getattr(term, "_processed_actions")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
|
||||
actual = asset.data.joint_vel[:, joint_ids]
|
||||
active = torch.abs(target) > 0.1
|
||||
same_sign = torch.sign(target) == torch.sign(actual)
|
||||
return torch.sum((active & same_sign).float(), dim=1) / torch.clamp(torch.sum(active.float(), dim=1), min=1.0)
|
||||
|
||||
|
||||
def upright_metric(env, asset_cfg=None) -> torch.Tensor:
|
||||
"""1 means upright, 0 means fully inverted according to projected gravity."""
|
||||
if asset_cfg is None:
|
||||
asset_cfg = SceneEntityCfg("robot")
|
||||
asset = env.scene[asset_cfg.name]
|
||||
return torch.clamp(-asset.data.projected_gravity_b[:, 2], 0.0, 1.0)
|
||||
|
||||
|
||||
def base_ground_contact_metric(env, sensor_name: str) -> torch.Tensor:
|
||||
"""Per-step base contact flag for diagnosing reset-biased metrics."""
|
||||
sensor = env.scene[sensor_name]
|
||||
contact = sensor.data.found > 0
|
||||
while contact.ndim > 1:
|
||||
contact = torch.any(contact, dim=-1)
|
||||
return contact.float()
|
||||
|
||||
@@ -53,11 +53,11 @@ WHEEL_ACTUATOR_CFG = BuiltinVelocityActuatorCfg(
|
||||
)
|
||||
|
||||
INIT_STATE = EntityCfg.InitialStateCfg(
|
||||
pos=(0.0, 0.0, 0.40),
|
||||
pos=(0.0, 0.0, 0.42),
|
||||
joint_pos={
|
||||
".*_hip_abduction_joint": 0.0,
|
||||
".*_hip_pitch_joint": 0.9,
|
||||
".*_knee_joint": -1.8,
|
||||
".*_hip_pitch_joint": 0.550,
|
||||
".*_knee_joint": -1.125,
|
||||
".*_wheel_joint": 0.0,
|
||||
},
|
||||
joint_vel={".*": 0.0},
|
||||
|
||||
@@ -24,9 +24,9 @@ _COLOR_PURPLE = (0.60, 0.20, 0.80)
|
||||
|
||||
@dataclass(kw_only=True)
|
||||
class RCWallTerrainCfg(SubTerrainCfg):
|
||||
"""Triple transverse wall obstacle terrain representing repeated race high walls.
|
||||
"""Repeated transverse wall obstacle terrain for high-wall gait training.
|
||||
|
||||
The robot must sprint from the flat platform, vault over three walls, and proceed.
|
||||
The robot must repeatedly step/vault over transverse walls and proceed.
|
||||
As difficulty scales from 0 to 1, the wall height increases linearly from
|
||||
wall_height_range[0] to wall_height_range[1].
|
||||
|
||||
@@ -41,9 +41,9 @@ class RCWallTerrainCfg(SubTerrainCfg):
|
||||
wall_length_frac: float = 0.8
|
||||
"""Wall length fraction of the terrain width (leaving gaps for visualization/debugging)."""
|
||||
platform_width: float = 1.5
|
||||
"""Sprint platform width (m)."""
|
||||
wall_centers_x: tuple[float, float, float] = (2.9, 4.45, 6.0)
|
||||
"""Wall center positions along x, spaced to keep a short sprint, two recovery gaps, and exit room."""
|
||||
"""Nominal start platform width (m)."""
|
||||
wall_centers_x: tuple[float, ...] = (2.1, 3.2, 4.3, 5.4, 6.5)
|
||||
"""Wall center positions along x."""
|
||||
|
||||
def function(
|
||||
self,
|
||||
@@ -69,7 +69,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
|
||||
origin = np.array([self.size[0] / 2, self.size[1] / 2, 0.0])
|
||||
return TerrainOutput(origin=origin, geometries=geometries)
|
||||
|
||||
# -- Wall geometry: three transverse walls oriented along y-axis --
|
||||
# -- Wall geometry: transverse walls oriented along y-axis --
|
||||
wall_length = self.wall_length_frac * self.size[1]
|
||||
cy = self.size[1] / 2
|
||||
|
||||
@@ -87,8 +87,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
|
||||
)
|
||||
geometries.append(TerrainGeometry(geom=wall_geom, color=wall_color))
|
||||
|
||||
# Spawn origin is set to the left platform area to allow a short sprint
|
||||
# before the first wall and limited recovery space between subsequent walls.
|
||||
# Spawn origin is set to the left platform area.
|
||||
origin = np.array([1.5, cy, 0.0])
|
||||
return TerrainOutput(origin=origin, geometries=geometries)
|
||||
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
# 导航地图与打点工具
|
||||
|
||||
本目录保存比赛后期使用的独立导航地图、PCD 查看、航点编辑、避障区域编辑和路线验证工具。它同时为 `sim2sim/nav_route_sim2sim_check.py` 提供航点与避障几何定义。
|
||||
|
||||
## 目录
|
||||
|
||||
```text
|
||||
nav_tools/
|
||||
├─ nav_map_viewer.py # 地图、PCD、XML、航点和避障区综合编辑器
|
||||
├─ avoid_region_tool.py # 独立避障多边形编辑器
|
||||
├─ route_safety_check.py # 离线路线净空检查
|
||||
├─ route_candidate_optimizer.py # 航点候选优化
|
||||
├─ run_route_experiments.py # 安全检查与 Sim2Sim 批量实验
|
||||
├─ mirror_nav_xml_points.py # XML 与航点镜像
|
||||
├─ pcd_transform_tool.py # PCD 平移和旋转
|
||||
├─ transform_pcd_xy.py # PCD 坐标原点变换
|
||||
├─ downsample_ascii_pcd.py # ASCII PCD 确定性抽样
|
||||
├─ annotate_odin1_relocalization_frame.py
|
||||
├─ pcd/ # 小于 10 MB 的打点预览点云
|
||||
├─ points/ # 比赛期间的路线迭代 JSON
|
||||
├─ xml/ # 1 号、2 号和 A/B 场地 XML
|
||||
├─ regions/ # 独立避障区域输出目录
|
||||
└─ assets/ # 坐标和机构示意图
|
||||
```
|
||||
|
||||
## 运行
|
||||
|
||||
在 `rc_mjlab` 根目录执行：
|
||||
|
||||
```powershell
|
||||
# 地图与航点综合编辑器
|
||||
uv run --with-requirements .\tools\nav_tools\requirements.txt `
|
||||
python .\tools\nav_tools\nav_map_viewer.py
|
||||
|
||||
# 指定 PCD 的独立避障区域编辑器
|
||||
uv run --with-requirements .\tools\nav_tools\requirements.txt `
|
||||
python .\tools\nav_tools\avoid_region_tool.py --pcd 1hao.pcd
|
||||
|
||||
# 默认检查 points_20260715_120154.json 与 1hao.xml
|
||||
uv run python .\tools\nav_tools\route_safety_check.py
|
||||
|
||||
# 使用最终 Rough 策略进行默认路线 Sim2Sim 检查
|
||||
uv run --with-requirements .\sim2sim\requirements.txt `
|
||||
python .\sim2sim\nav_route_sim2sim_check.py
|
||||
```
|
||||
|
||||
候选路线和批量实验必须显式保存为新文件，不应覆盖历史路线：
|
||||
|
||||
```powershell
|
||||
uv run python .\tools\nav_tools\route_candidate_optimizer.py
|
||||
|
||||
uv run --with-requirements .\sim2sim\requirements.txt `
|
||||
python .\tools\nav_tools\run_route_experiments.py `
|
||||
--points .\tools\nav_tools\points\points_20260715_120154.json
|
||||
```
|
||||
|
||||
## PCD 抽样
|
||||
|
||||
仓库中的两份 PCD 从原始 ASCII 点云按固定步长均匀抽样，字段、坐标和 PCD 头结构保持不变。它们面向地图显示和人工打点，不替代原始高密度点云用于建图、定位精度评估或点云算法基准。
|
||||
|
||||
| 文件 | 原始点数 | 抽样步长 | 仓库点数 | 仓库大小 | SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- |
|
||||
| `pcd/1hao.pcd` | 9,163,893 | 46 | 199,215 | 9,876,010 B | `48B231C52BECA51316F352300C8B2046133E92359E0855227D93DEB0D927AD34` |
|
||||
| `pcd/2hao.pcd` | 10,043,048 | 52 | 193,136 | 9,719,736 B | `714516A7A726D46311A58507149FBC93D622616BE79C59FFED622274D6526B1F` |
|
||||
|
||||
复现抽样：
|
||||
|
||||
```powershell
|
||||
python .\tools\nav_tools\downsample_ascii_pcd.py `
|
||||
<原始PCD> <输出PCD> --max-bytes 9900000
|
||||
```
|
||||
|
||||
## 航点数据
|
||||
|
||||
`points/` 保留原文件名和时间顺序，没有把多个路线重命名成 `old`、`new` 或 `final`。各文件的航点数、避障区和 XML 绑定见 [`points/README.md`](points/README.md)。
|
||||
|
||||
- 请求目录中的 `route_safety_check.py` 是较早版本，因此保留 `v0.8.0` 已归档的后期兼容版本。
|
||||
- `1B_FF.json` 引用了请求目录中缺失的 `xml/B_C.xml`，本次从后期整合目录补齐该文件。
|
||||
- `.uv-cache`、`__pycache__`、自动候选、实验日志和重复轨迹输出不归档。
|
||||
|
||||
整理时使用默认圆形包络和 `0.05 m` 额外净空检查 `points_20260715_120154.json`，报告了 3 组线段—避障区净空不足。该结果按原样保留，未自动移动航点；它表示保守几何检查仍有待复核，不等同于路线没有经过实机使用。
|
||||
|
||||
## 主要操作
|
||||
|
||||
- `Pan`：拖动地图；滚轮缩放；`F` 适应窗口。
|
||||
- `Point`：添加、插入、选择和编辑航点；`Backspace` 删除。
|
||||
- `Terrain`：选择并调整 XML 障碍组的位置和偏航。
|
||||
- `Avoid`：绘制避障多边形；`Enter` 闭合；`Delete` 删除。
|
||||
- `Load JSON`：载入 `waypoints` 或 `segments[].waypoints`。
|
||||
- `Save JSON` / `Save All JSON`：同时保存航点和避障区域。
|
||||
- `M`：切换 XML；`O`：切换坐标原点；`Esc`：退出。
|
||||
|
||||
任务字段约定：`none`、`slalom`、`gravel`、`wall`、`low_bar`、`stairs`、`ramp_bridge`、`spawn` 和 `return`。
|
||||
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate an annotated Odin1 relocalization-frame PNG.
|
||||
|
||||
The wiki defines Odin1 frames as:
|
||||
I: IMU frame
|
||||
L: LiDAR / point-cloud frame
|
||||
C = camera frame
|
||||
|
||||
Relocalization poses are tied to the SLAM point-cloud map, so the device pose
|
||||
should be treated as T_map_L unless a driver-specific TF remaps it.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from PIL import Image, ImageDraw, ImageFont
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
IMG_DIR = TOOL_DIR / "assets"
|
||||
OUT = TOOL_DIR / "odin1_relocalization_frame.png"
|
||||
|
||||
|
||||
def font(size: int, bold: bool = False) -> ImageFont.FreeTypeFont | ImageFont.ImageFont:
|
||||
return ImageFont.load_default()
|
||||
|
||||
|
||||
def arrow(draw: ImageDraw.ImageDraw, start: tuple[int, int], end: tuple[int, int], color: tuple[int, int, int], width: int = 5) -> None:
|
||||
draw.line([start, end], fill=color, width=width)
|
||||
sx, sy = start
|
||||
ex, ey = end
|
||||
dx = ex - sx
|
||||
dy = ey - sy
|
||||
length = max((dx * dx + dy * dy) ** 0.5, 1.0)
|
||||
ux, uy = dx / length, dy / length
|
||||
px, py = -uy, ux
|
||||
head = 18
|
||||
wing = 9
|
||||
points = [
|
||||
(ex, ey),
|
||||
(int(ex - ux * head + px * wing), int(ey - uy * head + py * wing)),
|
||||
(int(ex - ux * head - px * wing), int(ey - uy * head - py * wing)),
|
||||
]
|
||||
draw.polygon(points, fill=color)
|
||||
|
||||
|
||||
def label_box(
|
||||
draw: ImageDraw.ImageDraw,
|
||||
xy: tuple[int, int],
|
||||
text: str,
|
||||
fill: tuple[int, int, int],
|
||||
text_color: tuple[int, int, int] = (255, 255, 255),
|
||||
size: int = 24,
|
||||
) -> None:
|
||||
x, y = xy
|
||||
fnt = font(size, bold=True)
|
||||
lines = text.splitlines()
|
||||
widths = [draw.textbbox((0, 0), line, font=fnt)[2] for line in lines]
|
||||
heights = [draw.textbbox((0, 0), line, font=fnt)[3] - draw.textbbox((0, 0), line, font=fnt)[1] for line in lines]
|
||||
box_w = max(widths) + 28
|
||||
box_h = sum(heights) + 14 * (len(lines) - 1) + 24
|
||||
draw.rounded_rectangle((x, y, x + box_w, y + box_h), radius=10, fill=fill, outline=(255, 255, 255), width=2)
|
||||
cy = y + 12
|
||||
for line, h in zip(lines, heights):
|
||||
draw.text((x + 14, cy), line, fill=text_color, font=fnt)
|
||||
cy += h + 14
|
||||
|
||||
|
||||
def main() -> int:
|
||||
coordinate = Image.open(IMG_DIR / "coordinate.png").convert("RGB")
|
||||
structure = Image.open(IMG_DIR / "structure1.png").convert("RGB")
|
||||
|
||||
target_w = 1180
|
||||
coordinate = coordinate.resize((target_w, int(coordinate.height * target_w / coordinate.width)))
|
||||
structure = structure.resize((target_w, int(structure.height * target_w / structure.width)))
|
||||
|
||||
gap = 24
|
||||
margin = 30
|
||||
title_h = 118
|
||||
canvas = Image.new("RGB", (target_w + margin * 2, title_h + coordinate.height + gap + structure.height + margin), (245, 247, 250))
|
||||
draw = ImageDraw.Draw(canvas)
|
||||
|
||||
title_font = font(34, bold=True)
|
||||
body_font = font(22)
|
||||
draw.text((margin, 22), "Odin1 relocalization frame: LiDAR / point-cloud frame L", fill=(15, 23, 42), font=title_font)
|
||||
draw.text(
|
||||
(margin, 68),
|
||||
"Use the relocalization pose as T_map_L. Convert to robot base with your measured T_L_base.",
|
||||
fill=(51, 65, 85),
|
||||
font=body_font,
|
||||
)
|
||||
|
||||
top_y = title_h
|
||||
bot_y = title_h + coordinate.height + gap
|
||||
canvas.paste(coordinate, (margin, top_y))
|
||||
canvas.paste(structure, (margin, bot_y))
|
||||
|
||||
red = (220, 38, 38)
|
||||
blue = (37, 99, 235)
|
||||
green = (22, 163, 74)
|
||||
amber = (217, 119, 6)
|
||||
|
||||
# coordinate.png positions after scaling to 1180 px.
|
||||
lidar_origin = (margin + 690, top_y + 255)
|
||||
imu_origin = (margin + 286, top_y + 655)
|
||||
camera_origin = (margin + 885, top_y + 500)
|
||||
|
||||
draw.ellipse((lidar_origin[0] - 12, lidar_origin[1] - 12, lidar_origin[0] + 12, lidar_origin[1] + 12), fill=red, outline=(255, 255, 255), width=3)
|
||||
arrow(draw, (margin + 900, top_y + 150), lidar_origin, red, width=6)
|
||||
label_box(draw, (margin + 725, top_y + 70), "Relocalization pose\nis here: frame L", red)
|
||||
|
||||
draw.ellipse((imu_origin[0] - 9, imu_origin[1] - 9, imu_origin[0] + 9, imu_origin[1] + 9), fill=blue)
|
||||
arrow(draw, (margin + 160, top_y + 555), imu_origin, blue, width=4)
|
||||
label_box(draw, (margin + 40, top_y + 460), "IMU frame I\nnot the relocalization origin", blue, size=20)
|
||||
|
||||
draw.ellipse((camera_origin[0] - 9, camera_origin[1] - 9, camera_origin[0] + 9, camera_origin[1] + 9), fill=green)
|
||||
arrow(draw, (margin + 1015, top_y + 565), camera_origin, green, width=4)
|
||||
label_box(draw, (margin + 880, top_y + 585), "Camera frame C\nseparate optical frame", green, size=20)
|
||||
|
||||
# structure1.png positions after scaling to 1180 px.
|
||||
struct_imu = (margin + 398, bot_y + 357)
|
||||
struct_lidar_hint = (margin + 677, bot_y + 350)
|
||||
draw.ellipse((struct_imu[0] - 9, struct_imu[1] - 9, struct_imu[0] + 9, struct_imu[1] + 9), fill=blue)
|
||||
arrow(draw, (margin + 245, bot_y + 250), struct_imu, blue, width=4)
|
||||
label_box(draw, (margin + 45, bot_y + 165), "Wiki marks IMU separately", blue, size=20)
|
||||
|
||||
arrow(draw, (margin + 820, bot_y + 230), struct_lidar_hint, red, width=5)
|
||||
label_box(draw, (margin + 830, bot_y + 145), "L is the point-cloud/LiDAR frame\nshown in the coordinate diagram", red, size=20)
|
||||
|
||||
note = (
|
||||
"Fixed wiki extrinsic: T^imu_lidar translation = [-0.02663, 0.03447, 0.02174] m, rotation = identity.\n"
|
||||
"So L and I axes are parallel, but their origins are offset. Do not use shell center as pose origin."
|
||||
)
|
||||
note_box = (margin, canvas.height - margin - 78, canvas.width - margin, canvas.height - margin)
|
||||
draw.rounded_rectangle(note_box, radius=10, fill=(255, 251, 235), outline=amber, width=2)
|
||||
draw.text((margin + 16, canvas.height - margin - 64), note, fill=(120, 53, 15), font=font(19))
|
||||
|
||||
canvas.save(OUT)
|
||||
print(OUT)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 208 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 28 KiB |
@@ -0,0 +1,373 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Pygame tool for marking no-go / avoid regions on a PCD map."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import pkgutil
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
|
||||
os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
|
||||
|
||||
if not hasattr(pkgutil, "ImpImporter"):
|
||||
pkgutil.ImpImporter = pkgutil.zipimporter # type: ignore[attr-defined]
|
||||
|
||||
import pygame
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
PCD_DIR = TOOL_DIR / "pcd"
|
||||
REGION_DIR = TOOL_DIR / "regions"
|
||||
|
||||
COLOR_BG = (10, 15, 30)
|
||||
COLOR_GRID = (22, 29, 48)
|
||||
COLOR_PANEL = (18, 24, 42)
|
||||
COLOR_BORDER = (50, 65, 95)
|
||||
COLOR_TEXT = (248, 250, 252)
|
||||
COLOR_MUTED = (148, 163, 184)
|
||||
COLOR_PCD = (92, 160, 255)
|
||||
COLOR_REGION = (244, 63, 94)
|
||||
COLOR_CURRENT = (234, 179, 8)
|
||||
|
||||
|
||||
@dataclass
|
||||
class PointCloud:
|
||||
path: Path | None = None
|
||||
points: list[tuple[float, float, float]] = field(default_factory=list)
|
||||
total_count: int = 0
|
||||
sampled_count: int = 0
|
||||
error: str = ""
|
||||
|
||||
|
||||
@dataclass
|
||||
class Region:
|
||||
name: str
|
||||
points: list[tuple[float, float]]
|
||||
kind: str = "avoid"
|
||||
|
||||
|
||||
class Camera:
|
||||
def __init__(self, width: int, height: int, panel_width: int = 330) -> None:
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.panel_width = panel_width
|
||||
self.zoom = 55.0
|
||||
self.pan_x = 0.0
|
||||
self.pan_y = 0.0
|
||||
|
||||
@property
|
||||
def map_width(self) -> int:
|
||||
return max(200, self.width - self.panel_width)
|
||||
|
||||
def world_to_screen(self, x: float, y: float) -> tuple[int, int]:
|
||||
return int(self.map_width / 2 + x * self.zoom + self.pan_x), int(self.height / 2 - y * self.zoom + self.pan_y)
|
||||
|
||||
def screen_to_world(self, sx: float, sy: float) -> tuple[float, float]:
|
||||
return (sx - self.map_width / 2 - self.pan_x) / self.zoom, -(sy - self.height / 2 - self.pan_y) / self.zoom
|
||||
|
||||
def zoom_at(self, factor: float, pos: tuple[int, int]) -> None:
|
||||
before = self.screen_to_world(*pos)
|
||||
self.zoom = max(8.0, min(260.0, self.zoom * factor))
|
||||
after = self.screen_to_world(*pos)
|
||||
self.pan_x += (after[0] - before[0]) * self.zoom
|
||||
self.pan_y -= (after[1] - before[1]) * self.zoom
|
||||
|
||||
|
||||
def get_font(size: int, bold: bool = False) -> pygame.font.Font:
|
||||
return pygame.font.Font(None, size)
|
||||
|
||||
|
||||
def scan_pcd_files() -> list[Path]:
|
||||
PCD_DIR.mkdir(parents=True, exist_ok=True)
|
||||
return sorted(PCD_DIR.glob("*.pcd"), key=lambda item: item.name.lower())
|
||||
|
||||
|
||||
def iter_ascii_pcd_points(path: Path):
|
||||
with path.open("r", encoding="utf-8", errors="ignore") as handle:
|
||||
data = False
|
||||
for raw in handle:
|
||||
line = raw.strip()
|
||||
if data and line:
|
||||
parts = line.split()
|
||||
if len(parts) >= 3:
|
||||
yield float(parts[0]), float(parts[1]), float(parts[2])
|
||||
elif line.upper().startswith("DATA"):
|
||||
if "ascii" not in line.lower():
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {path}")
|
||||
data = True
|
||||
|
||||
|
||||
def load_pcd(path: Path, max_points: int = 90000, z_min: float = -5.0, z_max: float = 0.5) -> PointCloud:
|
||||
cloud = PointCloud(path=path)
|
||||
try:
|
||||
filtered = []
|
||||
for x, y, z in iter_ascii_pcd_points(path):
|
||||
cloud.total_count += 1
|
||||
if z_min <= z <= z_max:
|
||||
filtered.append((x, y, z))
|
||||
stride = max(1, math.ceil(len(filtered) / max_points)) if filtered else 1
|
||||
cloud.points = filtered[::stride]
|
||||
cloud.sampled_count = len(cloud.points)
|
||||
except Exception as exc:
|
||||
cloud.error = str(exc)
|
||||
return cloud
|
||||
|
||||
|
||||
def fit_camera(camera: Camera, cloud: PointCloud, regions: list[Region]) -> None:
|
||||
xs = [p[0] for p in cloud.points]
|
||||
ys = [p[1] for p in cloud.points]
|
||||
for region in regions:
|
||||
xs.extend(p[0] for p in region.points)
|
||||
ys.extend(p[1] for p in region.points)
|
||||
if not xs or not ys:
|
||||
return
|
||||
min_x, max_x = min(xs), max(xs)
|
||||
min_y, max_y = min(ys), max(ys)
|
||||
span_x = max(max_x - min_x, 1.0)
|
||||
span_y = max(max_y - min_y, 1.0)
|
||||
margin = 70
|
||||
camera.zoom = max(8.0, min(260.0, min((camera.map_width - margin * 2) / span_x, (camera.height - margin * 2) / span_y)))
|
||||
camera.pan_x = -((min_x + max_x) / 2) * camera.zoom
|
||||
camera.pan_y = ((min_y + max_y) / 2) * camera.zoom
|
||||
|
||||
|
||||
def draw_grid(surface: pygame.Surface, camera: Camera, font: pygame.font.Font) -> None:
|
||||
min_x, min_y = camera.screen_to_world(0, camera.height)
|
||||
max_x, max_y = camera.screen_to_world(camera.map_width, 0)
|
||||
for gx in range(math.floor(min_x), math.ceil(max_x) + 1):
|
||||
sx, _ = camera.world_to_screen(gx, 0)
|
||||
color = (38, 50, 78) if gx == 0 else COLOR_GRID
|
||||
pygame.draw.line(surface, color, (sx, 0), (sx, camera.height), 2 if gx == 0 else 1)
|
||||
if gx % 2 == 0 and 0 < sx < camera.map_width - 30:
|
||||
surface.blit(font.render(f"{gx}m", True, COLOR_MUTED), (sx + 4, camera.height - 22))
|
||||
for gy in range(math.floor(min_y), math.ceil(max_y) + 1):
|
||||
_, sy = camera.world_to_screen(0, gy)
|
||||
color = (38, 50, 78) if gy == 0 else COLOR_GRID
|
||||
pygame.draw.line(surface, color, (0, sy), (camera.map_width, sy), 2 if gy == 0 else 1)
|
||||
|
||||
|
||||
def draw_pcd(surface: pygame.Surface, camera: Camera, cloud: PointCloud) -> None:
|
||||
for x, y, z in cloud.points:
|
||||
sx, sy = camera.world_to_screen(x, y)
|
||||
if 0 <= sx < camera.map_width and 0 <= sy < camera.height:
|
||||
color = (148, 210, 255) if z > 0.3 else COLOR_PCD
|
||||
surface.set_at((sx, sy), color)
|
||||
|
||||
|
||||
def draw_regions(surface: pygame.Surface, camera: Camera, regions: list[Region], current: list[tuple[float, float]]) -> None:
|
||||
overlay = pygame.Surface(surface.get_size(), pygame.SRCALPHA)
|
||||
for region in regions:
|
||||
points = [camera.world_to_screen(x, y) for x, y in region.points]
|
||||
if len(points) >= 3:
|
||||
pygame.draw.polygon(overlay, (*COLOR_REGION, 75), points)
|
||||
pygame.draw.polygon(surface, COLOR_REGION, points, width=2)
|
||||
if current:
|
||||
points = [camera.world_to_screen(x, y) for x, y in current]
|
||||
for point in points:
|
||||
pygame.draw.circle(surface, COLOR_CURRENT, point, 5)
|
||||
if len(points) >= 2:
|
||||
pygame.draw.lines(surface, COLOR_CURRENT, False, points, width=2)
|
||||
surface.blit(overlay, (0, 0))
|
||||
|
||||
|
||||
def save_regions(regions: list[Region], cloud: PointCloud) -> Path:
|
||||
REGION_DIR.mkdir(parents=True, exist_ok=True)
|
||||
stamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
path = REGION_DIR / f"avoid_regions_{stamp}.json"
|
||||
payload = {
|
||||
"name": f"avoid_regions_{stamp}",
|
||||
"map": cloud.path.stem if cloud.path else "",
|
||||
"frame_id": "map",
|
||||
"regions": [
|
||||
{
|
||||
"id": index,
|
||||
"name": region.name,
|
||||
"kind": region.kind,
|
||||
"polygon": [{"x": x, "y": y} for x, y in region.points],
|
||||
}
|
||||
for index, region in enumerate(regions, start=1)
|
||||
],
|
||||
}
|
||||
path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
return path
|
||||
|
||||
|
||||
def draw_panel(
|
||||
surface: pygame.Surface,
|
||||
camera: Camera,
|
||||
title_font: pygame.font.Font,
|
||||
small_font: pygame.font.Font,
|
||||
pcd_files: list[Path],
|
||||
selected_pcd: int,
|
||||
cloud: PointCloud,
|
||||
regions: list[Region],
|
||||
current: list[tuple[float, float]],
|
||||
saved: Path | None,
|
||||
) -> list[tuple[pygame.Rect, str]]:
|
||||
x0 = camera.map_width
|
||||
pygame.draw.rect(surface, COLOR_PANEL, (x0, 0, camera.panel_width, camera.height))
|
||||
pygame.draw.line(surface, COLOR_BORDER, (x0, 0), (x0, camera.height), 2)
|
||||
x = x0 + 18
|
||||
y = 16
|
||||
buttons: list[tuple[pygame.Rect, str]] = []
|
||||
|
||||
surface.blit(title_font.render("avoid regions", True, COLOR_TEXT), (x, y))
|
||||
y += 34
|
||||
label = pcd_files[selected_pcd].name if pcd_files and selected_pcd >= 0 else "put .pcd in nav_tools/pcd"
|
||||
surface.blit(small_font.render(f"PCD: {label[:32]}", True, COLOR_MUTED), (x, y))
|
||||
y += 24
|
||||
if cloud.error:
|
||||
surface.blit(small_font.render(cloud.error[:36], True, COLOR_REGION), (x, y))
|
||||
else:
|
||||
surface.blit(small_font.render(f"points: {cloud.sampled_count}/{cloud.total_count}", True, COLOR_MUTED), (x, y))
|
||||
y += 34
|
||||
|
||||
for text, action in (("Prev PCD", "prev_pcd"), ("Next PCD", "next_pcd"), ("Fit", "fit"), ("Save JSON", "save")):
|
||||
rect = pygame.Rect(x, y, 128, 26)
|
||||
pygame.draw.rect(surface, (30, 41, 59), rect, border_radius=5)
|
||||
pygame.draw.rect(surface, COLOR_BORDER, rect, width=1, border_radius=5)
|
||||
surface.blit(small_font.render(text, True, COLOR_TEXT), (rect.x + 10, rect.y + 6))
|
||||
buttons.append((rect, action))
|
||||
y += 32
|
||||
|
||||
y += 12
|
||||
lines = [
|
||||
f"regions: {len(regions)}",
|
||||
f"current vertices: {len(current)}",
|
||||
"",
|
||||
"Left click: add vertex",
|
||||
"Enter: close polygon",
|
||||
"Backspace: undo vertex",
|
||||
"Delete: remove last region",
|
||||
"Right/Middle drag: pan",
|
||||
"Wheel: zoom",
|
||||
"S: save, F: fit",
|
||||
]
|
||||
for line in lines:
|
||||
color = COLOR_TEXT if line and ":" not in line else COLOR_MUTED
|
||||
surface.blit(small_font.render(line, True, color), (x, y))
|
||||
y += 20
|
||||
if saved:
|
||||
y += 8
|
||||
surface.blit(small_font.render(f"saved: {saved.name[:28]}", True, (16, 185, 129)), (x, y))
|
||||
return buttons
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Mark avoid/no-go regions on a PCD map.")
|
||||
parser.add_argument("--width", type=int, default=1280)
|
||||
parser.add_argument("--height", type=int, default=820)
|
||||
parser.add_argument("--pcd", type=Path, help="PCD file name/path. Relative paths are resolved from tools/nav_tools/pcd.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
pcd_files = scan_pcd_files()
|
||||
selected_pcd = 0
|
||||
if args.pcd:
|
||||
if args.pcd.is_absolute():
|
||||
raise SystemExit("Use a PCD file name under tools/nav_tools/pcd, not an absolute path.")
|
||||
requested = PCD_DIR / args.pcd
|
||||
if requested.exists() and requested not in pcd_files:
|
||||
pcd_files.append(requested)
|
||||
if requested in pcd_files:
|
||||
selected_pcd = pcd_files.index(requested)
|
||||
cloud = load_pcd(pcd_files[selected_pcd]) if pcd_files else PointCloud()
|
||||
regions: list[Region] = []
|
||||
current: list[tuple[float, float]] = []
|
||||
saved: Path | None = None
|
||||
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
screen = pygame.display.set_mode((args.width, args.height), pygame.RESIZABLE)
|
||||
pygame.display.set_caption("nav_tools - avoid region editor")
|
||||
title_font = get_font(18, bold=True)
|
||||
small_font = get_font(13)
|
||||
map_font = get_font(12)
|
||||
camera = Camera(args.width, args.height)
|
||||
fit_camera(camera, cloud, regions)
|
||||
clock = pygame.time.Clock()
|
||||
dragging = False
|
||||
last_mouse = (0, 0)
|
||||
buttons: list[tuple[pygame.Rect, str]] = []
|
||||
running = True
|
||||
|
||||
while running:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
elif event.type == pygame.VIDEORESIZE:
|
||||
camera.width, camera.height = event.w, event.h
|
||||
screen = pygame.display.set_mode((event.w, event.h), pygame.RESIZABLE)
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
running = False
|
||||
elif event.key in (pygame.K_RETURN, pygame.K_KP_ENTER):
|
||||
if len(current) >= 3:
|
||||
regions.append(Region(f"avoid_{len(regions) + 1}", current[:]))
|
||||
current.clear()
|
||||
elif event.key == pygame.K_BACKSPACE and current:
|
||||
current.pop()
|
||||
elif event.key == pygame.K_DELETE and regions:
|
||||
regions.pop()
|
||||
elif event.key == pygame.K_f:
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif event.key == pygame.K_s:
|
||||
saved = save_regions(regions, cloud)
|
||||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||||
clicked = None
|
||||
for rect, action in buttons:
|
||||
if rect.collidepoint(event.pos):
|
||||
clicked = action
|
||||
break
|
||||
if event.button == 1 and clicked:
|
||||
if clicked == "prev_pcd" and pcd_files:
|
||||
selected_pcd = (selected_pcd - 1) % len(pcd_files)
|
||||
cloud = load_pcd(pcd_files[selected_pcd])
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "next_pcd" and pcd_files:
|
||||
selected_pcd = (selected_pcd + 1) % len(pcd_files)
|
||||
cloud = load_pcd(pcd_files[selected_pcd])
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "fit":
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "save":
|
||||
saved = save_regions(regions, cloud)
|
||||
elif event.button == 1 and event.pos[0] < camera.map_width:
|
||||
current.append(camera.screen_to_world(*event.pos))
|
||||
elif event.button in (2, 3) or (event.button == 1 and event.pos[0] >= camera.map_width):
|
||||
dragging = True
|
||||
last_mouse = event.pos
|
||||
elif event.button == 4 and event.pos[0] < camera.map_width:
|
||||
camera.zoom_at(1.12, event.pos)
|
||||
elif event.button == 5 and event.pos[0] < camera.map_width:
|
||||
camera.zoom_at(1 / 1.12, event.pos)
|
||||
elif event.type == pygame.MOUSEBUTTONUP:
|
||||
dragging = False
|
||||
elif event.type == pygame.MOUSEMOTION and dragging:
|
||||
dx = event.pos[0] - last_mouse[0]
|
||||
dy = event.pos[1] - last_mouse[1]
|
||||
camera.pan_x += dx
|
||||
camera.pan_y += dy
|
||||
last_mouse = event.pos
|
||||
|
||||
screen.fill(COLOR_BG)
|
||||
draw_grid(screen, camera, map_font)
|
||||
draw_pcd(screen, camera, cloud)
|
||||
draw_regions(screen, camera, regions, current)
|
||||
buttons = draw_panel(screen, camera, title_font, small_font, pcd_files, selected_pcd, cloud, regions, current, saved)
|
||||
pygame.display.flip()
|
||||
clock.tick(60)
|
||||
|
||||
pygame.quit()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Deterministically downsample an ASCII PCD file to a byte-size limit."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
DEFAULT_MAX_BYTES = 9_900_000
|
||||
|
||||
|
||||
def read_header(path: Path) -> tuple[list[bytes], int, int]:
|
||||
header: list[bytes] = []
|
||||
declared_points: int | None = None
|
||||
|
||||
with path.open("rb") as stream:
|
||||
while True:
|
||||
line = stream.readline()
|
||||
if not line:
|
||||
raise ValueError(f"PCD header has no DATA line: {path}")
|
||||
header.append(line)
|
||||
fields = line.strip().split(maxsplit=1)
|
||||
if fields and fields[0].upper() == b"POINTS" and len(fields) == 2:
|
||||
declared_points = int(fields[1])
|
||||
if fields and fields[0].upper() == b"DATA":
|
||||
if len(fields) != 2 or fields[1].lower() != b"ascii":
|
||||
raise ValueError("Only DATA ascii PCD files are supported")
|
||||
data_offset = stream.tell()
|
||||
break
|
||||
|
||||
if declared_points is None:
|
||||
raise ValueError(f"PCD header has no POINTS field: {path}")
|
||||
return header, data_offset, declared_points
|
||||
|
||||
|
||||
def render_header(header: list[bytes], point_count: int) -> bytes:
|
||||
rendered: list[bytes] = []
|
||||
replaced_width = False
|
||||
replaced_points = False
|
||||
|
||||
for line in header:
|
||||
newline = b"\r\n" if line.endswith(b"\r\n") else b"\n"
|
||||
fields = line.strip().split(maxsplit=1)
|
||||
key = fields[0].upper() if fields else b""
|
||||
if key == b"WIDTH":
|
||||
rendered.append(f"WIDTH {point_count}".encode("ascii") + newline)
|
||||
replaced_width = True
|
||||
elif key == b"POINTS":
|
||||
rendered.append(f"POINTS {point_count}".encode("ascii") + newline)
|
||||
replaced_points = True
|
||||
elif key == b"HEIGHT":
|
||||
rendered.append(b"HEIGHT 1" + newline)
|
||||
else:
|
||||
rendered.append(line)
|
||||
|
||||
if not replaced_width or not replaced_points:
|
||||
raise ValueError("PCD header must contain WIDTH and POINTS fields")
|
||||
return b"".join(rendered)
|
||||
|
||||
|
||||
def measure_sample(
|
||||
path: Path,
|
||||
data_offset: int,
|
||||
stride: int,
|
||||
) -> tuple[int, int, int]:
|
||||
offset = stride // 2
|
||||
source_count = 0
|
||||
selected_count = 0
|
||||
selected_bytes = 0
|
||||
|
||||
with path.open("rb") as stream:
|
||||
stream.seek(data_offset)
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
if source_count % stride == offset:
|
||||
selected_count += 1
|
||||
selected_bytes += len(line)
|
||||
source_count += 1
|
||||
|
||||
return source_count, selected_count, selected_bytes
|
||||
|
||||
|
||||
def write_sample(
|
||||
source: Path,
|
||||
output: Path,
|
||||
header: bytes,
|
||||
data_offset: int,
|
||||
stride: int,
|
||||
) -> None:
|
||||
offset = stride // 2
|
||||
point_index = 0
|
||||
temporary = output.with_suffix(output.suffix + ".tmp")
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
try:
|
||||
with source.open("rb") as src, temporary.open("wb") as dst:
|
||||
dst.write(header)
|
||||
src.seek(data_offset)
|
||||
for line in src:
|
||||
if not line.strip():
|
||||
continue
|
||||
if point_index % stride == offset:
|
||||
dst.write(line)
|
||||
point_index += 1
|
||||
os.replace(temporary, output)
|
||||
finally:
|
||||
if temporary.exists():
|
||||
temporary.unlink()
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("source", type=Path, help="Source DATA ascii PCD file")
|
||||
parser.add_argument("output", type=Path, help="Downsampled output PCD file")
|
||||
parser.add_argument(
|
||||
"--max-bytes",
|
||||
type=int,
|
||||
default=DEFAULT_MAX_BYTES,
|
||||
help=f"Maximum output size in bytes (default: {DEFAULT_MAX_BYTES})",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = args.source.expanduser().resolve()
|
||||
output = args.output.expanduser().resolve()
|
||||
max_bytes = int(args.max_bytes)
|
||||
|
||||
if source == output:
|
||||
raise ValueError("Source and output paths must be different")
|
||||
if max_bytes <= 8192:
|
||||
raise ValueError("--max-bytes must be greater than 8192")
|
||||
|
||||
header, data_offset, declared_points = read_header(source)
|
||||
data_bytes = source.stat().st_size - data_offset
|
||||
payload_budget = max_bytes - 8192
|
||||
stride = max(1, math.ceil(data_bytes / payload_budget))
|
||||
|
||||
while True:
|
||||
source_count, selected_count, selected_bytes = measure_sample(
|
||||
source,
|
||||
data_offset,
|
||||
stride,
|
||||
)
|
||||
if source_count != declared_points:
|
||||
raise ValueError(
|
||||
f"POINTS declares {declared_points}, but {source_count} data rows were read"
|
||||
)
|
||||
output_header = render_header(header, selected_count)
|
||||
if len(output_header) + selected_bytes <= max_bytes:
|
||||
break
|
||||
stride += 1
|
||||
|
||||
write_sample(source, output, output_header, data_offset, stride)
|
||||
output_bytes = output.stat().st_size
|
||||
if output_bytes > max_bytes:
|
||||
raise RuntimeError(f"Generated file exceeds limit: {output_bytes} > {max_bytes}")
|
||||
|
||||
print(f"source={source}")
|
||||
print(f"output={output}")
|
||||
print(f"source_points={source_count}")
|
||||
print(f"output_points={selected_count}")
|
||||
print(f"stride={stride}")
|
||||
print(f"output_bytes={output_bytes}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,215 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Mirror a nav_tools terrain XML and route JSON together."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
XML_DIR = TOOL_DIR / "xml"
|
||||
POINTS_DIR = TOOL_DIR / "points"
|
||||
DEFAULT_XML = XML_DIR / "1hao.xml"
|
||||
DEFAULT_POINTS = POINTS_DIR / "points_20260715_120154.json"
|
||||
|
||||
|
||||
def format_float(value: float) -> str:
|
||||
if abs(value) < 0.0000005:
|
||||
value = 0.0
|
||||
return f"{value:.6f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def parse_float_tuple(text: str | None) -> tuple[float, ...]:
|
||||
if not text:
|
||||
return ()
|
||||
return tuple(float(item) for item in text.split())
|
||||
|
||||
|
||||
def mirror_coord(value: float, axis_value: float) -> float:
|
||||
return 2.0 * axis_value - value
|
||||
|
||||
|
||||
def normalize_yaw_deg(value: float) -> float:
|
||||
while value > 180.0:
|
||||
value -= 360.0
|
||||
while value <= -180.0:
|
||||
value += 360.0
|
||||
return value
|
||||
|
||||
|
||||
def mirror_yaw_deg(value: float, axis: str) -> float:
|
||||
if axis == "x":
|
||||
return normalize_yaw_deg(180.0 - value)
|
||||
return normalize_yaw_deg(-value)
|
||||
|
||||
|
||||
def mirror_quat(quat: tuple[float, ...], axis: str) -> tuple[float, float, float, float]:
|
||||
if axis == "x":
|
||||
return quat[0], quat[1], -quat[2], quat[3]
|
||||
return quat[0], -quat[1], quat[2], quat[3]
|
||||
|
||||
|
||||
def mirror_xml(source: Path, target: Path, axis: str, axis_value: float) -> None:
|
||||
parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True))
|
||||
tree = ET.parse(source, parser=parser)
|
||||
mirrored = copy.deepcopy(tree)
|
||||
root = mirrored.getroot()
|
||||
model_name = root.get("model", source.stem)
|
||||
if "mirror" not in model_name.lower():
|
||||
root.set("model", f"{model_name} mirror")
|
||||
|
||||
for element in root.iter():
|
||||
pos = parse_float_tuple(element.get("pos"))
|
||||
if len(pos) >= 1:
|
||||
mirrored_pos = list(pos)
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(mirrored_pos) > coord_index:
|
||||
mirrored_pos[coord_index] = mirror_coord(mirrored_pos[coord_index], axis_value)
|
||||
element.set("pos", " ".join(format_float(value) for value in mirrored_pos))
|
||||
|
||||
center = parse_float_tuple(element.get("center"))
|
||||
if len(center) >= 1:
|
||||
mirrored_center = list(center)
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(mirrored_center) > coord_index:
|
||||
mirrored_center[coord_index] = mirror_coord(mirrored_center[coord_index], axis_value)
|
||||
element.set("center", " ".join(format_float(value) for value in mirrored_center))
|
||||
|
||||
quat = parse_float_tuple(element.get("quat"))
|
||||
if len(quat) == 4:
|
||||
mirrored_quat = mirror_quat(quat, axis)
|
||||
element.set("quat", " ".join(format_float(value) for value in mirrored_quat))
|
||||
|
||||
ET.indent(mirrored, space=" ")
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
mirrored.write(target, encoding="utf-8", xml_declaration=False)
|
||||
|
||||
|
||||
def mirror_point_fields(row: dict[str, Any], axis: str, axis_value: float) -> None:
|
||||
coord_keys = ("x", "world_x") if axis == "x" else ("y", "world_y")
|
||||
for key in coord_keys:
|
||||
if key in row and isinstance(row[key], (int, float)):
|
||||
row[key] = mirror_coord(float(row[key]), axis_value)
|
||||
if "yawDeg" in row and isinstance(row["yawDeg"], (int, float)):
|
||||
row["yawDeg"] = mirror_yaw_deg(float(row["yawDeg"]), axis)
|
||||
if "yaw" in row and isinstance(row["yaw"], (int, float)):
|
||||
row["yaw"] = mirror_yaw_deg(float(row["yaw"]), axis)
|
||||
|
||||
|
||||
def mirror_polygon(points: Any, axis: str, axis_value: float) -> None:
|
||||
if not isinstance(points, list):
|
||||
return
|
||||
for point in points:
|
||||
if isinstance(point, dict):
|
||||
key = "x" if axis == "x" else "y"
|
||||
if isinstance(point.get(key), (int, float)):
|
||||
point[key] = mirror_coord(float(point[key]), axis_value)
|
||||
elif isinstance(point, list):
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(point) > coord_index and isinstance(point[coord_index], (int, float)):
|
||||
point[coord_index] = mirror_coord(float(point[coord_index]), axis_value)
|
||||
|
||||
|
||||
def mirror_regions(payload: dict[str, Any], axis: str, axis_value: float) -> None:
|
||||
for key in ("regions", "avoid_regions"):
|
||||
rows = payload.get(key)
|
||||
if not isinstance(rows, list):
|
||||
continue
|
||||
for region in rows:
|
||||
if isinstance(region, dict):
|
||||
mirror_polygon(region.get("polygon", region.get("points")), axis, axis_value)
|
||||
|
||||
|
||||
def mirror_waypoint_rows(rows: Any, axis: str, axis_value: float) -> None:
|
||||
if not isinstance(rows, list):
|
||||
return
|
||||
for row in rows:
|
||||
if isinstance(row, dict):
|
||||
mirror_point_fields(row, axis, axis_value)
|
||||
|
||||
|
||||
def mirror_points_json(source: Path, target: Path, axis: str, axis_value: float, mirrored_xml: Path | None) -> None:
|
||||
payload = json.loads(source.read_text(encoding="utf-8"))
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"Expected top-level JSON object: {source}")
|
||||
|
||||
payload["name"] = f"{payload.get('name', source.stem)}_mirror"
|
||||
payload["mirroredFrom"] = source.name
|
||||
payload["mirror"] = {
|
||||
"axis": axis,
|
||||
f"axis_{axis}": axis_value,
|
||||
"createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
||||
}
|
||||
if mirrored_xml is not None:
|
||||
payload["xml"] = f"xml/{mirrored_xml.name}"
|
||||
|
||||
origin = payload.get("origin")
|
||||
if isinstance(origin, dict):
|
||||
coord_key = "x" if axis == "x" else "y"
|
||||
if isinstance(origin.get(coord_key), (int, float)):
|
||||
origin[coord_key] = mirror_coord(float(origin[coord_key]), axis_value)
|
||||
if isinstance(origin.get("yaw_deg"), (int, float)):
|
||||
origin["yaw_deg"] = mirror_yaw_deg(float(origin["yaw_deg"]), axis)
|
||||
origin["yaw_rad"] = math.radians(float(origin["yaw_deg"]))
|
||||
elif isinstance(origin.get("yaw_rad"), (int, float)):
|
||||
origin["yaw_rad"] = math.radians(mirror_yaw_deg(math.degrees(float(origin["yaw_rad"])), axis))
|
||||
origin["yaw_deg"] = math.degrees(float(origin["yaw_rad"]))
|
||||
|
||||
mirror_waypoint_rows(payload.get("waypoints"), axis, axis_value)
|
||||
segments = payload.get("segments")
|
||||
if isinstance(segments, list):
|
||||
for segment in segments:
|
||||
if isinstance(segment, dict):
|
||||
mirror_waypoint_rows(segment.get("waypoints"), axis, axis_value)
|
||||
|
||||
mirror_regions(payload, axis, axis_value)
|
||||
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
target.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def default_output_path(source: Path, suffix: str) -> Path:
|
||||
return source.with_name(f"{source.stem}{suffix}{source.suffix}")
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Mirror a nav_tools XML and matching points JSON.")
|
||||
parser.add_argument("--xml", type=Path, default=DEFAULT_XML, help="Input XML path.")
|
||||
parser.add_argument("--points", type=Path, default=DEFAULT_POINTS, help="Input points JSON path.")
|
||||
parser.add_argument("--mirror-axis", choices=("x", "y"), default="x", help="Coordinate axis to mirror.")
|
||||
parser.add_argument("--axis-x", type=float, default=0.0, help="Mirror axis x value. Default: 0.")
|
||||
parser.add_argument("--axis-y", type=float, default=0.0, help="Mirror axis y value.")
|
||||
parser.add_argument("--out-xml", type=Path, help="Output XML path. Default: <input>_mirror.xml.")
|
||||
parser.add_argument("--out-points", type=Path, help="Output JSON path. Default: <input>_mirror.json.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
xml_path = args.xml if args.xml.is_absolute() else TOOL_DIR / args.xml
|
||||
points_path = args.points if args.points.is_absolute() else TOOL_DIR / args.points
|
||||
out_xml = args.out_xml if args.out_xml else default_output_path(xml_path, "_mirror")
|
||||
out_points = args.out_points if args.out_points else default_output_path(points_path, "_mirror")
|
||||
if not out_xml.is_absolute():
|
||||
out_xml = TOOL_DIR / out_xml
|
||||
if not out_points.is_absolute():
|
||||
out_points = TOOL_DIR / out_points
|
||||
|
||||
axis_value = args.axis_x if args.mirror_axis == "x" else args.axis_y
|
||||
mirror_xml(xml_path, out_xml, args.mirror_axis, axis_value)
|
||||
mirror_points_json(points_path, out_points, args.mirror_axis, axis_value, out_xml)
|
||||
print(f"XML: {out_xml}")
|
||||
print(f"JSON: {out_points}")
|
||||
print(f"axis_{args.mirror_axis}: {axis_value}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 311 KiB |
@@ -0,0 +1 @@
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,108 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Transform ASCII PCD maps by XY translation and yaw rotation.
|
||||
|
||||
Examples:
|
||||
python pcd_transform_tool.py pcd/1hao.pcd --dx -9.34 --dy -0.88 --yaw-deg -90 --output pcd/1hao_tf.pcd
|
||||
python pcd_transform_tool.py pcd/1hao.pcd --yaw-deg 90 --in-place
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import shutil
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def tool_relative(path: Path) -> Path:
|
||||
if path.is_absolute():
|
||||
raise ValueError("Use a path relative to this nav_tools folder.")
|
||||
return TOOL_DIR / path
|
||||
|
||||
|
||||
def format_float(value: float) -> str:
|
||||
if abs(value) < 5e-10:
|
||||
value = 0.0
|
||||
return f"{value:.8f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def transform_xy(x: float, y: float, dx: float, dy: float, yaw_rad: float) -> tuple[float, float]:
|
||||
tx = x + dx
|
||||
ty = y + dy
|
||||
c = math.cos(yaw_rad)
|
||||
s = math.sin(yaw_rad)
|
||||
return tx * c - ty * s, tx * s + ty * c
|
||||
|
||||
|
||||
def transform_ascii_pcd(source: Path, target: Path, dx: float, dy: float, yaw_deg: float) -> int:
|
||||
yaw_rad = math.radians(yaw_deg)
|
||||
data_started = False
|
||||
transformed = 0
|
||||
with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
|
||||
for raw in fin:
|
||||
line = raw.strip()
|
||||
if data_started and line:
|
||||
parts = line.split()
|
||||
if len(parts) >= 2:
|
||||
x, y = transform_xy(float(parts[0]), float(parts[1]), dx, dy, yaw_rad)
|
||||
parts[0] = format_float(x)
|
||||
parts[1] = format_float(y)
|
||||
fout.write(" ".join(parts) + "\n")
|
||||
transformed += 1
|
||||
else:
|
||||
fout.write(raw)
|
||||
else:
|
||||
fout.write(raw)
|
||||
if line.upper().startswith("DATA"):
|
||||
if "ascii" not in line.lower():
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {source}")
|
||||
data_started = True
|
||||
if transformed == 0:
|
||||
raise RuntimeError(f"No points transformed: {source}")
|
||||
return transformed
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD file.")
|
||||
parser.add_argument("pcd", type=Path, help="Input PCD path relative to this nav_tools folder.")
|
||||
parser.add_argument("--dx", type=float, default=0.0, help="X translation before rotation, meters.")
|
||||
parser.add_argument("--dy", type=float, default=0.0, help="Y translation before rotation, meters.")
|
||||
parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation after translation, degrees.")
|
||||
parser.add_argument("--output", type=Path, help="Output PCD path relative to this nav_tools folder.")
|
||||
parser.add_argument("--in-place", action="store_true", help="Overwrite input PCD after creating a .bak copy.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = tool_relative(args.pcd)
|
||||
if not source.exists():
|
||||
raise FileNotFoundError(source)
|
||||
|
||||
if args.in_place:
|
||||
target = source.with_suffix(source.suffix + ".tmp")
|
||||
elif args.output:
|
||||
target = tool_relative(args.output)
|
||||
else:
|
||||
target = source.with_name(f"{source.stem}_tf{source.suffix}")
|
||||
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
count = transform_ascii_pcd(source, target, args.dx, args.dy, args.yaw_deg)
|
||||
|
||||
if args.in_place:
|
||||
backup = source.with_suffix(source.suffix + f".bak_{time.strftime('%Y%m%d_%H%M%S')}")
|
||||
shutil.copy2(source, backup)
|
||||
target.replace(source)
|
||||
print(f"Transformed {count} points in-place: {source}")
|
||||
print(f"Backup: {backup}")
|
||||
else:
|
||||
print(f"Transformed {count} points: {target}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,24 @@
|
||||
# 航点路线演进
|
||||
|
||||
本目录按原始文件名保存导航打点阶段的路线快照。文件名中的 `new`、`real`、`reall` 等是开发期原名，仅表示当时的迭代语境，不统一解释为正式版本或最终验证状态。
|
||||
|
||||
| 源文件时间 | 文件 | 航点 | 避障区 | 绑定 XML |
|
||||
| --- | --- | ---: | ---: | --- |
|
||||
| 2026-07-10 23:15 | `1B_FF.json` | 48 | 26 | `B_C.xml` |
|
||||
| 2026-07-11 01:35 | `A_FFF.json` | 46 | 26 | `A_C.xml` |
|
||||
| 2026-07-12 23:55 | `1hao_wallspeed08_桥A适用于重定位.json` | 49 | 19 | `1hao.xml` |
|
||||
| 2026-07-13 00:10 | `1hao_not_raogan.json` | 32 | 19 | `1hao.xml` |
|
||||
| 2026-07-13 20:36 | `1hao_n.json` | 48 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 09:57 | `1hao_new.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 11:20 | `1hao_bridge_a_crawl.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 12:01 | `1hao_crawl2.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 12:16 | `1hao_new1.json` | 47 | 19 | `A_C.xml` |
|
||||
| 2026-07-14 12:19 | `1hao_jianyi.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 11:42 | `1hao_real.json` | 30 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 11:43 | `2hao_not_raogan.json` | 29 | 21 | `2hao.xml` |
|
||||
| 2026-07-15 12:01 | `1hao_reall.json` | 29 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 12:01 | `points_20260715_120154.json` | 29 | 19 | `1hao.xml` |
|
||||
|
||||
这些文件各有参数和路线差异，全部保留以呈现打点演进。默认工具入口使用时间戳文件 `points_20260715_120154.json`；它不是对其他文件的替代，其他路线仍可通过 `--points` 或界面下拉框载入。
|
||||
|
||||
默认路线在当前保守圆形包络检查下存在 3 组净空不足记录，因此这里称为“默认归档路线”，不额外标记为几何安全最终版。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
pygame>=2.6.1
|
||||
Pillow>=10.0.0
|
||||
@@ -0,0 +1,558 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate safer waypoint candidates for nav_tools route JSON files.
|
||||
|
||||
The optimizer keeps the original JSON intact, adjusts copied waypoints away
|
||||
from avoid polygons, recomputes yaw, and writes candidate JSON files for
|
||||
offline/sim2sim validation.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from route_safety_check import (
|
||||
AvoidRegion,
|
||||
Waypoint,
|
||||
analyze_route,
|
||||
default_lateral_footprint_radius,
|
||||
load_regions,
|
||||
load_waypoints,
|
||||
point_in_polygon,
|
||||
point_segment_distance,
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class MutablePoint:
|
||||
id: str
|
||||
x: float
|
||||
y: float
|
||||
yaw_deg: float
|
||||
speed: float | None
|
||||
policy: str
|
||||
tolerance: float | None
|
||||
row_refs: list[dict[str, Any]]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Generate safer route candidates.")
|
||||
parser.add_argument("--points", type=Path, default=Path("tools/nav_tools/points/points_20260715_120154.json"))
|
||||
parser.add_argument("--out-dir", type=Path, default=Path("tools/nav_tools/points/auto_candidates"))
|
||||
parser.add_argument("--avoid-margin", type=float, default=0.05)
|
||||
parser.add_argument("--extra-margin", type=float, default=0.08)
|
||||
parser.add_argument("--footprint-radius", type=float, default=None)
|
||||
parser.add_argument("--iterations", type=int, default=240)
|
||||
parser.add_argument("--step-scale", type=float, default=0.45)
|
||||
parser.add_argument("--max-move-per-iter", type=float, default=0.08)
|
||||
parser.add_argument("--max-total-move", type=float, default=0.55)
|
||||
parser.add_argument("--smooth-weight", type=float, default=0.12)
|
||||
parser.add_argument("--lock-ends", action="store_true", default=True)
|
||||
parser.add_argument("--no-lock-ends", dest="lock_ends", action="store_false")
|
||||
parser.add_argument("--slow-near-risk", action="store_true", default=True)
|
||||
parser.add_argument("--no-slow-near-risk", dest="slow_near_risk", action="store_false")
|
||||
parser.add_argument("--min-speed", type=float, default=0.22)
|
||||
parser.add_argument("--max-segment-length", type=float, default=0.65)
|
||||
parser.add_argument(
|
||||
"--focus-start-index",
|
||||
type=int,
|
||||
default=None,
|
||||
help="1-based first waypoint index allowed to move; points outside the focus range are locked.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--focus-end-index",
|
||||
type=int,
|
||||
default=None,
|
||||
help="1-based last waypoint index allowed to move; points outside the focus range are locked.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--policy-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:POLICY",
|
||||
help="Override waypoint policy by 1-based inclusive index range, e.g. 21:26:crawl. Can be repeated.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--speed-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:SPEED",
|
||||
help="Override waypoint speed by 1-based inclusive index range, e.g. 21:26:0.22. Can be repeated.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--shift-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:DX:DY",
|
||||
help="Shift waypoint positions by 1-based inclusive index range. Applied after optimization.",
|
||||
)
|
||||
parser.add_argument("--top", type=int, default=12)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_payload(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
payload = json.load(f)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"JSON root must be object: {path}")
|
||||
return payload
|
||||
|
||||
|
||||
def iter_waypoint_rows(payload: dict[str, Any]) -> list[dict[str, Any]]:
|
||||
rows: list[dict[str, Any]] = []
|
||||
if isinstance(payload.get("waypoints"), list):
|
||||
rows.extend(row for row in payload["waypoints"] if isinstance(row, dict))
|
||||
if isinstance(payload.get("segments"), list):
|
||||
for segment in payload["segments"]:
|
||||
if isinstance(segment, dict) and isinstance(segment.get("waypoints"), list):
|
||||
rows.extend(row for row in segment["waypoints"] if isinstance(row, dict))
|
||||
return rows
|
||||
|
||||
|
||||
def load_mutable_points(payload: dict[str, Any]) -> list[MutablePoint]:
|
||||
unique: list[MutablePoint] = []
|
||||
refs_by_key: dict[tuple[str, float, float], MutablePoint] = {}
|
||||
for index, row in enumerate(iter_waypoint_rows(payload), start=1):
|
||||
x = float(row.get("world_x", row.get("x", 0.0)))
|
||||
y = float(row.get("world_y", row.get("y", 0.0)))
|
||||
key = (str(row.get("id", index)), round(x, 9), round(y, 9))
|
||||
point = refs_by_key.get(key)
|
||||
if point is None:
|
||||
point = MutablePoint(
|
||||
id=str(row.get("id", index)),
|
||||
x=x,
|
||||
y=y,
|
||||
yaw_deg=float(row.get("yawDeg", row.get("yaw_deg", row.get("yaw", 0.0)))),
|
||||
speed=float(row["speed"]) if row.get("speed") is not None else None,
|
||||
policy=str(row.get("policy", "rough")),
|
||||
tolerance=float(row["tolerance"]) if row.get("tolerance") is not None else None,
|
||||
row_refs=[],
|
||||
)
|
||||
refs_by_key[key] = point
|
||||
unique.append(point)
|
||||
point.row_refs.append(row)
|
||||
if len(unique) < 2:
|
||||
raise ValueError("Route must contain at least two unique waypoints")
|
||||
return unique
|
||||
|
||||
|
||||
def mutable_to_waypoints(points: list[MutablePoint]) -> list[Waypoint]:
|
||||
return [
|
||||
Waypoint(
|
||||
index=i,
|
||||
id=p.id,
|
||||
x=p.x,
|
||||
y=p.y,
|
||||
yaw_deg=p.yaw_deg,
|
||||
speed=p.speed,
|
||||
policy=p.policy,
|
||||
tolerance=p.tolerance,
|
||||
)
|
||||
for i, p in enumerate(points, start=1)
|
||||
]
|
||||
|
||||
|
||||
def closest_point_on_segment(px: float, py: float, ax: float, ay: float, bx: float, by: float) -> tuple[float, float]:
|
||||
dx = bx - ax
|
||||
dy = by - ay
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq <= 1.0e-12:
|
||||
return ax, ay
|
||||
t = ((px - ax) * dx + (py - ay) * dy) / length_sq
|
||||
t = max(0.0, min(1.0, t))
|
||||
return ax + t * dx, ay + t * dy
|
||||
|
||||
|
||||
def closest_segment_polygon_pair(
|
||||
ax: float,
|
||||
ay: float,
|
||||
bx: float,
|
||||
by: float,
|
||||
polygon: tuple[tuple[float, float], ...],
|
||||
) -> tuple[float, float, float, float, float]:
|
||||
best = (float("inf"), ax, ay, polygon[0][0], polygon[0][1])
|
||||
samples = 17
|
||||
for i in range(samples):
|
||||
t = i / (samples - 1)
|
||||
sx = ax + (bx - ax) * t
|
||||
sy = ay + (by - ay) * t
|
||||
for px, py in polygon:
|
||||
dist = math.hypot(sx - px, sy - py)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, px, py)
|
||||
for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
|
||||
dist = math.hypot(sx - qx, sy - qy)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, qx, qy)
|
||||
for sx, sy in ((ax, ay), (bx, by)):
|
||||
for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
|
||||
dist = math.hypot(sx - qx, sy - qy)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, qx, qy)
|
||||
return best
|
||||
|
||||
|
||||
def polygon_centroid(polygon: tuple[tuple[float, float], ...]) -> tuple[float, float]:
|
||||
return (
|
||||
sum(x for x, _ in polygon) / len(polygon),
|
||||
sum(y for _, y in polygon) / len(polygon),
|
||||
)
|
||||
|
||||
|
||||
def point_inside_any_region(x: float, y: float, regions: list[AvoidRegion]) -> bool:
|
||||
return any(point_in_polygon(x, y, region.polygon) for region in regions)
|
||||
|
||||
|
||||
def nearest_region_clearance(x: float, y: float, regions: list[AvoidRegion]) -> float:
|
||||
best = float("inf")
|
||||
for region in regions:
|
||||
if point_in_polygon(x, y, region.polygon):
|
||||
return 0.0
|
||||
polygon = region.polygon
|
||||
for (ax, ay), (bx, by) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
best = min(best, point_segment_distance(x, y, ax, ay, bx, by))
|
||||
return best
|
||||
|
||||
|
||||
def optimize_points(
|
||||
points: list[MutablePoint],
|
||||
regions: list[AvoidRegion],
|
||||
required_clearance: float,
|
||||
args: argparse.Namespace,
|
||||
) -> tuple[list[MutablePoint], dict[str, Any]]:
|
||||
original = [(p.x, p.y) for p in points]
|
||||
target_clearance = required_clearance + float(args.extra_margin)
|
||||
locked = {0, len(points) - 1} if args.lock_ends else set()
|
||||
if args.focus_start_index is not None or args.focus_end_index is not None:
|
||||
focus_start = max(1, int(args.focus_start_index or 1)) - 1
|
||||
focus_end = min(len(points), int(args.focus_end_index or len(points))) - 1
|
||||
if focus_start > focus_end:
|
||||
raise ValueError(
|
||||
f"Invalid focus range: start={args.focus_start_index} end={args.focus_end_index}"
|
||||
)
|
||||
locked.update(i for i in range(len(points)) if i < focus_start or i > focus_end)
|
||||
risk_hit_counts = [0 for _ in points]
|
||||
|
||||
for _ in range(int(args.iterations)):
|
||||
deltas = [[0.0, 0.0] for _ in points]
|
||||
weights = [0.0 for _ in points]
|
||||
waypoints = mutable_to_waypoints(points)
|
||||
risks = analyze_route(waypoints, regions, target_clearance)
|
||||
active_risks = [risk for risk in risks if risk.margin_m < 0.0]
|
||||
if not active_risks:
|
||||
break
|
||||
|
||||
id_to_index = {p.id: i for i, p in enumerate(points)}
|
||||
for risk in active_risks:
|
||||
ia = id_to_index.get(risk.start_id)
|
||||
ib = id_to_index.get(risk.end_id)
|
||||
region = next((item for item in regions if item.name == risk.region), None)
|
||||
if ia is None or ib is None or region is None:
|
||||
continue
|
||||
ax, ay = points[ia].x, points[ia].y
|
||||
bx, by = points[ib].x, points[ib].y
|
||||
dist, sx, sy, qx, qy = closest_segment_polygon_pair(ax, ay, bx, by, region.polygon)
|
||||
vx = sx - qx
|
||||
vy = sy - qy
|
||||
norm = math.hypot(vx, vy)
|
||||
if norm < 1.0e-6 or risk.centerline_intersects:
|
||||
cx, cy = polygon_centroid(region.polygon)
|
||||
mx = 0.5 * (ax + bx)
|
||||
my = 0.5 * (ay + by)
|
||||
vx = mx - cx
|
||||
vy = my - cy
|
||||
norm = math.hypot(vx, vy)
|
||||
if norm < 1.0e-6:
|
||||
vx, vy, norm = 1.0, 0.0, 1.0
|
||||
ux = vx / norm
|
||||
uy = vy / norm
|
||||
push = min(float(args.max_move_per_iter), max(0.0, target_clearance - dist) * float(args.step_scale))
|
||||
for idx in (ia, ib):
|
||||
if idx in locked:
|
||||
continue
|
||||
deltas[idx][0] += ux * push
|
||||
deltas[idx][1] += uy * push
|
||||
weights[idx] += 1.0
|
||||
risk_hit_counts[idx] += 1
|
||||
|
||||
if args.smooth_weight > 0.0 and len(points) > 2:
|
||||
for i in range(1, len(points) - 1):
|
||||
if i in locked:
|
||||
continue
|
||||
avg_x = 0.5 * (points[i - 1].x + points[i + 1].x)
|
||||
avg_y = 0.5 * (points[i - 1].y + points[i + 1].y)
|
||||
deltas[i][0] += (avg_x - points[i].x) * float(args.smooth_weight)
|
||||
deltas[i][1] += (avg_y - points[i].y) * float(args.smooth_weight)
|
||||
weights[i] += 1.0
|
||||
|
||||
for i, point in enumerate(points):
|
||||
if i in locked or weights[i] <= 0.0:
|
||||
continue
|
||||
dx = deltas[i][0] / weights[i]
|
||||
dy = deltas[i][1] / weights[i]
|
||||
ox, oy = original[i]
|
||||
next_x = point.x + dx
|
||||
next_y = point.y + dy
|
||||
total_dx = next_x - ox
|
||||
total_dy = next_y - oy
|
||||
total = math.hypot(total_dx, total_dy)
|
||||
if total > float(args.max_total_move):
|
||||
scale = float(args.max_total_move) / total
|
||||
next_x = ox + total_dx * scale
|
||||
next_y = oy + total_dy * scale
|
||||
point.x = next_x
|
||||
point.y = next_y
|
||||
|
||||
for i, point in enumerate(points[:-1]):
|
||||
nxt = points[i + 1]
|
||||
point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
|
||||
points[-1].yaw_deg = points[-2].yaw_deg
|
||||
|
||||
if args.slow_near_risk:
|
||||
for i, point in enumerate(points):
|
||||
clearance = nearest_region_clearance(point.x, point.y, regions)
|
||||
if clearance < target_clearance + 0.12:
|
||||
current = point.speed if point.speed is not None else 0.35
|
||||
point.speed = max(float(args.min_speed), min(current, 0.28))
|
||||
if risk_hit_counts[i] > 0:
|
||||
current = point.speed if point.speed is not None else 0.35
|
||||
point.speed = max(float(args.min_speed), min(current, 0.25))
|
||||
|
||||
final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
|
||||
violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
moves = [
|
||||
{
|
||||
"id": point.id,
|
||||
"dx": round(point.x - ox, 4),
|
||||
"dy": round(point.y - oy, 4),
|
||||
"dist": round(math.hypot(point.x - ox, point.y - oy), 4),
|
||||
"risk_hits": risk_hit_counts[i],
|
||||
}
|
||||
for i, (point, (ox, oy)) in enumerate(zip(points, original))
|
||||
if math.hypot(point.x - ox, point.y - oy) > 1.0e-4 or risk_hit_counts[i] > 0
|
||||
]
|
||||
return points, {
|
||||
"violations": len(violations),
|
||||
"min_margin": round(final_risks[0].margin_m, 6) if final_risks else None,
|
||||
"focus_range": [
|
||||
args.focus_start_index,
|
||||
args.focus_end_index,
|
||||
],
|
||||
"moves": moves,
|
||||
"top_risks": [
|
||||
{
|
||||
"start_id": risk.start_id,
|
||||
"end_id": risk.end_id,
|
||||
"region": risk.region,
|
||||
"margin_m": round(risk.margin_m, 6),
|
||||
"clearance_m": round(risk.clearance_m, 6),
|
||||
"centerline_intersects": risk.centerline_intersects,
|
||||
}
|
||||
for risk in final_risks[: int(args.top)]
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def apply_policy_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 3 or not parts[2]:
|
||||
raise ValueError(f"Invalid --policy-range {item!r}; expected START:END:POLICY")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --policy-range {item!r}; start is after end")
|
||||
policy = parts[2]
|
||||
for index in range(start - 1, end):
|
||||
points[index].policy = policy
|
||||
applied.append({"start": start, "end": end, "policy": policy})
|
||||
return applied
|
||||
|
||||
|
||||
def apply_speed_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 3:
|
||||
raise ValueError(f"Invalid --speed-range {item!r}; expected START:END:SPEED")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --speed-range {item!r}; start is after end")
|
||||
speed = float(parts[2])
|
||||
for index in range(start - 1, end):
|
||||
points[index].speed = speed
|
||||
applied.append({"start": start, "end": end, "speed": speed})
|
||||
return applied
|
||||
|
||||
|
||||
def apply_shift_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 4:
|
||||
raise ValueError(f"Invalid --shift-range {item!r}; expected START:END:DX:DY")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --shift-range {item!r}; start is after end")
|
||||
dx = float(parts[2])
|
||||
dy = float(parts[3])
|
||||
for index in range(start - 1, end):
|
||||
points[index].x += dx
|
||||
points[index].y += dy
|
||||
applied.append({"start": start, "end": end, "dx": dx, "dy": dy})
|
||||
if applied:
|
||||
for i, point in enumerate(points[:-1]):
|
||||
nxt = points[i + 1]
|
||||
point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
|
||||
points[-1].yaw_deg = points[-2].yaw_deg
|
||||
return applied
|
||||
|
||||
|
||||
def refresh_safety_summary(
|
||||
summary: dict[str, Any],
|
||||
points: list[MutablePoint],
|
||||
regions: list[AvoidRegion],
|
||||
required_clearance: float,
|
||||
args: argparse.Namespace,
|
||||
) -> None:
|
||||
final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
|
||||
violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
summary["violations"] = len(violations)
|
||||
summary["min_margin"] = round(final_risks[0].margin_m, 6) if final_risks else None
|
||||
summary["top_risks"] = [
|
||||
{
|
||||
"start_id": risk.start_id,
|
||||
"end_id": risk.end_id,
|
||||
"region": risk.region,
|
||||
"margin_m": round(risk.margin_m, 6),
|
||||
"clearance_m": round(risk.clearance_m, 6),
|
||||
"centerline_intersects": risk.centerline_intersects,
|
||||
}
|
||||
for risk in final_risks[: int(args.top)]
|
||||
]
|
||||
|
||||
|
||||
def apply_points_to_payload(payload: dict[str, Any], points: list[MutablePoint]) -> None:
|
||||
for point in points:
|
||||
for row in point.row_refs:
|
||||
if "world_x" in row:
|
||||
row["world_x"] = point.x
|
||||
row["x"] = point.x
|
||||
if "world_y" in row:
|
||||
row["world_y"] = point.y
|
||||
row["y"] = point.y
|
||||
row["yawDeg"] = point.yaw_deg
|
||||
if point.speed is not None:
|
||||
row["speed"] = point.speed
|
||||
if point.tolerance is not None:
|
||||
row["tolerance"] = point.tolerance
|
||||
if point.policy:
|
||||
row["policy"] = point.policy
|
||||
|
||||
|
||||
def densify_segments(payload: dict[str, Any], max_len: float) -> None:
|
||||
if max_len <= 0.0:
|
||||
return
|
||||
if not isinstance(payload.get("segments"), list):
|
||||
return
|
||||
for segment in payload["segments"]:
|
||||
rows = segment.get("waypoints") if isinstance(segment, dict) else None
|
||||
if not isinstance(rows, list) or len(rows) < 2:
|
||||
continue
|
||||
new_rows: list[dict[str, Any]] = []
|
||||
for a, b in zip(rows, rows[1:]):
|
||||
new_rows.append(a)
|
||||
ax, ay = float(a.get("x", 0.0)), float(a.get("y", 0.0))
|
||||
bx, by = float(b.get("x", 0.0)), float(b.get("y", 0.0))
|
||||
dist = math.hypot(bx - ax, by - ay)
|
||||
inserts = max(0, int(math.ceil(dist / max_len)) - 1)
|
||||
for j in range(inserts):
|
||||
t = (j + 1) / (inserts + 1)
|
||||
row = copy.deepcopy(a)
|
||||
row["id"] = f"{a.get('id')}_{j + 1}"
|
||||
row["x"] = ax + (bx - ax) * t
|
||||
row["y"] = ay + (by - ay) * t
|
||||
row["yawDeg"] = math.degrees(math.atan2(by - ay, bx - ax))
|
||||
if a.get("speed") is not None and b.get("speed") is not None:
|
||||
row["speed"] = min(float(a["speed"]), float(b["speed"]))
|
||||
new_rows.append(row)
|
||||
new_rows.append(rows[-1])
|
||||
segment["waypoints"] = new_rows
|
||||
if isinstance(payload.get("waypoints"), list):
|
||||
flat: list[dict[str, Any]] = []
|
||||
for segment in payload["segments"]:
|
||||
rows = segment.get("waypoints") if isinstance(segment, dict) else None
|
||||
if isinstance(rows, list):
|
||||
flat.extend(copy.deepcopy(row) for row in rows if isinstance(row, dict))
|
||||
if flat:
|
||||
payload["waypoints"] = flat
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
payload = load_payload(args.points)
|
||||
regions = load_regions(payload)
|
||||
points = load_mutable_points(payload)
|
||||
footprint = float(args.footprint_radius) if args.footprint_radius is not None else default_lateral_footprint_radius()
|
||||
required = footprint + float(args.avoid_margin)
|
||||
|
||||
candidate_payload = copy.deepcopy(payload)
|
||||
candidate_points = load_mutable_points(candidate_payload)
|
||||
optimized, summary = optimize_points(candidate_points, regions, required, args)
|
||||
shift_overrides = apply_shift_ranges(optimized, list(args.shift_range))
|
||||
policy_overrides = apply_policy_ranges(optimized, list(args.policy_range))
|
||||
speed_overrides = apply_speed_ranges(optimized, list(args.speed_range))
|
||||
if shift_overrides:
|
||||
summary["shift_overrides"] = shift_overrides
|
||||
if policy_overrides:
|
||||
summary["policy_overrides"] = policy_overrides
|
||||
if speed_overrides:
|
||||
summary["speed_overrides"] = speed_overrides
|
||||
if shift_overrides or policy_overrides or speed_overrides:
|
||||
refresh_safety_summary(summary, optimized, regions, required, args)
|
||||
apply_points_to_payload(candidate_payload, optimized)
|
||||
densify_segments(candidate_payload, float(args.max_segment_length))
|
||||
|
||||
stamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
args.out_dir.mkdir(parents=True, exist_ok=True)
|
||||
out_path = args.out_dir / f"{args.points.stem}_auto_{stamp}.json"
|
||||
candidate_payload["name"] = f"{payload.get('name', args.points.stem)}_auto_{stamp}"
|
||||
candidate_payload["autoOptimize"] = {
|
||||
"source": str(args.points),
|
||||
"createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
||||
"footprintRadius": footprint,
|
||||
"avoidMargin": args.avoid_margin,
|
||||
"extraMargin": args.extra_margin,
|
||||
"summary": summary,
|
||||
}
|
||||
out_path.write_text(json.dumps(candidate_payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
print("Route candidate optimizer")
|
||||
print(f" source: {args.points}")
|
||||
print(f" output: {out_path}")
|
||||
print(f" required_clearance: {required:.3f} m, target: {required + args.extra_margin:.3f} m")
|
||||
print(f" final_violations: {summary['violations']}")
|
||||
print(f" min_margin: {summary['min_margin']} m")
|
||||
print(f" moved_points: {len(summary['moves'])}")
|
||||
for item in summary["top_risks"][: int(args.top)]:
|
||||
print(
|
||||
" risk "
|
||||
f"{item['start_id']}->{item['end_id']} {item['region']} "
|
||||
f"margin={item['margin_m']:.3f} clearance={item['clearance_m']:.3f}"
|
||||
)
|
||||
return 0 if summary["violations"] == 0 else 2
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,638 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline route safety checker for nav_tools waypoint JSON files.
|
||||
|
||||
The checker treats avoid regions as hard no-go polygons and validates the
|
||||
route centerline with a circular robot footprint. It is intentionally light on
|
||||
dependencies so it can run on the robot laptop without ROS, pygame, or shapely.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
ROBOT_BODY_LENGTH = 0.356
|
||||
ROBOT_BODY_WIDTH = 0.235
|
||||
ROBOT_BODY_CENTER_X = 0.1518
|
||||
ROBOT_ORIGIN_FROM_FRONT = 0.105
|
||||
ROBOT_WHEEL_VIS_LENGTH = 0.16
|
||||
ROBOT_WHEEL_VIS_WIDTH = 0.055
|
||||
ROBOT_POSE_HIP = 0.550
|
||||
ROBOT_POSE_KNEE = -1.125
|
||||
PCD_ROBOT_RADIUS = 0.18
|
||||
ROBOT_FOOTPRINT_PADDING = 0.03
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Waypoint:
|
||||
index: int
|
||||
id: str
|
||||
x: float
|
||||
y: float
|
||||
yaw_deg: float
|
||||
speed: float | None
|
||||
policy: str
|
||||
tolerance: float | None
|
||||
slalom_straight: bool = False
|
||||
slalom_script_break: bool = False
|
||||
slalom_script_pos_tolerance: float | None = None
|
||||
exact_reach: bool = False
|
||||
precision_follow: bool = False
|
||||
require_yaw: bool = False
|
||||
yaw_tolerance_deg: float | None = None
|
||||
stable_cycles: int | None = None
|
||||
mandatory_cross: bool = False
|
||||
mandatory_radius: float | None = None
|
||||
mandatory_center_x: float | None = None
|
||||
mandatory_center_y: float | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AvoidRegion:
|
||||
name: str
|
||||
kind: str
|
||||
polygon: tuple[tuple[float, float], ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SegmentRisk:
|
||||
start_id: str
|
||||
end_id: str
|
||||
region: str
|
||||
clearance_m: float
|
||||
required_m: float
|
||||
margin_m: float
|
||||
length_m: float
|
||||
centerline_intersects: bool
|
||||
|
||||
@property
|
||||
def status(self) -> str:
|
||||
if self.centerline_intersects:
|
||||
return "INTERSECT"
|
||||
if self.margin_m < 0.0:
|
||||
return "VIOLATION"
|
||||
if self.margin_m < 0.05:
|
||||
return "TIGHT"
|
||||
return "OK"
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Check nav_tools waypoint routes against avoid/no-go polygons."
|
||||
)
|
||||
parser.add_argument(
|
||||
"--points",
|
||||
type=Path,
|
||||
default=Path("tools/nav_tools/points/points_20260715_120154.json"),
|
||||
help="Route JSON exported by nav_map_viewer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--xml",
|
||||
type=Path,
|
||||
default=Path("tools/nav_tools/xml/1hao.xml"),
|
||||
help="Optional MuJoCo terrain XML used for metadata checks.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--onnx",
|
||||
type=Path,
|
||||
default=Path("model_6800.onnx"),
|
||||
help="Optional ONNX policy path used for input/output shape reporting.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--footprint-radius",
|
||||
type=float,
|
||||
default=None,
|
||||
help="Robot circular footprint radius in meters. Defaults to sim2real lateral footprint.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--avoid-margin",
|
||||
type=float,
|
||||
default=0.05,
|
||||
help="Extra clearance added outside the robot footprint.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--warn-margin",
|
||||
type=float,
|
||||
default=0.05,
|
||||
help="Report a TIGHT warning when spare margin is below this value.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--top",
|
||||
type=int,
|
||||
default=20,
|
||||
help="Number of closest segment-region pairs to print.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--json-out",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Optional machine-readable report path.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--allow-violations",
|
||||
action="store_true",
|
||||
help="Exit with code 0 even when violations are detected.",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_json(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"JSON root must be an object: {path}")
|
||||
return data
|
||||
|
||||
|
||||
def _optional_float(value: Any) -> float | None:
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def load_waypoints(payload: dict[str, Any]) -> list[Waypoint]:
|
||||
rows = None
|
||||
if isinstance(payload.get("segments"), list) and payload["segments"]:
|
||||
rows = []
|
||||
for segment in payload["segments"]:
|
||||
if isinstance(segment, dict) and isinstance(segment.get("waypoints"), list):
|
||||
rows.extend(segment["waypoints"])
|
||||
if rows is None:
|
||||
rows = payload.get("waypoints")
|
||||
if not isinstance(rows, list):
|
||||
raise ValueError("Route JSON has no top-level waypoints or segments[].waypoints")
|
||||
|
||||
waypoints: list[Waypoint] = []
|
||||
for index, row in enumerate(rows, start=1):
|
||||
if not isinstance(row, dict):
|
||||
continue
|
||||
x = float(row.get("world_x", row.get("x", 0.0)))
|
||||
y = float(row.get("world_y", row.get("y", 0.0)))
|
||||
yaw = float(row.get("yawDeg", row.get("yaw_deg", row.get("yaw", 0.0))))
|
||||
speed = row.get("speed")
|
||||
tolerance = row.get("tolerance")
|
||||
waypoints.append(
|
||||
Waypoint(
|
||||
index=index,
|
||||
id=str(row.get("id", index)),
|
||||
x=x,
|
||||
y=y,
|
||||
yaw_deg=yaw,
|
||||
speed=float(speed) if speed is not None else None,
|
||||
policy=str(row.get("policy", "")),
|
||||
tolerance=float(tolerance) if tolerance is not None else None,
|
||||
slalom_straight=bool(row.get("slalom_straight", row.get("slalomStraight", False))),
|
||||
slalom_script_break=bool(
|
||||
row.get("slalom_script_break", row.get("slalomScriptBreak", False))
|
||||
),
|
||||
slalom_script_pos_tolerance=_optional_float(
|
||||
row.get(
|
||||
"slalom_script_pos_tolerance",
|
||||
row.get("slalomScriptPosTolerance", row.get("scriptTolerance")),
|
||||
)
|
||||
),
|
||||
exact_reach=bool(row.get("exact_reach", row.get("exactReach", False))),
|
||||
precision_follow=bool(row.get("precision_follow", row.get("precisionFollow", False))),
|
||||
require_yaw=bool(row.get("require_yaw", row.get("requireYaw", False))),
|
||||
yaw_tolerance_deg=_optional_float(
|
||||
row.get("yaw_tolerance_deg", row.get("yawToleranceDeg"))
|
||||
),
|
||||
stable_cycles=(
|
||||
int(row.get("stable_cycles", row.get("stableCycles")))
|
||||
if row.get("stable_cycles", row.get("stableCycles")) is not None
|
||||
else None
|
||||
),
|
||||
mandatory_cross=bool(row.get("mandatory_cross", row.get("mandatoryCross", False))),
|
||||
mandatory_radius=_optional_float(
|
||||
row.get("mandatory_radius", row.get("mandatoryRadius"))
|
||||
),
|
||||
mandatory_center_x=_optional_float(
|
||||
row.get("mandatory_center_x", row.get("mandatoryCenterX"))
|
||||
),
|
||||
mandatory_center_y=_optional_float(
|
||||
row.get("mandatory_center_y", row.get("mandatoryCenterY"))
|
||||
),
|
||||
)
|
||||
)
|
||||
if len(waypoints) < 2:
|
||||
raise ValueError("Route must contain at least two waypoints")
|
||||
return waypoints
|
||||
|
||||
|
||||
def load_regions(payload: dict[str, Any]) -> list[AvoidRegion]:
|
||||
rows = payload.get("regions", payload.get("avoid_regions", []))
|
||||
if not isinstance(rows, list):
|
||||
return []
|
||||
|
||||
regions: list[AvoidRegion] = []
|
||||
for index, row in enumerate(rows, start=1):
|
||||
if not isinstance(row, dict):
|
||||
continue
|
||||
polygon_rows = row.get("polygon", row.get("points", []))
|
||||
if not isinstance(polygon_rows, list):
|
||||
continue
|
||||
polygon: list[tuple[float, float]] = []
|
||||
for point in polygon_rows:
|
||||
if isinstance(point, dict):
|
||||
polygon.append((float(point.get("x", 0.0)), float(point.get("y", 0.0))))
|
||||
elif isinstance(point, (list, tuple)) and len(point) >= 2:
|
||||
polygon.append((float(point[0]), float(point[1])))
|
||||
if len(polygon) >= 3:
|
||||
regions.append(
|
||||
AvoidRegion(
|
||||
name=str(row.get("name", f"avoid_{index}")),
|
||||
kind=str(row.get("kind", "avoid")),
|
||||
polygon=tuple(polygon),
|
||||
)
|
||||
)
|
||||
return regions
|
||||
|
||||
|
||||
def robot_wheel_local_points(body_center_offset_x: float) -> list[tuple[float, float]]:
|
||||
thigh_dx = -0.25 * math.sin(ROBOT_POSE_HIP)
|
||||
shank_dx = -0.2 * math.sin(ROBOT_POSE_HIP + ROBOT_POSE_KNEE)
|
||||
wheel_positions = (
|
||||
((0.32826 + 0.06389) - ROBOT_BODY_CENTER_X, 0.066172 - 0.027344, 0.1035, 0.014699, 0.04074, 0.0),
|
||||
((0.32826 + 0.06389) - ROBOT_BODY_CENTER_X, -0.065853 + 0.027311, -0.1035, -0.018447, -0.040735, -0.00075079),
|
||||
((-0.024743 - 0.06389) - ROBOT_BODY_CENTER_X, 0.066141 - 0.027309, 0.099459, 0.012475, 0.040737, 0.0),
|
||||
((-0.024743 - 0.06389) - ROBOT_BODY_CENTER_X, -0.065884 + 0.027341, -0.099408, -0.012435, -0.040737, -0.00075079),
|
||||
)
|
||||
return [
|
||||
(
|
||||
body_center_offset_x + pitch_x + knee_x + thigh_dx + shank_dx,
|
||||
pitch_y + knee_y + wheel_y + wheel_geom_y,
|
||||
)
|
||||
for pitch_x, pitch_y, knee_y, wheel_y, wheel_geom_y, knee_x in wheel_positions
|
||||
]
|
||||
|
||||
|
||||
def default_lateral_footprint_radius() -> float:
|
||||
half_width = ROBOT_BODY_WIDTH * 0.5
|
||||
radius = max(PCD_ROBOT_RADIUS, half_width)
|
||||
body_center_offset_x = ROBOT_ORIGIN_FROM_FRONT - ROBOT_BODY_LENGTH * 0.5
|
||||
for _, wheel_y in robot_wheel_local_points(body_center_offset_x):
|
||||
radius = max(radius, abs(wheel_y) + ROBOT_WHEEL_VIS_WIDTH * 0.5)
|
||||
return radius + ROBOT_FOOTPRINT_PADDING
|
||||
|
||||
|
||||
def point_segment_distance(
|
||||
px: float,
|
||||
py: float,
|
||||
ax: float,
|
||||
ay: float,
|
||||
bx: float,
|
||||
by: float,
|
||||
) -> float:
|
||||
dx = bx - ax
|
||||
dy = by - ay
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq <= 1.0e-12:
|
||||
return math.hypot(px - ax, py - ay)
|
||||
t = ((px - ax) * dx + (py - ay) * dy) / length_sq
|
||||
t = max(0.0, min(1.0, t))
|
||||
qx = ax + t * dx
|
||||
qy = ay + t * dy
|
||||
return math.hypot(px - qx, py - qy)
|
||||
|
||||
|
||||
def orientation(
|
||||
ax: float,
|
||||
ay: float,
|
||||
bx: float,
|
||||
by: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
) -> float:
|
||||
return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax)
|
||||
|
||||
|
||||
def on_segment(
|
||||
ax: float,
|
||||
ay: float,
|
||||
bx: float,
|
||||
by: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
) -> bool:
|
||||
return (
|
||||
min(ax, bx) - 1.0e-9 <= cx <= max(ax, bx) + 1.0e-9
|
||||
and min(ay, by) - 1.0e-9 <= cy <= max(ay, by) + 1.0e-9
|
||||
and abs(orientation(ax, ay, bx, by, cx, cy)) <= 1.0e-9
|
||||
)
|
||||
|
||||
|
||||
def segments_intersect(
|
||||
a: tuple[float, float],
|
||||
b: tuple[float, float],
|
||||
c: tuple[float, float],
|
||||
d: tuple[float, float],
|
||||
) -> bool:
|
||||
ax, ay = a
|
||||
bx, by = b
|
||||
cx, cy = c
|
||||
dx, dy = d
|
||||
o1 = orientation(ax, ay, bx, by, cx, cy)
|
||||
o2 = orientation(ax, ay, bx, by, dx, dy)
|
||||
o3 = orientation(cx, cy, dx, dy, ax, ay)
|
||||
o4 = orientation(cx, cy, dx, dy, bx, by)
|
||||
if o1 * o2 < 0.0 and o3 * o4 < 0.0:
|
||||
return True
|
||||
return (
|
||||
on_segment(ax, ay, bx, by, cx, cy)
|
||||
or on_segment(ax, ay, bx, by, dx, dy)
|
||||
or on_segment(cx, cy, dx, dy, ax, ay)
|
||||
or on_segment(cx, cy, dx, dy, bx, by)
|
||||
)
|
||||
|
||||
|
||||
def point_in_polygon(x: float, y: float, polygon: tuple[tuple[float, float], ...]) -> bool:
|
||||
inside = False
|
||||
for index, (ax, ay) in enumerate(polygon):
|
||||
bx, by = polygon[(index + 1) % len(polygon)]
|
||||
if point_segment_distance(x, y, ax, ay, bx, by) <= 1.0e-9:
|
||||
return True
|
||||
if (ay > y) != (by > y):
|
||||
x_cross = (bx - ax) * (y - ay) / (by - ay) + ax
|
||||
if x < x_cross:
|
||||
inside = not inside
|
||||
return inside
|
||||
|
||||
|
||||
def segment_polygon_intersects(
|
||||
a: tuple[float, float],
|
||||
b: tuple[float, float],
|
||||
polygon: tuple[tuple[float, float], ...],
|
||||
) -> bool:
|
||||
if point_in_polygon(a[0], a[1], polygon) or point_in_polygon(b[0], b[1], polygon):
|
||||
return True
|
||||
return any(
|
||||
segments_intersect(a, b, polygon[index], polygon[(index + 1) % len(polygon)])
|
||||
for index in range(len(polygon))
|
||||
)
|
||||
|
||||
|
||||
def segment_polygon_distance(
|
||||
a: tuple[float, float],
|
||||
b: tuple[float, float],
|
||||
polygon: tuple[tuple[float, float], ...],
|
||||
) -> float:
|
||||
if segment_polygon_intersects(a, b, polygon):
|
||||
return 0.0
|
||||
distances = [point_segment_distance(px, py, a[0], a[1], b[0], b[1]) for px, py in polygon]
|
||||
for index, (ax, ay) in enumerate(polygon):
|
||||
bx, by = polygon[(index + 1) % len(polygon)]
|
||||
distances.append(point_segment_distance(a[0], a[1], ax, ay, bx, by))
|
||||
distances.append(point_segment_distance(b[0], b[1], ax, ay, bx, by))
|
||||
return min(distances)
|
||||
|
||||
|
||||
def analyze_route(
|
||||
waypoints: list[Waypoint],
|
||||
regions: list[AvoidRegion],
|
||||
required_clearance: float,
|
||||
) -> list[SegmentRisk]:
|
||||
risks: list[SegmentRisk] = []
|
||||
for start, end in zip(waypoints, waypoints[1:]):
|
||||
a = (start.x, start.y)
|
||||
b = (end.x, end.y)
|
||||
length = math.hypot(end.x - start.x, end.y - start.y)
|
||||
for region in regions:
|
||||
intersects = segment_polygon_intersects(a, b, region.polygon)
|
||||
clearance = 0.0 if intersects else segment_polygon_distance(a, b, region.polygon)
|
||||
risks.append(
|
||||
SegmentRisk(
|
||||
start_id=start.id,
|
||||
end_id=end.id,
|
||||
region=region.name,
|
||||
clearance_m=clearance,
|
||||
required_m=required_clearance,
|
||||
margin_m=clearance - required_clearance,
|
||||
length_m=length,
|
||||
centerline_intersects=intersects,
|
||||
)
|
||||
)
|
||||
risks.sort(key=lambda item: (item.margin_m, item.clearance_m))
|
||||
return risks
|
||||
|
||||
|
||||
def parse_xml_summary(path: Path) -> dict[str, Any]:
|
||||
if not path.exists():
|
||||
return {"path": str(path), "exists": False}
|
||||
root = ET.parse(path).getroot()
|
||||
geoms = [geom for geom in root.iter("geom")]
|
||||
collidable = [
|
||||
geom for geom in geoms
|
||||
if geom.get("name") != "floor"
|
||||
and geom.get("contype", "1") != "0"
|
||||
and geom.get("conaffinity", "1") != "0"
|
||||
]
|
||||
return {
|
||||
"path": str(path),
|
||||
"exists": True,
|
||||
"model": root.get("model", ""),
|
||||
"geom_count": len(geoms),
|
||||
"collidable_geom_count": len(collidable),
|
||||
}
|
||||
|
||||
|
||||
def parse_onnx_summary(path: Path) -> dict[str, Any]:
|
||||
if not path.exists():
|
||||
return {"path": str(path), "exists": False}
|
||||
try:
|
||||
import onnx # type: ignore
|
||||
except Exception as exc: # pragma: no cover - depends on local env
|
||||
return {"path": str(path), "exists": True, "error": f"onnx import failed: {exc}"}
|
||||
|
||||
model = onnx.load(str(path))
|
||||
inputs = [
|
||||
{
|
||||
"name": item.name,
|
||||
"shape": [
|
||||
dim.dim_value if dim.dim_value else dim.dim_param
|
||||
for dim in item.type.tensor_type.shape.dim
|
||||
],
|
||||
}
|
||||
for item in model.graph.input
|
||||
]
|
||||
outputs = [
|
||||
{
|
||||
"name": item.name,
|
||||
"shape": [
|
||||
dim.dim_value if dim.dim_value else dim.dim_param
|
||||
for dim in item.type.tensor_type.shape.dim
|
||||
],
|
||||
}
|
||||
for item in model.graph.output
|
||||
]
|
||||
return {
|
||||
"path": str(path),
|
||||
"exists": True,
|
||||
"inputs": inputs,
|
||||
"outputs": outputs,
|
||||
"metadata_keys": [prop.key for prop in model.metadata_props],
|
||||
}
|
||||
|
||||
|
||||
def risk_to_dict(risk: SegmentRisk) -> dict[str, Any]:
|
||||
return {
|
||||
"start_id": risk.start_id,
|
||||
"end_id": risk.end_id,
|
||||
"region": risk.region,
|
||||
"clearance_m": round(risk.clearance_m, 6),
|
||||
"required_m": round(risk.required_m, 6),
|
||||
"margin_m": round(risk.margin_m, 6),
|
||||
"length_m": round(risk.length_m, 6),
|
||||
"centerline_intersects": risk.centerline_intersects,
|
||||
"status": risk.status,
|
||||
}
|
||||
|
||||
|
||||
def print_report(
|
||||
points_path: Path,
|
||||
xml_summary: dict[str, Any],
|
||||
onnx_summary: dict[str, Any],
|
||||
waypoints: list[Waypoint],
|
||||
regions: list[AvoidRegion],
|
||||
footprint_radius: float,
|
||||
avoid_margin: float,
|
||||
warn_margin: float,
|
||||
risks: list[SegmentRisk],
|
||||
top: int,
|
||||
) -> None:
|
||||
required_clearance = footprint_radius + avoid_margin
|
||||
violations = [risk for risk in risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
tight = [
|
||||
risk for risk in risks
|
||||
if risk.margin_m >= 0.0 and risk.margin_m < warn_margin
|
||||
]
|
||||
route_len = sum(
|
||||
math.hypot(b.x - a.x, b.y - a.y)
|
||||
for a, b in zip(waypoints, waypoints[1:])
|
||||
)
|
||||
|
||||
print("Route safety check")
|
||||
print(f" points: {points_path}")
|
||||
print(f" waypoints: {len(waypoints)}, regions: {len(regions)}, path_length: {route_len:.3f} m")
|
||||
print(
|
||||
" clearance: "
|
||||
f"footprint={footprint_radius:.3f} m + avoid_margin={avoid_margin:.3f} m "
|
||||
f"=> required={required_clearance:.3f} m"
|
||||
)
|
||||
if xml_summary.get("exists"):
|
||||
print(
|
||||
" xml: "
|
||||
f"{xml_summary.get('path')} "
|
||||
f"(model={xml_summary.get('model')}, geoms={xml_summary.get('geom_count')}, "
|
||||
f"collidable={xml_summary.get('collidable_geom_count')})"
|
||||
)
|
||||
else:
|
||||
print(f" xml: missing ({xml_summary.get('path')})")
|
||||
if onnx_summary.get("exists") and not onnx_summary.get("error"):
|
||||
print(f" onnx: {onnx_summary.get('path')}")
|
||||
print(f" inputs: {onnx_summary.get('inputs')}")
|
||||
print(f" outputs: {onnx_summary.get('outputs')}")
|
||||
elif onnx_summary.get("exists"):
|
||||
print(f" onnx: {onnx_summary.get('error')}")
|
||||
else:
|
||||
print(f" onnx: missing ({onnx_summary.get('path')})")
|
||||
|
||||
print("")
|
||||
if violations:
|
||||
print(f"FAIL: {len(violations)} segment-region pairs are inside required clearance.")
|
||||
elif tight:
|
||||
print(f"WARN: no violations, but {len(tight)} segment-region pairs are tight.")
|
||||
else:
|
||||
print("PASS: all segment-region pairs satisfy the requested clearance.")
|
||||
|
||||
print("")
|
||||
print(f"Closest {min(top, len(risks))} segment-region pairs:")
|
||||
print(" status wp_start->wp_end region clear req spare")
|
||||
for risk in risks[:top]:
|
||||
print(
|
||||
f" {risk.status:<10} "
|
||||
f"{risk.start_id:>4}->{risk.end_id:<4} "
|
||||
f"{risk.region:<10} "
|
||||
f"{risk.clearance_m:>6.3f} "
|
||||
f"{risk.required_m:>6.3f} "
|
||||
f"{risk.margin_m:>7.3f}"
|
||||
)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
points_path = args.points.resolve()
|
||||
xml_path = args.xml.resolve()
|
||||
onnx_path = args.onnx.resolve()
|
||||
|
||||
payload = load_json(points_path)
|
||||
waypoints = load_waypoints(payload)
|
||||
regions = load_regions(payload)
|
||||
if not regions:
|
||||
raise ValueError(f"No avoid regions found in {points_path}")
|
||||
|
||||
footprint_radius = (
|
||||
float(args.footprint_radius)
|
||||
if args.footprint_radius is not None
|
||||
else default_lateral_footprint_radius()
|
||||
)
|
||||
required_clearance = footprint_radius + float(args.avoid_margin)
|
||||
risks = analyze_route(waypoints, regions, required_clearance)
|
||||
xml_summary = parse_xml_summary(xml_path)
|
||||
onnx_summary = parse_onnx_summary(onnx_path)
|
||||
|
||||
print_report(
|
||||
points_path,
|
||||
xml_summary,
|
||||
onnx_summary,
|
||||
waypoints,
|
||||
regions,
|
||||
footprint_radius,
|
||||
float(args.avoid_margin),
|
||||
float(args.warn_margin),
|
||||
risks,
|
||||
max(0, int(args.top)),
|
||||
)
|
||||
|
||||
violations = [risk for risk in risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
tight = [
|
||||
risk for risk in risks
|
||||
if risk.margin_m >= 0.0 and risk.margin_m < float(args.warn_margin)
|
||||
]
|
||||
report = {
|
||||
"points": str(points_path),
|
||||
"waypoint_count": len(waypoints),
|
||||
"region_count": len(regions),
|
||||
"footprint_radius_m": round(footprint_radius, 6),
|
||||
"avoid_margin_m": round(float(args.avoid_margin), 6),
|
||||
"required_clearance_m": round(required_clearance, 6),
|
||||
"violations": [risk_to_dict(risk) for risk in violations],
|
||||
"tight": [risk_to_dict(risk) for risk in tight],
|
||||
"closest": [risk_to_dict(risk) for risk in risks[: max(0, int(args.top))]],
|
||||
"xml": xml_summary,
|
||||
"onnx": onnx_summary,
|
||||
}
|
||||
if args.json_out:
|
||||
args.json_out.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.json_out.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
if violations and not args.allow_violations:
|
||||
return 2
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
raise SystemExit(main())
|
||||
except Exception as exc:
|
||||
print(f"ERROR: {exc}", file=sys.stderr)
|
||||
raise SystemExit(1)
|
||||
@@ -0,0 +1,203 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run repeatable safety and sim2sim experiments for a route candidate."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
PROJECT_ROOT = Path(__file__).resolve().parents[2]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Run route safety + sim2sim experiment suite.")
|
||||
parser.add_argument("--points", type=Path, required=True)
|
||||
parser.add_argument("--terrain-xml", type=Path, default=PROJECT_ROOT / "tools/nav_tools/xml/1hao.xml")
|
||||
parser.add_argument("--onnx", type=Path, default=PROJECT_ROOT / "model_6800.onnx")
|
||||
parser.add_argument("--out-dir", type=Path, default=PROJECT_ROOT / "sim2sim/route_experiments")
|
||||
parser.add_argument("--start-yaw-offset-deg", type=float, default=-180.0)
|
||||
parser.add_argument("--heading-offset-deg", type=float, default=180.0)
|
||||
parser.add_argument("--start-z", type=float, default=0.75)
|
||||
parser.add_argument("--settle-steps", type=int, default=500)
|
||||
parser.add_argument("--follower", choices=("waypoint", "pure-pursuit"), default="pure-pursuit")
|
||||
parser.add_argument("--lookahead", type=float, default=0.45)
|
||||
parser.add_argument("--max-vx", type=float, default=0.22)
|
||||
parser.add_argument("--min-cmd-vx", type=float, default=0.04)
|
||||
parser.add_argument("--creep-cmd-vx", type=float, default=0.04)
|
||||
parser.add_argument("--yaw-stop-threshold-deg", type=float, default=45.0)
|
||||
parser.add_argument("--turn-in-place-enter-deg", type=float, default=70.0)
|
||||
parser.add_argument("--cmd-vx-scale", type=float, default=1.0)
|
||||
parser.add_argument("--no-local-safety", action="store_true")
|
||||
parser.add_argument("--include-reverse-vx", action="store_true")
|
||||
parser.add_argument("--quick", action="store_true", help="Use shorter timeouts for fast iteration.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def run_command(args: list[str], cwd: Path) -> tuple[int, str]:
|
||||
proc = subprocess.run(
|
||||
args,
|
||||
cwd=str(cwd),
|
||||
text=True,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
)
|
||||
return proc.returncode, proc.stdout
|
||||
|
||||
|
||||
def newest_report(out_dir: Path) -> Path | None:
|
||||
reports = sorted(out_dir.glob("route_check_*/report.json"), key=lambda p: p.stat().st_mtime, reverse=True)
|
||||
return reports[0] if reports else None
|
||||
|
||||
|
||||
def load_report(path: Path | None) -> dict[str, Any]:
|
||||
if path is None:
|
||||
return {}
|
||||
return json.loads(path.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def run_sim_case(
|
||||
name: str,
|
||||
args: argparse.Namespace,
|
||||
extra: list[str],
|
||||
max_time: int,
|
||||
waypoint_timeout: int,
|
||||
) -> dict[str, Any]:
|
||||
case_out = args.out_dir / name
|
||||
case_out.mkdir(parents=True, exist_ok=True)
|
||||
cmd = [
|
||||
sys.executable,
|
||||
"sim2sim/nav_route_sim2sim_check.py",
|
||||
"--terrain-xml",
|
||||
str(args.terrain_xml),
|
||||
"--points",
|
||||
str(args.points),
|
||||
"--onnx",
|
||||
str(args.onnx),
|
||||
"--policy-backend",
|
||||
"policy-runner",
|
||||
"--mission",
|
||||
"json",
|
||||
"--start-yaw-offset-deg",
|
||||
str(args.start_yaw_offset_deg),
|
||||
"--heading-offset-deg",
|
||||
str(args.heading_offset_deg),
|
||||
"--start-z",
|
||||
str(args.start_z),
|
||||
"--settle-steps",
|
||||
str(args.settle_steps),
|
||||
"--follower",
|
||||
str(args.follower),
|
||||
"--lookahead",
|
||||
str(args.lookahead),
|
||||
"--max-vx",
|
||||
str(args.max_vx),
|
||||
"--min-cmd-vx",
|
||||
str(args.min_cmd_vx),
|
||||
"--creep-cmd-vx",
|
||||
str(args.creep_cmd_vx),
|
||||
"--yaw-stop-threshold-deg",
|
||||
str(args.yaw_stop_threshold_deg),
|
||||
"--turn-in-place-enter-deg",
|
||||
str(args.turn_in_place_enter_deg),
|
||||
"--cmd-vx-scale",
|
||||
str(args.cmd_vx_scale),
|
||||
"--max-time",
|
||||
str(max_time),
|
||||
"--waypoint-timeout",
|
||||
str(waypoint_timeout),
|
||||
"--sample-every",
|
||||
"10",
|
||||
"--out-dir",
|
||||
str(case_out),
|
||||
*extra,
|
||||
]
|
||||
if args.no_local_safety:
|
||||
cmd.append("--no-local-safety")
|
||||
code, output = run_command(cmd, PROJECT_ROOT)
|
||||
report_path = newest_report(case_out)
|
||||
report = load_report(report_path)
|
||||
return {
|
||||
"name": name,
|
||||
"exit_code": code,
|
||||
"success": bool(report.get("success", False)),
|
||||
"reason": report.get("reason", "no report"),
|
||||
"reached": f"{report.get('reached_count', '?')}/{report.get('waypoint_count', '?')}",
|
||||
"min_margin": report.get("min_margin"),
|
||||
"report": str(report_path) if report_path else None,
|
||||
"output_tail": "\n".join(output.splitlines()[-12:]),
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
args.out_dir.mkdir(parents=True, exist_ok=True)
|
||||
suite_dir = args.out_dir / f"suite_{time.strftime('%Y%m%d_%H%M%S')}"
|
||||
suite_dir.mkdir(parents=True, exist_ok=True)
|
||||
args.out_dir = suite_dir
|
||||
|
||||
safety_cmd = [
|
||||
sys.executable,
|
||||
"tools/nav_tools/route_safety_check.py",
|
||||
"--points",
|
||||
str(args.points),
|
||||
"--xml",
|
||||
str(args.terrain_xml),
|
||||
"--onnx",
|
||||
str(args.onnx),
|
||||
"--top",
|
||||
"12",
|
||||
]
|
||||
safety_code, safety_output = run_command(safety_cmd, PROJECT_ROOT)
|
||||
cases = []
|
||||
full_time = 60 if args.quick else 180
|
||||
slice_time = 45 if args.quick else 90
|
||||
cases.append(run_sim_case("full", args, [], full_time, 20))
|
||||
cases.append(run_sim_case("start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
|
||||
cases.append(run_sim_case("mid_13_22", args, ["--start-id", "13", "--end-id", "22"], slice_time, 20))
|
||||
cases.append(run_sim_case("slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
|
||||
if args.include_reverse_vx:
|
||||
old_scale = args.cmd_vx_scale
|
||||
args.cmd_vx_scale = -abs(old_scale)
|
||||
cases.append(run_sim_case("reverse_start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
|
||||
cases.append(run_sim_case("reverse_slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
|
||||
args.cmd_vx_scale = old_scale
|
||||
|
||||
summary = {
|
||||
"points": str(args.points),
|
||||
"terrain_xml": str(args.terrain_xml),
|
||||
"onnx": str(args.onnx),
|
||||
"follower": args.follower,
|
||||
"lookahead": args.lookahead,
|
||||
"max_vx": args.max_vx,
|
||||
"cmd_vx_scale": args.cmd_vx_scale,
|
||||
"no_local_safety": args.no_local_safety,
|
||||
"suite_dir": str(suite_dir),
|
||||
"safety_exit_code": safety_code,
|
||||
"safety_tail": "\n".join(safety_output.splitlines()[-18:]),
|
||||
"cases": cases,
|
||||
}
|
||||
summary_path = suite_dir / "summary.json"
|
||||
summary_path.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
print("Route experiment suite")
|
||||
print(f" points: {args.points}")
|
||||
print(f" suite: {suite_dir}")
|
||||
print(f" safety: {'PASS' if safety_code == 0 else 'FAIL'}")
|
||||
for case in cases:
|
||||
print(
|
||||
f" {case['name']}: success={case['success']} "
|
||||
f"reached={case['reached']} reason={case['reason']} "
|
||||
f"margin={case['min_margin']} report={case['report']}"
|
||||
)
|
||||
print(f" summary: {summary_path}")
|
||||
return 0 if safety_code == 0 and all(case["success"] for case in cases) else 2
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,117 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Translate and yaw-rotate an ASCII PCD in XY.
|
||||
|
||||
Example:
|
||||
python transform_pcd_xy.py pcd/1hao.pcd --in-place --origin-x 9.34 --origin-y 0.88 --yaw-deg -90
|
||||
|
||||
The transform is:
|
||||
1. subtract origin from x/y
|
||||
2. rotate around (0, 0) by yaw-deg
|
||||
|
||||
Only ASCII PCD files are supported. Non-x/y fields are preserved.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def tool_relative(path: Path) -> Path:
|
||||
if path.is_absolute():
|
||||
raise ValueError("Use a path relative to this nav_tools folder.")
|
||||
return TOOL_DIR / path
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD in XY.")
|
||||
parser.add_argument("input", type=Path, help="Input .pcd file.")
|
||||
parser.add_argument("--output", type=Path, help="Output .pcd file. Required unless --in-place is used.")
|
||||
parser.add_argument("--in-place", action="store_true", help="Replace the input file after a successful transform.")
|
||||
parser.add_argument("--origin-x", type=float, default=0.0, help="X value to subtract before rotation.")
|
||||
parser.add_argument("--origin-y", type=float, default=0.0, help="Y value to subtract before rotation.")
|
||||
parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation in degrees after origin subtraction.")
|
||||
parser.add_argument("--precision", type=int, default=8, help="Decimal precision for transformed x/y.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def fmt(value: float, precision: int) -> str:
|
||||
if abs(value) < 0.5 * 10 ** (-precision):
|
||||
value = 0.0
|
||||
return f"{value:.{precision}f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def transform_file(
|
||||
source: Path,
|
||||
target: Path,
|
||||
origin_x: float,
|
||||
origin_y: float,
|
||||
yaw_deg: float,
|
||||
precision: int,
|
||||
) -> int:
|
||||
yaw = math.radians(yaw_deg)
|
||||
cos_yaw = math.cos(yaw)
|
||||
sin_yaw = math.sin(yaw)
|
||||
data_started = False
|
||||
fields: list[str] = []
|
||||
x_idx = 0
|
||||
y_idx = 1
|
||||
count = 0
|
||||
|
||||
with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
|
||||
for line in fin:
|
||||
stripped = line.strip()
|
||||
upper = stripped.upper()
|
||||
if data_started and stripped:
|
||||
parts = stripped.split()
|
||||
if len(parts) > max(x_idx, y_idx):
|
||||
x0 = float(parts[x_idx]) - origin_x
|
||||
y0 = float(parts[y_idx]) - origin_y
|
||||
x1 = x0 * cos_yaw - y0 * sin_yaw
|
||||
y1 = x0 * sin_yaw + y0 * cos_yaw
|
||||
parts[x_idx] = fmt(x1, precision)
|
||||
parts[y_idx] = fmt(y1, precision)
|
||||
fout.write(" ".join(parts) + "\n")
|
||||
count += 1
|
||||
else:
|
||||
fout.write(line)
|
||||
else:
|
||||
fout.write(line)
|
||||
if upper.startswith("FIELDS "):
|
||||
fields = stripped.split()[1:]
|
||||
if "x" in fields and "y" in fields:
|
||||
x_idx = fields.index("x")
|
||||
y_idx = fields.index("y")
|
||||
elif upper.startswith("DATA "):
|
||||
if "ASCII" not in upper:
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {source}")
|
||||
data_started = True
|
||||
return count
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = tool_relative(args.input)
|
||||
if args.in_place:
|
||||
target = source.with_suffix(source.suffix + ".tmp")
|
||||
elif args.output:
|
||||
target = tool_relative(args.output)
|
||||
else:
|
||||
raise SystemExit("--output is required unless --in-place is used")
|
||||
|
||||
count = transform_file(source, target, args.origin_x, args.origin_y, args.yaw_deg, args.precision)
|
||||
if count <= 0:
|
||||
target.unlink(missing_ok=True)
|
||||
raise SystemExit("No PCD points transformed")
|
||||
if args.in_place:
|
||||
target.replace(source)
|
||||
print(f"transformed {count} points: origin=({args.origin_x}, {args.origin_y}), yaw={args.yaw_deg} deg")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain A_C">
|
||||
<statistic center="-5.5 1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary transformed from A_bian: x_C=y_bian, y_C=-x_bian. -->
|
||||
<geom name="border_x_min" pos="-9.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.620583 -0.010049 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.470246 2.97224 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.970246 3.97224 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.219014 4.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.219014 3.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.219014 3.906386 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.325629 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.025629 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.725629 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 -0.037815 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.574371 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.874371 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.174371 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 0.612185 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 0.912185 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 1.212185 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.540216 0.040352 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 0.040352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 0.890352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 1.440352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 1.990352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 2.540352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 3.090352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.920216 3.940352 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.420216 3.040552 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 3.590352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 3.890352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 4.190352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.079784 3.940352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.579784 2.874152 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2.079784 4.008452 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.121128 0.005994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.521128 1.405994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.771128 -0.244006 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom name="slalom_pole_1_base" pos="-6.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_1_shaft" pos="-6.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_2_base" pos="-7.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_2_shaft" pos="-7.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_3_base" pos="-8.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_3_shaft" pos="-8.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_4_base" pos="-8.521128 1.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_4_shaft" pos="-8.521128 1.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain B_C">
|
||||
<statistic center="-5.5 -1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
|
||||
<geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.401477 -1.937395 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.589248 0.022509 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.430146 -2.855226 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.930146 -3.855226 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.231818 -4.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.231818 -3.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.231818 -3.872727 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.521477 -0.037395 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -0.037395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -0.887395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -1.437395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -1.987395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -2.537395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -3.087395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.901477 -3.937395 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.401477 -3.037595 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -3.587395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -3.887395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -4.187395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.098523 -3.937395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.598523 -2.871195 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2.098523 -4.005495 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.075243 0.034483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.475243 -1.365517 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.725243 0.284483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 -0.965517 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 -0.965517 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,67 @@
|
||||
<mujoco model="nav_tools terrain A_C">
|
||||
<statistic center="-5.5 1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.4 3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.9 4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.5 4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3 3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.5 3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.5 2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2 3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.025 0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.425 1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.675 -0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain B_C">
|
||||
<statistic center="-5.5 -1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
|
||||
<geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.4 -3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.9 -4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.5 -4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 -3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 -3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3 -3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.5 -3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.5 -2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2 -3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.025 -0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.425 -1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.675 0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
RC_WheelLeg 是山东华宇工学院 16DOF 串联轮足机器人项目。
|
||||
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、第一代软件闭环和第一份新版 MJCF/训练框架已经完成整理。
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo/Sim2Sim 和导航打点工具已经完成整理。
|
||||
|
||||
## 平台概览
|
||||
|
||||
@@ -34,6 +34,11 @@ RC_WheelLeg/
|
||||
- [x] 整理第一代 MuJoCo、Sim2Sim 和 MJCF
|
||||
- [x] 整理 IK 真机控制与第一代 Python Sim2Real
|
||||
- [x] 整理第一份新版 MJCF 与 mjlab 训练框架
|
||||
- [x] 整理第二版 Sim2Real 随机化训练配置
|
||||
- [x] 整理比赛最终训练代码架构
|
||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
||||
- [ ] 核对比赛机械与仿真模型参数
|
||||
- [ ] 整理 URDF/MJCF 机器人描述
|
||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||
|
||||
Reference in New Issue
Block a user