[sim] 整理后期Sim2Sim与比赛Rough策略
This commit is contained in:
@@ -36,6 +36,7 @@ checkpoints/
|
||||
wandb/
|
||||
sim2sim_log_*.txt
|
||||
**/sim2sim_temp.xml
|
||||
**/route_check_runs/
|
||||
|
||||
# IDE and operating system files
|
||||
.idea/
|
||||
|
||||
@@ -12,6 +12,7 @@
|
||||
| `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
|
||||
| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
@@ -51,3 +52,12 @@
|
||||
- 增加 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 和大量重复路线实验不在本阶段归档。
|
||||
|
||||
@@ -28,7 +28,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.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`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
`rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
|
||||
|
||||
当前内容对应 `v0.7.0`：训练代码保持 `v0.6.0` 的比赛架构，新增后期 MuJoCo 姿态、IK、动力学和 MPC 工具。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
|
||||
当前内容对应 `v0.8.0`：训练代码保持 `v0.6.0` 的比赛架构，包含 `v0.7.0` 的 MuJoCo 独立工具，并新增后期 Sim2Sim、路线检查和比赛最终 Rough ONNX 策略。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
|
||||
|
||||
## 内容
|
||||
|
||||
@@ -12,6 +12,7 @@
|
||||
- `mujoco_sim`：不依赖训练循环的姿态、IK、动力学和 MPC 分析
|
||||
- `mjlab`：固定版本的本地训练框架依赖
|
||||
- `model_rough.pt`：本阶段 Rough 策略权重
|
||||
- `model_6800.onnx`：比赛最终使用的 Rough 策略
|
||||
- `pyproject.toml`、`uv.lock`：Python 环境与依赖锁定
|
||||
|
||||
与 `v0.3.0` 相比，本版本更新了 MJCF 质量和惯性参数，并将 mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。机械 CAD 未发生变化。
|
||||
@@ -22,4 +23,6 @@
|
||||
|
||||
`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)。
|
||||
|
||||
工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
- `mjlab[cu128]`
|
||||
- PyTorch CUDA 12.8 环境
|
||||
- `pynput`
|
||||
- 后期 Sim2Sim 可选依赖：Pygame、ONNX Runtime
|
||||
|
||||
精确解析结果保存在 `uv.lock`。项目使用本地可编辑的 `mjlab`：
|
||||
|
||||
@@ -39,4 +40,10 @@ 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
|
||||
```
|
||||
|
||||
GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。
|
||||
|
||||
> 当前目录对应 `v0.7.0`：保留 `v0.6.0` 的比赛训练架构，并加入后期 MuJoCo 独立工具集。当前目录中的 `model_rough.pt` 是早期参考权重；比赛最终使用的 `model_6800.onnx` 将随最终部署版本归档。
|
||||
> 当前目录对应 `v0.8.0`：保留 `v0.6.0` 的比赛训练架构与 `v0.7.0` 的 MuJoCo 独立工具集，加入后期 Sim2Sim 工具和比赛最终 Rough 策略 `model_6800.onnx`。`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 输入输出与传感器、低通滤波器接口
|
||||
@@ -54,7 +58,9 @@ rc_mjlab/
|
||||
│ ├── scene.xml # mjlab 场景入口文件
|
||||
│ └── meshes/ # STL/OBJ 碰撞与外观网格
|
||||
├── mujoco_sim/ # 姿态、IK、动力学和 MPC 独立工具
|
||||
├── model_rough.pt # 本阶段用于回放和 Sim2Sim 的 Rough 策略
|
||||
├── tools/nav_tools/ # 路线安全检查公共模块
|
||||
├── model_rough.pt # 早期 Rough 参考 checkpoint
|
||||
├── model_6800.onnx # 比赛最终 Rough 策略
|
||||
├── pyproject.toml # 项目依赖（uv 管理，含清华镜像源加速）
|
||||
└── uv.lock # 精确依赖锁定文件
|
||||
```
|
||||
@@ -89,6 +95,8 @@ cd sim2sim
|
||||
uv run python nav_sim2sim.py
|
||||
```
|
||||
|
||||
后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。
|
||||
|
||||
---
|
||||
|
||||
## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)
|
||||
|
||||
Binary file not shown.
@@ -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` 的航点和避障几何定义。
|
||||
|
||||
运行时生成的日志、临时 XML 和 `route_check_runs/` 不纳入版本库。源目录中的大量路线试验结果也未复制；它们包含重复轨迹和本机绝对路径，不属于可复用程序源码。
|
||||
@@ -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"
|
||||
|
||||
@@ -0,0 +1,7 @@
|
||||
# 路线检查公共模块
|
||||
|
||||
`route_safety_check.py` 提供航点、避障区域、机器人平面包络和几何距离计算，供 `sim2sim/nav_route_sim2sim_check.py` 复用。
|
||||
|
||||
该模块只依赖 Python 标准库。原开发目录中的地图编辑器、PCD、比赛路线 JSON、备份和批量实验结果不属于本次后期 Sim2Sim 里程碑，未在这里复制。
|
||||
|
||||
外部路线文件需要包含 `waypoints`（或 `segments[].waypoints`）以及 `regions` / `avoid_regions`。也可以不提供路线文件，直接使用 Sim2Sim 检查器的内置任务。
|
||||
@@ -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_20260705_174627.json"),
|
||||
help="Route JSON exported by nav_map_viewer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--xml",
|
||||
type=Path,
|
||||
default=Path("tools/nav_tools/xml/A.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)
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
RC_WheelLeg 是山东华宇工学院 16DOF 串联轮足机器人项目。
|
||||
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构和后期 MuJoCo 工具集已经完成整理。
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo 工具集和后期 Sim2Sim 已经完成整理。
|
||||
|
||||
## 平台概览
|
||||
|
||||
@@ -37,6 +37,7 @@ RC_WheelLeg/
|
||||
- [x] 整理第二版 Sim2Real 随机化训练配置
|
||||
- [x] 整理比赛最终训练代码架构
|
||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||
- [ ] 核对比赛机械与仿真模型参数
|
||||
- [ ] 整理 URDF/MJCF 机器人描述
|
||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||
|
||||
Reference in New Issue
Block a user