初始提交：关节模组仿真平台
- 三接口契约：自包含 MJCF / 配置 schema / 报告计算规范 - Python 流水线：urdf_to_mjcf → generate_schema → simulate_report（validate_module 一键编排） - 输入案例 urdf + 生成产物 output（自包含 MJCF/schema/报告/网格副本） - 详细架构说明 docs/architecture.md
This commit is contained in:
@@ -0,0 +1,6 @@
|
|||||||
|
# Python
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
|
||||||
|
# 编辑器
|
||||||
|
.vscode/
|
||||||
@@ -0,0 +1,93 @@
|
|||||||
|
# 关节模组仿真平台 —— 快速上手
|
||||||
|
|
||||||
|
把「关节模组」（行星轮系）在本地用 MuJoCo 跑通，并留好三个接口给网页团队，最终让
|
||||||
|
「平台内置的关节模组」和「用户上传的关节模组」都能在浏览器里跑仿真、出报告。
|
||||||
|
|
||||||
|
> 团队三模块分工：关节模组（本仓库）/ 机械臂 / 四足机器狗。网页用 MuJoCo WASM
|
||||||
|
> （`@mujoco/mujoco`）展示，引擎与本地 Python 是同一个 C++ 核心，**MJCF（.xml）是统一模型格式**。
|
||||||
|
|
||||||
|
**只花 30 秒，先看「目录地图」和「一条命令跑起来」两节就够。**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 目录地图（先读哪个）
|
||||||
|
|
||||||
|
```
|
||||||
|
planetary_joint_split_motor_demo_urdf/
|
||||||
|
├── README.md ★ 你正在看的这份：一页看懂
|
||||||
|
├── docs/ ── 给「网页团队」的接口契约 + 详细架构说明
|
||||||
|
│ ├── schema.md 接口② 配置 schema（观测接口：输入/输出/减速比/限位）
|
||||||
|
│ ├── report_spec.md 接口③ 报告计算规范（指标 → 公式）
|
||||||
|
│ └── architecture.md 详细版：脚本语义 / 网页接入 / 用户上传流程 / 占位参数
|
||||||
|
├── scripts/ ── 给「后端」的 Python 流水线
|
||||||
|
│ ├── validate_module.py 一键入口（编排下面三步）
|
||||||
|
│ ├── urdf_to_mjcf.py ① URDF → MJCF
|
||||||
|
│ ├── generate_schema.py ② URDF → schema
|
||||||
|
│ └── simulate_report.py ③ schema → 仿真报告
|
||||||
|
├── urdf/ ── 输入案例（原始 URDF + 原始网格，不动）
|
||||||
|
│ └── planetary_joint_split_motor_demo.urdf
|
||||||
|
└── output/ ── 生成产物（每次重跑覆盖）
|
||||||
|
├── planetary_joint_split_motor_demo.xml MJCF 模型
|
||||||
|
├── planetary_joint_split_motor_demo.json schema
|
||||||
|
├── report.txt / timeseries.csv / report_curves.png
|
||||||
|
└── meshes/ 自包含网格副本
|
||||||
|
```
|
||||||
|
|
||||||
|
| 你是谁 | 先读什么 |
|
||||||
|
|---|---|
|
||||||
|
| 刚接手、想整体了解 | 本 README 全文（一页） |
|
||||||
|
| 网页团队（对接三接口） | [docs/schema.md](docs/schema.md) → [docs/report_spec.md](docs/report_spec.md) |
|
||||||
|
| 后端 / 本地方（跑流水线、改脚本） | [docs/architecture.md](docs/architecture.md) + 各脚本头部注释 |
|
||||||
|
| 只想知道怎么跑 | 下一节「一条命令跑起来」 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 一条命令跑起来
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd scripts
|
||||||
|
python3 validate_module.py \
|
||||||
|
--urdf ../urdf/planetary_joint_split_motor_demo.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint \
|
||||||
|
--work-dir ../output --plot
|
||||||
|
```
|
||||||
|
|
||||||
|
产出（都在 `../output/`）：MJCF、schema、报告 `report.txt`、观测 `timeseries.csv`、曲线 `report_curves.png`。
|
||||||
|
|
||||||
|
依赖：`mujoco`、`numpy`（绘图另需 `matplotlib`，转换另需 `trimesh`）。
|
||||||
|
|
||||||
|
> 常用可调参数：`--load-torque`（输出端负载，默认 -3 N·m）、`--damping`（关节阻尼，默认 0.01）、
|
||||||
|
> `--mode`（`normal` / `overload`）、`--torque-limit`（输入力矩限位，默认 ±10 N·m）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 三接口契约（给网页团队）
|
||||||
|
|
||||||
|
| 接口 | 文件 | 是什么 |
|
||||||
|
|---|---|---|
|
||||||
|
| ① 自包含 MJCF | `output/*.xml` + `output/meshes/` | 浏览器加载的模型，含网格/质量/惯量/齿轮约束/电机 |
|
||||||
|
| ② 配置 schema | [docs/schema.md](docs/schema.md) | 输入/输出关节名、减速比、限位、仿真参数 |
|
||||||
|
| ③ 报告规范 | [docs/report_spec.md](docs/report_spec.md) | 报告要测哪些值、怎么算 |
|
||||||
|
|
||||||
|
一句话：**MJCF 管「怎么动」，schema 管「观测什么」，报告规范管「算出什么」。**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 我要改 X，去哪找
|
||||||
|
|
||||||
|
| 想改什么 | 去这里 |
|
||||||
|
|---|---|
|
||||||
|
| 材料密度 / 力矩限位 / 阻尼等常数 | `scripts/urdf_to_mjcf.py` 顶部（`DENSITIES`、`TORQUE_LIMIT`、`JOINT_DAMPING`） |
|
||||||
|
| 减速比来源 / 仿真默认参数 | `scripts/generate_schema.py` 顶部 `DEFAULTS` |
|
||||||
|
| 报告指标怎么算 | `docs/report_spec.md`（规范）+ `scripts/simulate_report.py`（参考实现） |
|
||||||
|
| 流水线怎么编排 | `scripts/validate_module.py` |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 待填的占位参数
|
||||||
|
|
||||||
|
1. **力矩限位**（默认 ±10 N·m，对应 URDF `effort="1"` 占位）：填真实电机额定值。
|
||||||
|
2. **关节阻尼**（`JOINT_DAMPING = 0.01`）：报告里 ~98% 的「效率损失」来自这个阻尼，真实摩擦/效率需用实际参数建模。
|
||||||
|
3. **材料密度**（`urdf_to_mjcf.py` 顶部）：目前无真值。
|
||||||
|
|
||||||
|
详见 [docs/architecture.md](docs/architecture.md) 第 8 节。
|
||||||
@@ -0,0 +1,201 @@
|
|||||||
|
# 关节模组仿真平台 —— 架构与开发说明（详细版）
|
||||||
|
|
||||||
|
> 想快速上手，先看根目录 [README.md](../README.md)（一页看懂）；本文件是**详细版**，讲脚本语义、网页接入、用户上传流程与占位参数。
|
||||||
|
|
||||||
|
本项目是「浏览器在线 MuJoCo 仿真平台」的**关节模组**部分：把关节模组（行星轮系）在本地用
|
||||||
|
MuJoCo 跑通，并留好三个接口给网页团队，最终让「平台内置的关节模组」和「用户上传的关节模组」
|
||||||
|
都能在浏览器里跑仿真、出报告。
|
||||||
|
|
||||||
|
> 团队三模块分工：关节模组（本目录）/ 机械臂 / 四足机器狗。团队用 MuJoCo WASM
|
||||||
|
> （`@mujoco/mujoco`）做网页展示，引擎与本地 Python 是同一个 C++ 核心，**MJCF（.xml）是统一模型格式**。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 文件清单（哪个文件是什么）
|
||||||
|
|
||||||
|
| 文件 | 是什么 | 给谁用 |
|
||||||
|
|---|---|---|
|
||||||
|
| `../urdf/planetary_joint_split_motor_demo.urdf` | **原始 URDF**（本案例的输入，只有运动学+视觉，缺质量/惯量/电机） | 转换工具输入 |
|
||||||
|
| `../urdf/meshes/*.stl` | 原始网格（STL，单位 mm） | 转换工具读体积分 |
|
||||||
|
| `../output/planetary_joint_split_motor_demo.xml` | **MJCF 模型**（`urdf_to_mjcf.py` 生成，自包含，可直接仿真） | 网页端加载 / 仿真 |
|
||||||
|
| `../output/meshes/*.stl` | 自包含网格副本（MJCF 的 `meshdir` 指向这里） | 随 MJCF 一起交付 |
|
||||||
|
| `../output/*.json / report.txt / timeseries.csv / report_curves.png` | 生成的 schema / 报告 / 观测 / 曲线 | 交付与对照 |
|
||||||
|
| `../scripts/urdf_to_mjcf.py` | **URDF→MJCF 转换工具**（关节模组专用） | 后端转换 |
|
||||||
|
| `../scripts/generate_schema.py` | **URDF→schema 生成器**（读输入/输出关节、减速比、限位） | 后端生成 |
|
||||||
|
| `../scripts/simulate_report.py` | **通用仿真 + 报告脚本**（读 schema 跑仿真、采数据、出报告） | 本地验证 / 报告参考实现 |
|
||||||
|
| `../scripts/validate_module.py` | **一键验证入口**（编排上面三步：URDF→MJCF→schema→报告） | 本地验证 / 后端流水线 |
|
||||||
|
| `schema.md` | **接口 2：配置 schema**（观测接口：输入/输出/减速比/限位） | 网页团队 |
|
||||||
|
| `report_spec.md` | **接口 3：报告计算规范**（指标→公式） | 网页团队 |
|
||||||
|
|
||||||
|
**一句话速记：** URDF 是「输入」，MJCF 是「引擎吃的模型」，schema/report_spec 是「网页团队要看的接口文档」。
|
||||||
|
流水线是 `urdf_to_mjcf.py → generate_schema.py → simulate_report.py` 三步，`validate_module.py` 把三步打包成一条命令。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 三个接口（给网页团队）
|
||||||
|
|
||||||
|
| 接口 | 文件 | 是什么 | 谁负责产出 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| ① 自包含 MJCF | `*.xml` + `meshes/` | 浏览器加载的模型，含网格/质量/惯量/齿轮约束/电机 | 转换工具 |
|
||||||
|
| ② 配置 schema | [schema.md](schema.md) | 输入/输出关节名、减速比、限位、仿真参数 | 后端从 URDF 生成 |
|
||||||
|
| ③ 报告规范 | [report_spec.md](report_spec.md) | 报告要测哪些值、怎么算 | 固定逻辑，前端照实现 |
|
||||||
|
|
||||||
|
关系：**MJCF 管「怎么动」，schema 管「观测什么」，报告规范管「算出什么」。**
|
||||||
|
schema 不描述内部构型（几级、几个行星轮都在 MJCF 里），只统一观测接口。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 本案例怎么跑起来
|
||||||
|
|
||||||
|
**一条命令跑完整条流水线（推荐）：**
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd scripts
|
||||||
|
python3 validate_module.py \
|
||||||
|
--urdf ../urdf/planetary_joint_split_motor_demo.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint \
|
||||||
|
--work-dir ../output --plot
|
||||||
|
```
|
||||||
|
|
||||||
|
产出（都在 `../output/`）：`planetary_joint_split_motor_demo.xml`（MJCF）+ `planetary_joint_split_motor_demo.json`
|
||||||
|
（schema）+ `timeseries.csv`（逐时间步）+ `report_curves.png`（曲线）。
|
||||||
|
|
||||||
|
**只想跑最后一步仿真/报告（已有 MJCF + schema 时）：**
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd scripts
|
||||||
|
python3 simulate_report.py --schema ../output/planetary_joint_split_motor_demo.json --plot # 读 schema 跑
|
||||||
|
python3 simulate_report.py --schema ../output/planetary_joint_split_motor_demo.json --headless # 无显示器，只计算
|
||||||
|
```
|
||||||
|
|
||||||
|
依赖：`mujoco`、`numpy`（绘图需 `matplotlib`；转换需 `trimesh`）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 三个脚本各自怎么用（分步跑）
|
||||||
|
|
||||||
|
流水线三步各自都能单独跑，`validate_module.py` 只是把它们按顺序调用。三步都**显式指定
|
||||||
|
输入/输出关节**（`--input` / `--output`），因为自动识别在真实 URDF 上不可靠（多级级联有
|
||||||
|
多个正 multiplier 的 mimic、fixed 关节也没有 mimic，程序分不清哪级是末端输出）。
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd scripts
|
||||||
|
|
||||||
|
# ① URDF → MJCF（自包含：<compiler meshdir="meshes"/> + 拷贝网格）
|
||||||
|
python3 urdf_to_mjcf.py \
|
||||||
|
--urdf ../urdf/planetary_joint_split_motor_demo.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint \
|
||||||
|
--out ../output/planetary_joint_split_motor_demo.xml --meshdir ../output/meshes
|
||||||
|
|
||||||
|
# ② URDF → schema（读输入/输出、减速比、限位、默认仿真参数）
|
||||||
|
python3 generate_schema.py \
|
||||||
|
--urdf ../urdf/planetary_joint_split_motor_demo.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint \
|
||||||
|
--model planetary_joint_split_motor_demo.xml \
|
||||||
|
--out ../output/planetary_joint_split_motor_demo.json
|
||||||
|
|
||||||
|
# ③ schema → 仿真 + 报告
|
||||||
|
python3 simulate_report.py --schema ../output/planetary_joint_split_motor_demo.json --plot
|
||||||
|
```
|
||||||
|
|
||||||
|
依赖：`numpy`、`trimesh`（`pip install trimesh`，仅转换步骤需要）。
|
||||||
|
|
||||||
|
工具做的事（详见脚本头注释）：
|
||||||
|
|
||||||
|
1. **照搬不改**：关节层级、`<joint><origin>`→子 `<body pos>`、`<axis>`→`<joint axis>`、
|
||||||
|
`<limit>`→`<joint range>`、`<mimic>`→`<equality>` polycoef、`<visual><origin>`→`<geom pos>`。
|
||||||
|
2. **补质量/惯量**：从每个 link 的 STL 做体积分，按脚本顶部的**密度常数**算出质量/重心/惯性张量，
|
||||||
|
多网格用平行轴定理合成。
|
||||||
|
3. **补作动器**：`input_motor`（输入端）+ `load_motor`（输出端负载）。
|
||||||
|
4. **渲染网格覆盖**：个别 STL 面数超 MuJoCo 上限，渲染换降采样版（质量仍按原始网格算）。
|
||||||
|
|
||||||
|
**要改的常数都在脚本顶部**：
|
||||||
|
|
||||||
|
| 常数 | 位置 | 说明 |
|
||||||
|
|---|---|---|
|
||||||
|
| 材料密度 `DENSITIES` / `DEFAULT_DENSITY` | 顶部 | kg/m³，换材料时改这里 |
|
||||||
|
| `TORQUE_LIMIT` | 顶部 | 力矩限位（占位，应填真实电机额定值） |
|
||||||
|
| `JOINT_DAMPING` / `JOINT_ARMATURE` | 顶部 | 数值稳定参数 |
|
||||||
|
|
||||||
|
> ⚠️ **本工具是关节模组专用**。它假设「一个电机输入 + 一个模组输出 + 减速比关系」，
|
||||||
|
> 用「无 `<mimic>` 的关节 = 输入、正 multiplier 跟随者 = 输出」自动识别。**所有类别 URDF 的
|
||||||
|
> 通用转换器由团队后续开发**，本工具只覆盖关节模组。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 把「本案例」接入网页需要什么
|
||||||
|
|
||||||
|
1. **模型文件**：`planetary_joint_split_motor_demo.xml` + `meshes/`（自包含，一起上传）。
|
||||||
|
2. **配置**：按 [schema.md](schema.md) 第 4 节填好这份 JSON，随模型一起给前端。
|
||||||
|
3. **报告逻辑**：前端按 [report_spec.md](report_spec.md) 实现（或后端把本案例的
|
||||||
|
`simulate_report.py` 逻辑翻译到 JS）。
|
||||||
|
|
||||||
|
前端只需：加载 MJCF → 读 schema 拿 `input_joint`/`output_joint`/`gear_ratio`/`limits` →
|
||||||
|
跑仿真 → 按报告规范输出。**不需要**理解行星轮系的内部构型。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 把「用户上传 URDF」接入网页需要什么
|
||||||
|
|
||||||
|
用户上传的是 **zip 包（URDF + mesh 文件）**，不是单个 .urdf（只有 urdf 没有 STL 加载不出模型）。
|
||||||
|
|
||||||
|
后端流水线（就是 `validate_module.py` 干的同一件事）：
|
||||||
|
|
||||||
|
1. **接收 + 校验**：解压 zip，检查 URDF 可解析、mesh 引用齐全、路径无穿越（`../` 之类）。
|
||||||
|
2. **转换**：跑 `urdf_to_mjcf.py` → 产出自包含 MJCF + 网格。
|
||||||
|
3. **生成 schema**：跑 `generate_schema.py`（输入/输出关节**显式指定**，减速比从 `<mimic>` 取或手动给）→ 产出 schema JSON。
|
||||||
|
4. **返回给前端**：`{ mjcf + meshes, schema }`。
|
||||||
|
|
||||||
|
前端拿到这两样后，与「本案例」走同一条路（见第 5 节）。**用户始终只传 URDF+mesh，
|
||||||
|
不传 schema**——schema 是后端算出来的。
|
||||||
|
|
||||||
|
### 6.1 内置模组 vs 用户上传，靠谁区分？
|
||||||
|
|
||||||
|
**是的，全看前端在哪一步分叉。** 分叉点只有一个：**模型从哪来**。分叉之后，两条路立刻并回
|
||||||
|
同一条流水线（跑仿真 → 出报告 → 出曲线），后面不再有任何区别：
|
||||||
|
|
||||||
|
```text
|
||||||
|
┌─ 内置模组：直接用预生成的 MJCF + schema（跳过①②）
|
||||||
|
前端选择 ────────┤
|
||||||
|
└─ 用户上传：上传 URDF+mesh → 后端 ①转MJCF ②生成schema
|
||||||
|
（此分支需要额外给「输入/输出关节」）
|
||||||
|
↓
|
||||||
|
两条路都拿到 { MJCF + schema }
|
||||||
|
↓
|
||||||
|
③ 跑仿真 → 出报告 → 出曲线（完全相同）
|
||||||
|
```
|
||||||
|
|
||||||
|
所以前端要做的「分割」只有两件事：
|
||||||
|
|
||||||
|
1. **选哪个入口**：内置（有现成的 MJCF+schema）还是上传（现转）。
|
||||||
|
2. **给输入/输出关节**：内置模组的输入/输出已经写死在它自带的 schema 里；上传模组则要
|
||||||
|
前端在用户上传时让用户选一下（或后端自动识别 + 用户确认），因为只有用户/构型才知道
|
||||||
|
哪两个关节是输入输出端——这一步程序猜不靠谱（见第 4 节）。
|
||||||
|
|
||||||
|
除这两点外，报告计算、曲线绘制、安全余量判定**零差别**，前端不需要写两套报告逻辑。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 各角色去哪个文件看什么
|
||||||
|
|
||||||
|
| 你想知道… | 去看… |
|
||||||
|
|---|---|
|
||||||
|
| 哪个是 URDF / MJCF / 仿真脚本 | 本文件第 1 节 |
|
||||||
|
| 转换工具怎么跑、改什么常数 | `../scripts/urdf_to_mjcf.py` 头部注释 + 本文件第 4 节 |
|
||||||
|
| schema 怎么从 URDF 生成 | [../scripts/generate_schema.py](../scripts/generate_schema.py) 头部注释 |
|
||||||
|
| 一键验证 / 用户上传流水线怎么编排 | [../scripts/validate_module.py](../scripts/validate_module.py) 头部注释 |
|
||||||
|
| schema 的字段和示例 | [schema.md](schema.md) |
|
||||||
|
| 报告测什么、怎么算 | [report_spec.md](report_spec.md) |
|
||||||
|
| URDF→MJCF 的关键语义（易错点） | `../scripts/urdf_to_mjcf.py` 头部注释（`<joint><origin>`→`<body pos>` 等） |
|
||||||
|
| 通用仿真/报告参考实现 | [../scripts/simulate_report.py](../scripts/simulate_report.py) |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. 仍需填的占位参数
|
||||||
|
|
||||||
|
1. **力矩限位**（默认 ±10 N·m，对应 URDF `effort="1"` 占位）：填真实电机额定值。有两处：
|
||||||
|
- MJCF 作动器 `ctrlrange`：`../scripts/urdf_to_mjcf.py` 顶部的 `TORQUE_LIMIT`；
|
||||||
|
- 报告安全余量用的限位：`../scripts/generate_schema.py` 的 `--torque-limit`（写进 schema.limits.torque）。
|
||||||
|
2. **关节阻尼**（`JOINT_DAMPING = 0.01`）：报告中 ~98% 的「效率损失」来自这个阻尼；
|
||||||
|
真实摩擦/效率需用实际参数建模。
|
||||||
|
3. **材料密度**（`../scripts/urdf_to_mjcf.py` 顶部 `DENSITIES` / `DEFAULT_DENSITY`）：目前无真值，先不改。
|
||||||
@@ -0,0 +1,161 @@
|
|||||||
|
# 接口 3 —— 报告计算规范（指标接口）
|
||||||
|
|
||||||
|
> 用途：定义「关节模组仿真报告」里**固定要测、要输出的值**，以及每个值**怎么从 MuJoCo 数据算出来**。
|
||||||
|
> 前端（WASM）照着这份规范实现报告模块即可，**不需要后端再给脚本**。
|
||||||
|
|
||||||
|
**为什么报告逻辑是固定的？** 运动关系随上传的构型不同而不同（几级、几个行星轮都不一样），但
|
||||||
|
「要测什么」是确定的：输入/输出的位置、速度、加速度、力矩，以及由它们导出的功率、效率、减速比、
|
||||||
|
安全余量。唯一随构型变的是**哪个关节是输入、哪个是输出**——这由 [schema.md](schema.md) 提供。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 输入
|
||||||
|
|
||||||
|
报告模块需要的输入分三类：
|
||||||
|
|
||||||
|
| 输入 | 来源 | 说明 |
|
||||||
|
|---|---|---|
|
||||||
|
| 模型 | MJCF（`schema.model`） | 含网格、质量/惯量、齿轮约束、作动器 |
|
||||||
|
| 观测配置 | schema | `input_joint` / `output_joint` / `gear_ratio` / `limits` |
|
||||||
|
| 场景参数 | schema 的 `simulation` | 步长、时长、PD 增益、参考轨迹、负载 |
|
||||||
|
|
||||||
|
约定记号（下文统一用）：
|
||||||
|
|
||||||
|
| 记号 | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `q_in`, `qd_in`, `qacc_in` | 输入关节位置 / 速度 / 加速度 |
|
||||||
|
| `q_out`, `qd_out`, `qacc_out` | 输出关节位置 / 速度 / 加速度 |
|
||||||
|
| `τ_in`, `τ_out` | 输入力矩 / 输出力矩 |
|
||||||
|
| `N` | 减速比（`schema.gear_ratio`） |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 仿真流程（固定）
|
||||||
|
|
||||||
|
```
|
||||||
|
加载 MJCF → mj_resetData
|
||||||
|
for k in 0..(duration/timestep)-1:
|
||||||
|
t = k · timestep
|
||||||
|
q_ref = amplitude · sin(2π · frequency · t)
|
||||||
|
qd_ref = amplitude · 2π·frequency · cos(2π · frequency · t)
|
||||||
|
|
||||||
|
τ_in = kp·(q_ref − q_in) + kd·(qd_ref − qd_in) # PD 位置控制
|
||||||
|
ctrl[input_motor] = τ_in # 输入电机力矩
|
||||||
|
ctrl[load_motor] = load_torque # 输出端恒值负载
|
||||||
|
mj_step() # 齿轮约束在步内解算
|
||||||
|
记录本步数据
|
||||||
|
```
|
||||||
|
|
||||||
|
> 齿轮耦合是**软约束**（`<equality>` + `solref`），`mj_step` 内自动解算；输出力矩
|
||||||
|
> `τ_out` 从约束反力读出（见下）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 指标 → 数据源 → 公式（核心表）
|
||||||
|
|
||||||
|
MuJoCo 数据源以 `d.*` 表示 `MjData` 的字段；`input_dof` / `output_dof` 是输入/输出关节
|
||||||
|
的自由度下标（`m.jnt_dofadr[...]`）。
|
||||||
|
|
||||||
|
### 3.1 运动学（跟踪精度）
|
||||||
|
|
||||||
|
| 指标 | 符号 | 数据源（逐时间步） | 单位 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| 输入位置 | `q_in` | `d.qpos[input_dof]` | rad |
|
||||||
|
| 输入速度 | `qd_in` | `d.qvel[input_dof]` | rad/s |
|
||||||
|
| 输入加速度 | `qacc_in` | `d.qacc[input_dof]` | rad/s² |
|
||||||
|
| 输出位置 | `q_out` | `d.qpos[output_dof]` | rad |
|
||||||
|
| 输出速度 | `qd_out` | `d.qvel[output_dof]` | rad/s |
|
||||||
|
| 输出加速度 | `qacc_out` | `d.qacc[output_dof]` | rad/s² |
|
||||||
|
|
||||||
|
**汇总（对整段轨迹统计）：**
|
||||||
|
|
||||||
|
| 指标 | 公式 | 说明 |
|
||||||
|
|---|---|---|
|
||||||
|
| 输入位置峰值 | `max |q_in|` | 参考幅值 `amplitude` |
|
||||||
|
| 位置跟踪误差 RMS | `√ mean((q_in − q_ref)²)` | 越小越准 |
|
||||||
|
| 速度跟踪误差 RMS | `√ mean((qd_in − qd_ref)²)` | 越小越准 |
|
||||||
|
| 输出位置峰值 | `max |q_out|` | 期望 ≈ `amplitude / N` |
|
||||||
|
| **传动比实测** | 带截距最小二乘斜率（`q_out` 对 `q_in` 拟合，取稳态后半段） | 期望 ≈ `1/N` |
|
||||||
|
|
||||||
|
> 不要用逐点 `q_out/q_in` 再取均值：正弦参考下 `q_in` 每周期过零，软约束相位滞后会让
|
||||||
|
> 过零处比值爆表甚至变号，把均值带偏（实测 0.112368 vs 真实 0.111111）。带截距的
|
||||||
|
> 最小二乘斜率对相位滞后和负载静偏置（`q_out` 恒滞后一个常数角）都不敏感，能精确还原 `1/N`。
|
||||||
|
|
||||||
|
### 3.2 动力学（力矩 / 功率）
|
||||||
|
|
||||||
|
| 指标 | 符号 | 数据源 / 公式 | 单位 |
|
||||||
|
|---|---|---|---|
|
||||||
|
| 输入力矩 | `τ_in` | `d.actuator_force[input_motor]` | N·m |
|
||||||
|
| 输出力矩 | `τ_out` | `d.qfrc_constraint[output_dof]` | N·m |
|
||||||
|
| 输出功率 | `P_out` | `τ_out · qd_out` | W |
|
||||||
|
| 理想输出力矩 | `τ_ideal` | `N · τ_in` | N·m |
|
||||||
|
| 力矩损失 | `Δτ` | `N·τ_in − τ_out` | N·m |
|
||||||
|
| **效率** | `η` | `τ_out / (N · τ_in) × 100%`（稳态均值） | % |
|
||||||
|
|
||||||
|
> 稳态均值 = 取时间序列后半段（让瞬态衰减完）的 `mean(|·|)`。
|
||||||
|
|
||||||
|
### 3.3 安全性（安全余量）
|
||||||
|
|
||||||
|
| 指标 | 公式 | 单位 |
|
||||||
|
|---|---|---|
|
||||||
|
| 位置余量 | `(‖limit_pos‖ − max|q_in|) / ‖limit_pos‖ × 100%` | % |
|
||||||
|
| 力矩余量 | `(‖limit_trq‖ − max|τ_in|) / ‖limit_trq‖ × 100%` | % |
|
||||||
|
| 过载判定 | 力矩余量 < 20% → 警告 | — |
|
||||||
|
|
||||||
|
其中 `limit_pos = schema.limits.position[1]`，`limit_trq = schema.limits.torque[1]`。
|
||||||
|
|
||||||
|
### 3.4 可选：行星轮观测（仅行星构型）
|
||||||
|
|
||||||
|
若模组有行星轮（本案例有 3 个），可额外记录某一行星轮的位置/速度 `q_p, qd_p`（`d.qpos[planet_dof]`）。
|
||||||
|
这是**可选项**——报告必填的只有输入/输出端；行星轮数量/命名随构型变，不属于固定指标。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 输出格式
|
||||||
|
|
||||||
|
### 4.1 逐时间步数据（CSV）
|
||||||
|
|
||||||
|
列名与关节名**解耦**（用固定的 `input_*` / `output_*`，不写死太阳轮/行星架），
|
||||||
|
前端按 `schema.input_joint` / `schema.output_joint` 填充即可：
|
||||||
|
|
||||||
|
| 列 | 内容 |
|
||||||
|
|---|---|
|
||||||
|
| `time` | 时间 [s] |
|
||||||
|
| `input_q, input_qd, input_qacc` | 输入位置/速度/加速度（`schema.input_joint`） |
|
||||||
|
| `q_ref, qd_ref` | 参考轨迹位置/速度 |
|
||||||
|
| `output_q, output_qd, output_qacc` | 输出位置/速度/加速度（`schema.output_joint`） |
|
||||||
|
| `planet_q, planet_qd` | （可选）行星轮位置/速度，仅行星构型且有记录时才有 |
|
||||||
|
| `tau_in, tau_out, power_out` | 输入/输出力矩、输出功率 |
|
||||||
|
|
||||||
|
> 本地 `simulate_report.py` 按固定顺序写出：`time, input_q, input_qd, input_qacc, q_ref,
|
||||||
|
> qd_ref, output_q, output_qd, output_qacc, [planet_q, planet_qd,] tau_in, tau_out, power_out`。
|
||||||
|
|
||||||
|
### 4.2 汇总报告（终端 / JSON）
|
||||||
|
|
||||||
|
按第 3 节的三类汇总指标输出：运动学（峰值、跟踪误差、传动比实测）、动力学（力矩、
|
||||||
|
功率、效率）、安全性（两类余量 + 过载判定）。
|
||||||
|
|
||||||
|
报告首行需注明**仿真模式**（`schema.simulation.mode`，`normal` / `overload`），并显示当前
|
||||||
|
使用的负载力矩与阻尼。当 `mode = overload` 时，额外产出一份 **过载报告**（`overload_report.txt`）：
|
||||||
|
|
||||||
|
| 指标 | 公式 | 单位 |
|
||||||
|
|---|---|---|
|
||||||
|
| 额定力矩限位 | `limit_trq = schema.limits.torque[1]` | N·m |
|
||||||
|
| 施加负载力矩 | `schema.simulation.load_torque` | N·m |
|
||||||
|
| 输入力矩峰值 | `max |τ_in|` | N·m |
|
||||||
|
| 力矩余量 | `(limit_trq − max|τ_in|) / limit_trq × 100%` | % |
|
||||||
|
| 位置跟踪误差 | `√ mean((q_in − q_ref)²)` | rad |
|
||||||
|
| 过载判定 | 余量 ≤ 0 → 已过载；0 < 余量 < 20% → 接近过载；否则未过载 | — |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 参考实现
|
||||||
|
|
||||||
|
本地参考实现见 [../scripts/simulate_report.py](../scripts/simulate_report.py)，它就是这份规范的可执行版本。
|
||||||
|
前端实现时应**以本规范为准**，脚本只作对照。
|
||||||
|
|
||||||
|
**两个易错点（实现时务必注意）：**
|
||||||
|
|
||||||
|
1. **输出力矩读约束反力，不读作动器力。** 齿轮耦合是 `<equality>` 约束，`τ_out` 取
|
||||||
|
`d.qfrc_constraint[output_dof]`；若读 `load_motor` 的 `actuator_force` 只会得到常数负载。
|
||||||
|
2. **稳态统计取后半段。** 参考轨迹从静止起跳会有初始瞬态尖峰，前半段不参与均值/效率计算。
|
||||||
+159
@@ -0,0 +1,159 @@
|
|||||||
|
# 接口 2 —— 关节模组配置 Schema
|
||||||
|
|
||||||
|
> 用途：给「浏览器端 WASM 仿真」一个**统一观测接口**。无论用户上传的是什么构型的关节模组
|
||||||
|
> （一级/多级、三个/四个行星轮、太阳轮+行星架、谐波、摆线…），后端把它归一化成这份 JSON，
|
||||||
|
> 前端只读这份 JSON 就能：找到输入端/输出端、知道减速比、拿到限位、按默认参数跑仿真。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. Schema 是什么 / 不是什么
|
||||||
|
|
||||||
|
| | 说明 |
|
||||||
|
|---|---|
|
||||||
|
| ✅ 是什么 | 关节模组的**对外观测契约**：输入关节、输出关节、减速比、限位、默认仿真参数 |
|
||||||
|
| ❌ 不是什么 | **不描述内部构型**（几级传动、几个行星轮、齿数、几何）。这些全部已经编码在 MJCF 里，前端不需要知道 |
|
||||||
|
| ❌ 不是用户上传的 | 这份 JSON 是**后端从 URDF 生成**的，不是用户填写的。用户上传的始终是「URDF + mesh」的 zip |
|
||||||
|
|
||||||
|
**为什么必须要有 schema？**
|
||||||
|
1. 报告里要测的输入/输出力矩，是「哪个关节」取决于构型。schema 告诉前端哪两个关节是输入/输出端。
|
||||||
|
2. 减速比的标称值（`gear_ratio`）可能来自 URDF 的 `<mimic>`，也可能是标定/手动填的，需要显式声明来源。
|
||||||
|
3. 前端要做统一的限位检查、统一的安全余量，就需要统一格式的限位。
|
||||||
|
|
||||||
|
> 一句话：**MJCF 管「怎么动」，schema 管「观测什么」，报告规范管「算出什么」。**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 字段定义（格式契约，定义一次）
|
||||||
|
|
||||||
|
| 字段 | 类型 | 单位 | 必填 | 含义 | 取值来源 |
|
||||||
|
|---|---|---|---|---|---|
|
||||||
|
| `module_id` | string | — | ✅ | 模组唯一标识 | 后端生成（如模型名） |
|
||||||
|
| `name` | string | — | | 显示名 | 后端生成 |
|
||||||
|
| `model` | string | — | ✅ | 要加载的 MJCF 文件名（自包含，网格与它同目录） | 转换工具输出 |
|
||||||
|
| `input_joint` | string | — | ✅ | **输入关节名**（电机端） | URDF 里唯一没有 `<mimic>` 的关节 |
|
||||||
|
| `output_joint` | string | — | ✅ | **输出关节名**（模组输出端） | URDF 里以正 multiplier 跟随输入的关节 |
|
||||||
|
| `gear_ratio` | number | — | ✅ | 标称减速比 = 输入 / 输出 = `1/multiplier` | 默认取 `<mimic multiplier>` 倒数；可标定覆盖 |
|
||||||
|
| `gear_ratio_source` | enum | — | ✅ | `mimic` / `calibrated` / `manual` | 减速比来源 |
|
||||||
|
| `limits.position` | [number, number] | rad | ✅ | 输入关节位置限位 `[lower, upper]` | URDF `<limit lower/upper>` |
|
||||||
|
| `limits.torque` | [number, number] | N·m | ✅ | 输入力矩限位 `[-T, +T]` | MJCF `<actuator ctrlrange>`（占位，待填真实值） |
|
||||||
|
| `limits.velocity` | [number, number] | rad/s | | 速度限位 | URDF `<limit velocity>`（若有） |
|
||||||
|
| `simulation.timestep` | number | s | ✅ | 仿真步长 | 默认 `0.001` |
|
||||||
|
| `simulation.duration` | number | s | ✅ | 仿真时长 | 默认 `4.0` |
|
||||||
|
| `simulation.kp` / `kd` | number | — | ✅ | 输入 PD 位置控制增益 | 默认 `20.0` / `0.3` |
|
||||||
|
| `simulation.amplitude` | number | rad | ✅ | 参考轨迹幅值 | 默认 `2π`（1 圈） |
|
||||||
|
| `simulation.frequency` | number | Hz | ✅ | 参考轨迹频率 | 默认 `0.25` |
|
||||||
|
| `simulation.load_torque` | number | N·m | ✅ | 输出端恒值负载（负 = 阻力） | 默认 `-3.0` |
|
||||||
|
| `simulation.damping` | number | N·m·s/rad | ✅ | 关节粘性阻尼 | 默认 `0.01` |
|
||||||
|
| `simulation.mode` | enum | — | ✅ | 仿真模式：`normal` / `overload` | 默认 `normal` |
|
||||||
|
|
||||||
|
> 所有带默认值的字段，前端都可以覆盖（用户调参）。`input_joint` / `output_joint` /
|
||||||
|
> `limits` / `gear_ratio` 是后端算好的「事实」，前端只读。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. 形式化 JSON Schema（供前端校验用，draft 2020-12）
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"$schema": "https://json-schema.org/draft/2020-12/schema",
|
||||||
|
"$id": "joint-module.schema.json",
|
||||||
|
"title": "关节模组配置",
|
||||||
|
"type": "object",
|
||||||
|
"required": ["module_id", "model", "input_joint", "output_joint",
|
||||||
|
"gear_ratio", "gear_ratio_source", "limits", "simulation"],
|
||||||
|
"additionalProperties": false,
|
||||||
|
"properties": {
|
||||||
|
"module_id": { "type": "string" },
|
||||||
|
"name": { "type": "string" },
|
||||||
|
"model": { "type": "string" },
|
||||||
|
"input_joint": { "type": "string" },
|
||||||
|
"output_joint": { "type": "string" },
|
||||||
|
"gear_ratio": { "type": "number", "exclusiveMinimum": 0 },
|
||||||
|
"gear_ratio_source": { "type": "string", "enum": ["mimic", "calibrated", "manual"] },
|
||||||
|
"limits": {
|
||||||
|
"type": "object",
|
||||||
|
"required": ["position", "torque"],
|
||||||
|
"properties": {
|
||||||
|
"position": { "type": "array", "items": { "type": "number" }, "minItems": 2, "maxItems": 2 },
|
||||||
|
"torque": { "type": "array", "items": { "type": "number" }, "minItems": 2, "maxItems": 2 },
|
||||||
|
"velocity": { "type": "array", "items": { "type": "number" }, "minItems": 2, "maxItems": 2 }
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"simulation": {
|
||||||
|
"type": "object",
|
||||||
|
"properties": {
|
||||||
|
"timestep": { "type": "number", "default": 0.001 },
|
||||||
|
"duration": { "type": "number", "default": 4.0 },
|
||||||
|
"kp": { "type": "number", "default": 20.0 },
|
||||||
|
"kd": { "type": "number", "default": 0.3 },
|
||||||
|
"amplitude": { "type": "number", "default": 6.283185307179586 },
|
||||||
|
"frequency": { "type": "number", "default": 0.25 },
|
||||||
|
"load_torque": { "type": "number", "default": -3.0 },
|
||||||
|
"damping": { "type": "number", "default": 0.01 },
|
||||||
|
"mode": { "type": "string", "enum": ["normal", "overload"], "default": "normal" }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 本案例的实例
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"module_id": "planetary_joint_split_motor_demo",
|
||||||
|
"name": "行星轮系分体电机关节模组",
|
||||||
|
"model": "planetary_joint_split_motor_demo.xml",
|
||||||
|
|
||||||
|
"input_joint": "sun_input_joint",
|
||||||
|
"output_joint": "carrier_output_joint",
|
||||||
|
|
||||||
|
"gear_ratio": 6.0,
|
||||||
|
"gear_ratio_source": "mimic",
|
||||||
|
|
||||||
|
"limits": {
|
||||||
|
"position": [-37.6991118431, 37.6991118431],
|
||||||
|
"torque": [-10.0, 10.0],
|
||||||
|
"velocity": [-6.28318530718, 6.28318530718]
|
||||||
|
},
|
||||||
|
|
||||||
|
"simulation": {
|
||||||
|
"timestep": 0.001,
|
||||||
|
"duration": 4.0,
|
||||||
|
"kp": 20.0,
|
||||||
|
"kd": 0.3,
|
||||||
|
"amplitude": 6.283185307179586,
|
||||||
|
"frequency": 0.25,
|
||||||
|
"load_torque": -3.0,
|
||||||
|
"damping": 0.01,
|
||||||
|
"mode": "normal"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
对照说明：
|
||||||
|
|
||||||
|
| 实例值 | 怎么来的 |
|
||||||
|
|---|---|
|
||||||
|
| `input_joint = sun_input_joint` | URDF 里 5 个关节中，唯一没有 `<mimic>` 的是 `sun_input_joint`（独立驱动源） |
|
||||||
|
| `output_joint = carrier_output_joint` | `<mimic joint="sun_input_joint" multiplier="0.16667">`，正 multiplier → 减速输出端 |
|
||||||
|
| `gear_ratio = 6.0` | `1 / 0.166666666666667 = 6`（1 级行星轮系，齿圈固定，太阳轮:行星架 = 6:1） |
|
||||||
|
| `limits.position = ±37.6991` | URDF `<limit lower="-37.6991118431" upper="37.6991118431">`（±6 圈 = ±12π） |
|
||||||
|
| `limits.torque = ±10` | MJCF `<actuator ctrlrange="-10 10">`（**占位**，待填真实电机额定值） |
|
||||||
|
| `limits.velocity = ±6.2832` | URDF `<limit velocity="6.28318530718">`（1 圈/秒） |
|
||||||
|
| `simulation.*` | 报告规范的默认场景参数，见 [report_spec.md](report_spec.md) |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 减速比来源的优先级（多来源怎么取）
|
||||||
|
|
||||||
|
后端生成 schema 时，`gear_ratio` 按下面顺序确定，并在 `gear_ratio_source` 里记录用的是哪个：
|
||||||
|
|
||||||
|
1. **`mimic`** —— URDF `<mimic multiplier>` 的倒数（最优先，本案例）。
|
||||||
|
2. **`calibrated`** —— 跑一次仿真，`mean(q_out / q_in)` 标定得到（无 `<mimic>` 或传动力不可靠时）。
|
||||||
|
3. **`manual`** —— 前端手动填（都没有时兜底）。
|
||||||
|
|
||||||
|
> 减速比只进 schema 与报告，**不进 MJCF**——MJCF 里的传动关系由 `<equality>` 精确表达，
|
||||||
|
> schema 里的 `gear_ratio` 只是「标称参考值」，用于报告的效率/理想力矩计算。
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,34 @@
|
|||||||
|
{
|
||||||
|
"module_id": "planetary_joint_split_motor_demo",
|
||||||
|
"name": "planetary_joint_split_motor_demo",
|
||||||
|
"model": "planetary_joint_split_motor_demo.xml",
|
||||||
|
"input_joint": "sun_input_joint",
|
||||||
|
"output_joint": "carrier_output_joint",
|
||||||
|
"gear_ratio": 5.9999999999999885,
|
||||||
|
"gear_ratio_source": "mimic",
|
||||||
|
"limits": {
|
||||||
|
"position": [
|
||||||
|
-37.6991118431,
|
||||||
|
37.6991118431
|
||||||
|
],
|
||||||
|
"torque": [
|
||||||
|
-10.0,
|
||||||
|
10.0
|
||||||
|
],
|
||||||
|
"velocity": [
|
||||||
|
-6.28318530718,
|
||||||
|
6.28318530718
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"simulation": {
|
||||||
|
"timestep": 0.001,
|
||||||
|
"duration": 4.0,
|
||||||
|
"kp": 20.0,
|
||||||
|
"kd": 0.3,
|
||||||
|
"amplitude": 6.283185307179586,
|
||||||
|
"frequency": 0.25,
|
||||||
|
"load_torque": -3.0,
|
||||||
|
"damping": 0.01,
|
||||||
|
"mode": "normal"
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,68 @@
|
|||||||
|
<mujoco model="planetary_joint_split_motor_demo">
|
||||||
|
<!-- 由 urdf_to_mjcf.py 自动生成：只补质量/惯量/作动器，不改变运动学/动力学/传动关系 -->
|
||||||
|
<compiler angle="radian" meshdir="meshes"/>
|
||||||
|
|
||||||
|
<option timestep="0.001" gravity="0 0 -9.81"/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<joint damping="0.01" armature="0.0005"/>
|
||||||
|
<geom contype="0" conaffinity="0"/>
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh name="carrier_link" file="carrier_link.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="fixed_structure" file="fixed_structure.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="motor_rotor" file="motor_rotor.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="motor_stator" file="motor_stator_decimated.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="planet_0_link" file="planet_0_link.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="planet_1_link" file="planet_1_link.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="planet_2_link" file="planet_2_link.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
<mesh name="sun_drive" file="sun_drive.stl" scale="0.001 0.001 0.001"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<body name="base_link">
|
||||||
|
<inertial pos="0.0257167 7.22096e-05 -0.000297192" mass="0.551159" fullinertia="0.000656887 0.00056339 0.000561847 7.94201e-07 -3.24687e-06 -4.98415e-07"/>
|
||||||
|
<geom type="mesh" mesh="fixed_structure" rgba="0.62 0.68 0.72 1"/>
|
||||||
|
<geom type="mesh" mesh="motor_stator" rgba="0.3 0.34 0.38 1"/>
|
||||||
|
<body name="sun_link">
|
||||||
|
<inertial pos="0.0313876 -2.35229e-07 1.25343e-06" mass="0.277539" fullinertia="0.000266278 0.000167853 0.000167874 -1.18454e-09 1.34599e-10 -3.3208e-09"/>
|
||||||
|
<joint name="sun_input_joint" type="hinge" axis="1 0 0" range="-37.6991118431 37.6991118431"/>
|
||||||
|
<geom type="mesh" mesh="sun_drive" rgba="0.95 0.62 0.16 1"/>
|
||||||
|
<geom type="mesh" mesh="motor_rotor" rgba="0.8 0.33 0.16 1"/>
|
||||||
|
</body>
|
||||||
|
<body name="carrier_link">
|
||||||
|
<inertial pos="0.060147 -8.40334e-08 -3.49296e-08" mass="0.163946" fullinertia="8.63028e-05 4.65499e-05 4.65516e-05 -9.27503e-11 -4.76284e-12 3.77741e-10"/>
|
||||||
|
<joint name="carrier_output_joint" type="hinge" axis="1 0 0" range="-37.6991118431 37.6991118431"/>
|
||||||
|
<geom type="mesh" mesh="carrier_link" rgba="0.28 0.56 0.82 1"/>
|
||||||
|
<body name="planet_0_link" pos="0.0535 -0.0239016371622 -0.00217065450165">
|
||||||
|
<inertial pos="-0.00192323 -3.97574e-13 -1.58818e-10" mass="0.0159851" fullinertia="2.47374e-06 1.2559e-06 1.2559e-06 -2.46924e-15 1.66624e-17 1.84976e-14"/>
|
||||||
|
<joint name="planet_0_spin_joint" type="hinge" axis="1 0 0" range="-37.6991118431 37.6991118431"/>
|
||||||
|
<geom type="mesh" mesh="planet_0_link" rgba="0.76 0.78 0.8 1" pos="-0.0535 0.0239016371622 0.00217065450165"/>
|
||||||
|
</body>
|
||||||
|
<body name="planet_1_link" pos="0.0535 0.0138306605224 -0.0196140977237">
|
||||||
|
<inertial pos="-0.00192323 -1.31616e-10 3.93298e-10" mass="0.0159851" fullinertia="2.47374e-06 1.2559e-06 1.2559e-06 2.59167e-16 2.08757e-15 3.78659e-14"/>
|
||||||
|
<joint name="planet_1_spin_joint" type="hinge" axis="1 0 0" range="-37.6991118431 37.6991118431"/>
|
||||||
|
<geom type="mesh" mesh="planet_1_link" rgba="0.76 0.78 0.8 1" pos="-0.0535 -0.0138306605224 0.0196140977237"/>
|
||||||
|
</body>
|
||||||
|
<body name="planet_2_link" pos="0.0535 0.0100709766398 0.0217847522253">
|
||||||
|
<inertial pos="-0.00192323 -9.59476e-10 1.01754e-10" mass="0.0159851" fullinertia="2.47374e-06 1.2559e-06 1.2559e-06 2.46116e-15 -3.08763e-15 4.97491e-15"/>
|
||||||
|
<joint name="planet_2_spin_joint" type="hinge" axis="1 0 0" range="-37.6991118431 37.6991118431"/>
|
||||||
|
<geom type="mesh" mesh="planet_2_link" rgba="0.76 0.78 0.8 1" pos="-0.0535 -0.0100709766398 -0.0217847522253"/>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<equality>
|
||||||
|
<joint joint1="carrier_output_joint" joint2="sun_input_joint" polycoef="0 0.166666666666667 0 0 0" solref="0.002 1" solimp="0.9 0.95 0.0001"/>
|
||||||
|
<joint joint1="planet_0_spin_joint" joint2="sun_input_joint" polycoef="0 -0.416666666666667 0 0 0" solref="0.002 1" solimp="0.9 0.95 0.0001"/>
|
||||||
|
<joint joint1="planet_1_spin_joint" joint2="sun_input_joint" polycoef="0 -0.416666666666667 0 0 0" solref="0.002 1" solimp="0.9 0.95 0.0001"/>
|
||||||
|
<joint joint1="planet_2_spin_joint" joint2="sun_input_joint" polycoef="0 -0.416666666666667 0 0 0" solref="0.002 1" solimp="0.9 0.95 0.0001"/>
|
||||||
|
</equality>
|
||||||
|
|
||||||
|
<actuator>
|
||||||
|
<motor name="input_motor" joint="sun_input_joint" gear="1" ctrlrange="-10 10"/>
|
||||||
|
<motor name="load_motor" joint="carrier_output_joint" gear="1" ctrlrange="-1e+06 1e+06"/>
|
||||||
|
</actuator>
|
||||||
|
</mujoco>
|
||||||
@@ -0,0 +1,33 @@
|
|||||||
|
================================================================
|
||||||
|
关节模组仿真报告
|
||||||
|
================================================================
|
||||||
|
模型 : planetary_joint_split_motor_demo.xml
|
||||||
|
输入端 : sun_input_joint 输出端: carrier_output_joint
|
||||||
|
标称减速比 1 : 6.0000（输出 = 输入/6.0000）
|
||||||
|
仿真模式 : normal
|
||||||
|
负载力矩 : -3.0000 N·m 阻尼: 0.01
|
||||||
|
|
||||||
|
[1] 运动学（跟踪精度）
|
||||||
|
输入位置峰值 : 6.3087 rad (参考 6.2832)
|
||||||
|
位置跟踪误差 (RMS) : 0.0251 rad
|
||||||
|
速度跟踪误差 (RMS) : 0.0727 rad/s
|
||||||
|
输出位置峰值 : 1.0532 rad (应为 1.0472)
|
||||||
|
传动比实测 : 0.166667 (期望 0.166667)
|
||||||
|
|
||||||
|
[2] 动力学（力矩与功率）
|
||||||
|
输入力矩 (稳态均值) : 0.5102 N·m
|
||||||
|
输出力矩 (稳态均值) : 3.0010 N·m
|
||||||
|
理想输出 = 输入×6.0000 : 3.0611 N·m
|
||||||
|
力矩损失 : 0.0601 N·m
|
||||||
|
效率 η = 输出/(输入×6.0000) : 98.04 %
|
||||||
|
输入力矩峰值 (瞬态) : 0.7543 N·m
|
||||||
|
输出功率峰值 : 7.5876 W
|
||||||
|
|
||||||
|
[3] 安全性（安全余量）
|
||||||
|
位置余量 (限位 ±37.6991 rad) : 83.27 %
|
||||||
|
力矩余量 (限位 ±10.0000 N·m) : 92.46 %
|
||||||
|
过载判定 : ✓ 余量充足
|
||||||
|
|
||||||
|
================================================================
|
||||||
|
已写出 /home/csj/planetary_joint_split_motor_demo_urdf/output/timeseries.csv
|
||||||
|
================================================================
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 85 KiB |
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,189 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
关节模组 schema 生成器（接口 2 的后端实现）
|
||||||
|
====================================================================
|
||||||
|
|
||||||
|
从一份关节模组 URDF 生成前端要的配置 JSON（schema），即 [schema.md](schema.md)
|
||||||
|
定义的观测契约：输入/输出关节名、减速比、限位、默认仿真参数。
|
||||||
|
|
||||||
|
输入/输出关节由用户**显式指定**（--input / --output）。因为自动识别在真实 URDF 上
|
||||||
|
不可靠（见 docs/architecture.md 说明：多级级联有多个正 multiplier 的 mimic，且 fixed 关节也没有
|
||||||
|
mimic，程序分不清哪级是末端输出）。
|
||||||
|
|
||||||
|
减速比来源优先级（同 schema.md 第 5 节）：
|
||||||
|
1. mimic —— 输出关节 <mimic multiplier> 的倒数（默认）
|
||||||
|
2. manual —— --ratio 手动指定（URDF 无 mimic 时）
|
||||||
|
|
||||||
|
用法（在 scripts/ 目录下运行）：
|
||||||
|
python3 generate_schema.py \
|
||||||
|
--urdf ../urdf/planetary_joint_split_motor_demo.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint \
|
||||||
|
--model planetary_joint_split_motor_demo.xml \
|
||||||
|
--out planetary_joint_split_motor_demo.json
|
||||||
|
|
||||||
|
依赖：仅标准库（xml.etree / json / argparse），不需要 trimesh / mujoco。
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import os
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
|
|
||||||
|
# 默认仿真参数（与 schema.md / report_spec.md 保持一致）
|
||||||
|
DEFAULTS = {
|
||||||
|
"timestep": 0.001,
|
||||||
|
"duration": 4.0,
|
||||||
|
"kp": 20.0,
|
||||||
|
"kd": 0.3,
|
||||||
|
"amplitude": 2.0 * math.pi,
|
||||||
|
"frequency": 0.25,
|
||||||
|
"load_torque": -3.0,
|
||||||
|
"damping": 0.01,
|
||||||
|
}
|
||||||
|
# 输入关节无 <limit>（continuous）时的位置限位占位：±6 圈 = ±12π
|
||||||
|
DEFAULT_POSITION_LIMIT = 12.0 * math.pi
|
||||||
|
# 输入力矩限位占位（应填真实电机额定值，见 schema.md limits.torque）
|
||||||
|
DEFAULT_TORQUE_LIMIT = 10.0
|
||||||
|
|
||||||
|
|
||||||
|
def _float(attr):
|
||||||
|
try:
|
||||||
|
return float(attr)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def parse_joints(root):
|
||||||
|
"""返回 {关节名: <joint> 元素}。"""
|
||||||
|
return {j.get("name"): j for j in root.findall("joint")}
|
||||||
|
|
||||||
|
|
||||||
|
def get_mimic(joint_el):
|
||||||
|
"""返回 (joint, multiplier, offset) 或 None。"""
|
||||||
|
m = joint_el.find("mimic")
|
||||||
|
if m is None:
|
||||||
|
return None
|
||||||
|
return {
|
||||||
|
"joint": m.get("joint"),
|
||||||
|
"multiplier": _float(m.get("multiplier")),
|
||||||
|
"offset": _float(m.get("offset", 0.0)),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def get_limit(joint_el):
|
||||||
|
"""返回 (lower, upper, velocity) 或 None。关节无 <limit>（continuous）返回 None。"""
|
||||||
|
lim = joint_el.find("limit")
|
||||||
|
if lim is None:
|
||||||
|
return None
|
||||||
|
return {
|
||||||
|
"lower": _float(lim.get("lower")),
|
||||||
|
"upper": _float(lim.get("upper")),
|
||||||
|
"velocity": _float(lim.get("velocity")),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser(description="关节模组 URDF → schema JSON")
|
||||||
|
ap.add_argument("--urdf", required=True, help="输入 URDF 路径")
|
||||||
|
ap.add_argument("--input", required=True, help="输入关节名（电机端，显式指定）")
|
||||||
|
ap.add_argument("--output", required=True, help="输出关节名（模组末端，显式指定）")
|
||||||
|
ap.add_argument("--model", default=None,
|
||||||
|
help="要引用进 schema.model 的 MJCF 文件名（默认 <robot名>.xml）")
|
||||||
|
ap.add_argument("--out", default=None, help="schema 输出 JSON 路径（默认 <robot名>.json）")
|
||||||
|
ap.add_argument("--module-id", default=None, help="module_id（默认 robot 名）")
|
||||||
|
ap.add_argument("--name", default=None, help="显示名（默认 robot 名）")
|
||||||
|
ap.add_argument("--ratio", type=float, default=None,
|
||||||
|
help="手动指定减速比（URDF 无 mimic 时用，gear_ratio_source=manual）")
|
||||||
|
ap.add_argument("--torque-limit", type=float, default=DEFAULT_TORQUE_LIMIT,
|
||||||
|
help="输入力矩限位 [N·m]，默认 ±10（占位）")
|
||||||
|
ap.add_argument("--position-limit", type=float, default=DEFAULT_POSITION_LIMIT,
|
||||||
|
help="输入关节无 <limit> 时的位置限位 ±rad，默认 ±12π")
|
||||||
|
for k, v in DEFAULTS.items():
|
||||||
|
ap.add_argument(f"--{k.replace('_', '-')}", type=float, default=v,
|
||||||
|
help=f"仿真参数 {k}（默认 {v}）")
|
||||||
|
ap.add_argument("--mode", choices=["normal", "overload"], default="normal",
|
||||||
|
help="仿真模式（normal / overload，默认 normal）")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
tree = ET.parse(args.urdf)
|
||||||
|
robot = tree.getroot()
|
||||||
|
robot_name = robot.get("name")
|
||||||
|
joints = parse_joints(robot)
|
||||||
|
|
||||||
|
if args.input not in joints:
|
||||||
|
ap.error(f"输入关节 '{args.input}' 在 URDF 中不存在；可用关节：{sorted(joints)}")
|
||||||
|
if args.output not in joints:
|
||||||
|
ap.error(f"输出关节 '{args.output}' 在 URDF 中不存在；可用关节：{sorted(joints)}")
|
||||||
|
|
||||||
|
# ---- 减速比 ----
|
||||||
|
mimic = get_mimic(joints[args.output])
|
||||||
|
gear_ratio = None
|
||||||
|
gear_ratio_source = None
|
||||||
|
if mimic is not None and mimic["multiplier"] not in (None, 0.0):
|
||||||
|
gear_ratio = 1.0 / abs(mimic["multiplier"])
|
||||||
|
gear_ratio_source = "mimic"
|
||||||
|
elif args.ratio is not None:
|
||||||
|
gear_ratio = args.ratio
|
||||||
|
gear_ratio_source = "manual"
|
||||||
|
else:
|
||||||
|
ap.error("输出关节没有 <mimic>，请用 --ratio 手动指定减速比")
|
||||||
|
|
||||||
|
# ---- 限位 ----
|
||||||
|
in_limit = get_limit(joints[args.input])
|
||||||
|
if in_limit is not None and in_limit["lower"] is not None and in_limit["upper"] is not None:
|
||||||
|
pos_lim = [in_limit["lower"], in_limit["upper"]]
|
||||||
|
else:
|
||||||
|
pos_lim = [-args.position_limit, args.position_limit]
|
||||||
|
velocity = in_limit["velocity"] if in_limit is not None else None
|
||||||
|
|
||||||
|
module_id = args.module_id or robot_name
|
||||||
|
model = args.model or (module_id + ".xml")
|
||||||
|
out_path = args.out or (module_id + ".json")
|
||||||
|
|
||||||
|
schema = {
|
||||||
|
"module_id": module_id,
|
||||||
|
"name": args.name or robot_name,
|
||||||
|
"model": model,
|
||||||
|
"input_joint": args.input,
|
||||||
|
"output_joint": args.output,
|
||||||
|
"gear_ratio": gear_ratio,
|
||||||
|
"gear_ratio_source": gear_ratio_source,
|
||||||
|
"limits": {
|
||||||
|
"position": pos_lim,
|
||||||
|
"torque": [-args.torque_limit, args.torque_limit],
|
||||||
|
},
|
||||||
|
"simulation": {
|
||||||
|
"timestep": args.timestep,
|
||||||
|
"duration": args.duration,
|
||||||
|
"kp": args.kp,
|
||||||
|
"kd": args.kd,
|
||||||
|
"amplitude": args.amplitude,
|
||||||
|
"frequency": args.frequency,
|
||||||
|
"load_torque": args.load_torque,
|
||||||
|
"damping": args.damping,
|
||||||
|
"mode": args.mode,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
if velocity is not None:
|
||||||
|
schema["limits"]["velocity"] = [-velocity, velocity]
|
||||||
|
|
||||||
|
with open(out_path, "w", encoding="utf-8") as f:
|
||||||
|
json.dump(schema, f, ensure_ascii=False, indent=2)
|
||||||
|
f.write("\n")
|
||||||
|
|
||||||
|
print(f"已生成 schema: {os.path.abspath(out_path)}")
|
||||||
|
print(f" module_id : {module_id}")
|
||||||
|
print(f" input_joint : {args.input}")
|
||||||
|
print(f" output_joint: {args.output}")
|
||||||
|
print(f" gear_ratio : {gear_ratio:.6f} (source={gear_ratio_source})")
|
||||||
|
print(f" position_lim: {pos_lim[0]:.4f} ~ {pos_lim[1]:.4f} rad")
|
||||||
|
print(f" torque_lim : ±{args.torque_limit} N·m")
|
||||||
|
print(f" mode : {args.mode}")
|
||||||
|
print(f" load_torque : {args.load_torque} N·m")
|
||||||
|
print(f" damping : {args.damping}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,396 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
关节模组通用仿真 + 报告脚本（边可视化边计算）
|
||||||
|
====================================================================
|
||||||
|
|
||||||
|
对任意一个关节模组 MJCF 跑仿真并产出报告。报告逻辑固定（见 report_spec.md），
|
||||||
|
随构型变的只有：输入/输出关节名、减速比、限位、仿真参数 —— 这些统一从 schema JSON
|
||||||
|
读入（schema 由 generate_schema.py 生成）。
|
||||||
|
|
||||||
|
两种用法：
|
||||||
|
1) 通用（读 schema）：
|
||||||
|
python3 simulate_report.py --schema <模块>.json --headless --plot
|
||||||
|
2) demo（不传 --schema，回退到本案例默认值，保持兼容）：
|
||||||
|
python3 simulate_report.py # 弹窗 + 打印报告 + timeseries.csv
|
||||||
|
python3 simulate_report.py --headless # 无窗口
|
||||||
|
python3 simulate_report.py --plot # 追加 report_curves.png
|
||||||
|
|
||||||
|
也可用 --xml / --input / --output / --ratio 覆盖 schema 里的个别字段（快速调试用）。
|
||||||
|
|
||||||
|
报告测三类数据（report_spec.md 第 3 节）：
|
||||||
|
1. 运动学 —— 输入/输出位置、速度、加速度、传动比实测
|
||||||
|
2. 动力学 —— 输入/输出力矩、功率、效率
|
||||||
|
3. 安全性 —— 位置余量、力矩余量、过载判定
|
||||||
|
|
||||||
|
输出文件写在 MJCF 所在目录：timeseries.csv（逐时间步）、report.txt（仿真报告）、
|
||||||
|
report_curves.png（--plot）。
|
||||||
|
|
||||||
|
依赖：mujoco、numpy（绘图需 matplotlib）。
|
||||||
|
"""
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import mujoco
|
||||||
|
import mujoco.viewer # noqa: F401 (确保 viewer 子模块可用)
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------ demo 默认值（不传 --schema 时用） ------------------------------
|
||||||
|
DEMO = {
|
||||||
|
"xml": "planetary_joint_split_motor_demo.xml",
|
||||||
|
"input_joint": "sun_input_joint",
|
||||||
|
"output_joint": "carrier_output_joint",
|
||||||
|
"planet_joint": "planet_0_spin_joint",
|
||||||
|
"gear_ratio": 6.0,
|
||||||
|
"duration": 4.0,
|
||||||
|
"dt": 0.001,
|
||||||
|
"kp": 20.0,
|
||||||
|
"kd": 0.3,
|
||||||
|
"amplitude": 2.0 * np.pi,
|
||||||
|
"frequency": 0.25,
|
||||||
|
"load_torque": -3.0,
|
||||||
|
"damping": 0.01,
|
||||||
|
"mode": "normal",
|
||||||
|
"pos_lim": None, # None → 从 MJCF 读
|
||||||
|
"trq_lim": None,
|
||||||
|
}
|
||||||
|
|
||||||
|
# 参考轨迹平滑启动时长 [s]：让参考从 0 位置、0 速度起跳，消除 t=0 的微分项冲击
|
||||||
|
RAMP_TIME = 0.5
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------ 工具函数 ------------------------------
|
||||||
|
def jid(m, name):
|
||||||
|
return mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, name)
|
||||||
|
|
||||||
|
|
||||||
|
def aid(m, name):
|
||||||
|
return mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||||
|
|
||||||
|
|
||||||
|
def margin(limit, peak):
|
||||||
|
"""安全余量 = (限位值 - |峰值|) / 限位值 × 100%"""
|
||||||
|
if limit is None or abs(limit) < 1e-12:
|
||||||
|
return float("nan")
|
||||||
|
return (abs(limit) - abs(peak)) / abs(limit) * 100.0
|
||||||
|
|
||||||
|
|
||||||
|
def _setup_cjk_font(plt):
|
||||||
|
"""配置中文字体，避免图中中文标签显示成方框（tofu）。"""
|
||||||
|
candidates = [
|
||||||
|
"Noto Sans CJK SC", "Noto Sans CJK JP", "AR PL UMing CN",
|
||||||
|
"AR PL UKai CN", "Droid Sans Fallback",
|
||||||
|
]
|
||||||
|
import matplotlib.font_manager as fm
|
||||||
|
available = {f.name for f in fm.fontManager.ttflist}
|
||||||
|
chosen = next((c for c in candidates if c in available), None)
|
||||||
|
if chosen is not None:
|
||||||
|
plt.rcParams["font.sans-serif"] = [chosen, "DejaVu Sans"]
|
||||||
|
print(f"已启用中文字体：{chosen}")
|
||||||
|
else:
|
||||||
|
print("警告：未找到中文字体，图中中文可能显示为方框")
|
||||||
|
plt.rcParams["axes.unicode_minus"] = False
|
||||||
|
|
||||||
|
|
||||||
|
def load_config(args):
|
||||||
|
"""从 schema / CLI 参数合成一份运行配置。返回 (cfg, schema_dir)。"""
|
||||||
|
cfg = dict(DEMO)
|
||||||
|
schema_dir = None
|
||||||
|
|
||||||
|
if args.schema:
|
||||||
|
with open(args.schema, encoding="utf-8") as f:
|
||||||
|
s = json.load(f)
|
||||||
|
schema_dir = os.path.dirname(os.path.abspath(args.schema))
|
||||||
|
cfg["xml"] = s.get("model", cfg["xml"])
|
||||||
|
cfg["input_joint"] = s.get("input_joint", cfg["input_joint"])
|
||||||
|
cfg["output_joint"] = s.get("output_joint", cfg["output_joint"])
|
||||||
|
cfg["gear_ratio"] = float(s.get("gear_ratio", cfg["gear_ratio"]))
|
||||||
|
lim = s.get("limits", {})
|
||||||
|
pos = lim.get("position")
|
||||||
|
trq = lim.get("torque")
|
||||||
|
cfg["pos_lim"] = float(pos[1]) if pos else None
|
||||||
|
cfg["trq_lim"] = float(trq[1]) if trq else None
|
||||||
|
sim = s.get("simulation", {})
|
||||||
|
cfg["duration"] = float(sim.get("duration", cfg["duration"]))
|
||||||
|
cfg["dt"] = float(sim.get("timestep", cfg["dt"]))
|
||||||
|
cfg["kp"] = float(sim.get("kp", cfg["kp"]))
|
||||||
|
cfg["kd"] = float(sim.get("kd", cfg["kd"]))
|
||||||
|
cfg["amplitude"] = float(sim.get("amplitude", cfg["amplitude"]))
|
||||||
|
cfg["frequency"] = float(sim.get("frequency", cfg["frequency"]))
|
||||||
|
cfg["load_torque"] = float(sim.get("load_torque", cfg["load_torque"]))
|
||||||
|
cfg["damping"] = float(sim.get("damping", cfg["damping"]))
|
||||||
|
cfg["mode"] = sim.get("mode", cfg["mode"])
|
||||||
|
# schema 不含行星轮（可选观测），通用模式默认不记录行星轮
|
||||||
|
cfg["planet_joint"] = None
|
||||||
|
|
||||||
|
# CLI 覆盖
|
||||||
|
if args.xml:
|
||||||
|
cfg["xml"] = args.xml
|
||||||
|
if args.input:
|
||||||
|
cfg["input_joint"] = args.input
|
||||||
|
if args.output:
|
||||||
|
cfg["output_joint"] = args.output
|
||||||
|
if args.ratio is not None:
|
||||||
|
cfg["gear_ratio"] = args.ratio
|
||||||
|
if args.planet:
|
||||||
|
cfg["planet_joint"] = args.planet
|
||||||
|
if args.mode:
|
||||||
|
cfg["mode"] = args.mode
|
||||||
|
if args.load_torque is not None:
|
||||||
|
cfg["load_torque"] = args.load_torque
|
||||||
|
|
||||||
|
# XML 路径：相对路径相对 schema 所在目录（无 schema 则相对 CWD）解析
|
||||||
|
xml = cfg["xml"]
|
||||||
|
if not os.path.isabs(xml):
|
||||||
|
base = schema_dir if schema_dir else os.getcwd()
|
||||||
|
xml = os.path.abspath(os.path.join(base, xml))
|
||||||
|
cfg["xml"] = xml
|
||||||
|
cfg["out_dir"] = os.path.dirname(xml)
|
||||||
|
return cfg
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------ 主流程 ------------------------------
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser(description="关节模组通用仿真 + 报告")
|
||||||
|
ap.add_argument("--schema", default=None, help="schema JSON 路径")
|
||||||
|
ap.add_argument("--xml", default=None, help="MJCF XML 路径（覆盖 schema.model）")
|
||||||
|
ap.add_argument("--input", default=None, help="输入关节名（覆盖 schema）")
|
||||||
|
ap.add_argument("--output", default=None, help="输出关节名（覆盖 schema）")
|
||||||
|
ap.add_argument("--ratio", type=float, default=None, help="减速比（覆盖 schema）")
|
||||||
|
ap.add_argument("--planet", default=None, help="可选：要记录的行星轮关节名")
|
||||||
|
ap.add_argument("--mode", choices=["normal", "overload"], default=None,
|
||||||
|
help="仿真模式（normal / overload，默认读 schema）")
|
||||||
|
ap.add_argument("--load-torque", type=float, default=None,
|
||||||
|
help="输出端负载 [N·m]（覆盖 schema）")
|
||||||
|
ap.add_argument("--headless", action="store_true", help="无窗口")
|
||||||
|
ap.add_argument("--fast", action="store_true", help="弹窗但不按真实时间步进")
|
||||||
|
ap.add_argument("--plot", action="store_true", help="导出 PNG 曲线（需 matplotlib）")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
cfg = load_config(args)
|
||||||
|
|
||||||
|
m = mujoco.MjModel.from_xml_path(cfg["xml"])
|
||||||
|
d = mujoco.MjData(m)
|
||||||
|
|
||||||
|
in_j, out_j = jid(m, cfg["input_joint"]), jid(m, cfg["output_joint"])
|
||||||
|
if in_j < 0 or out_j < 0:
|
||||||
|
sys.exit("找不到输入/输出关节，请检查 schema 的 input_joint / output_joint")
|
||||||
|
in_dof = m.jnt_dofadr[in_j]
|
||||||
|
out_dof = m.jnt_dofadr[out_j]
|
||||||
|
|
||||||
|
in_m, load_m = aid(m, "input_motor"), aid(m, "load_motor")
|
||||||
|
if in_m < 0 or load_m < 0:
|
||||||
|
sys.exit("找不到 input_motor / load_motor 作动器（应由 urdf_to_mjcf.py 生成）")
|
||||||
|
|
||||||
|
p_j = jid(m, cfg["planet_joint"]) if cfg["planet_joint"] else -1
|
||||||
|
p_dof = m.jnt_dofadr[p_j] if p_j >= 0 else -1
|
||||||
|
has_planet = p_dof >= 0
|
||||||
|
|
||||||
|
# 限位：优先 schema（cfg.pos_lim/trq_lim），否则从 MJCF 读（demo 回退路径）
|
||||||
|
pos_lim = cfg["pos_lim"] if cfg["pos_lim"] is not None else m.jnt_range[in_j][1]
|
||||||
|
trq_lim = cfg["trq_lim"] if cfg["trq_lim"] is not None else m.actuator_ctrlrange[in_m][1]
|
||||||
|
|
||||||
|
DT = cfg["dt"]
|
||||||
|
nsteps = int(cfg["duration"] / DT)
|
||||||
|
t_arr = np.zeros(nsteps)
|
||||||
|
q_in, qd_in, qacc_in = np.zeros(nsteps), np.zeros(nsteps), np.zeros(nsteps)
|
||||||
|
q_out, qd_out, qacc_out = np.zeros(nsteps), np.zeros(nsteps), np.zeros(nsteps)
|
||||||
|
q_p, qd_p = np.zeros(nsteps), np.zeros(nsteps)
|
||||||
|
q_ref_arr, qd_ref_arr = np.zeros(nsteps), np.zeros(nsteps)
|
||||||
|
tau_in_arr, tau_out_arr, power_arr = np.zeros(nsteps), np.zeros(nsteps), np.zeros(nsteps)
|
||||||
|
|
||||||
|
mujoco.mj_resetData(m, d)
|
||||||
|
|
||||||
|
viewer = None
|
||||||
|
if not args.headless:
|
||||||
|
try:
|
||||||
|
viewer = mujoco.viewer.launch_passive(m, d)
|
||||||
|
print("已打开可视化窗口（关闭窗口可提前结束仿真）")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"无法打开可视化窗口（可能缺显示器/X11）：{e}")
|
||||||
|
print("退回无头模式继续计算…")
|
||||||
|
viewer = None
|
||||||
|
|
||||||
|
w = 2.0 * np.pi * cfg["frequency"]
|
||||||
|
t_start = time.perf_counter()
|
||||||
|
try:
|
||||||
|
for k in range(nsteps):
|
||||||
|
t = k * DT
|
||||||
|
# 升余弦包络平滑启动：参考从 0 位置、0 速度起跳，避免 t=0 速度跳变
|
||||||
|
if t < RAMP_TIME:
|
||||||
|
env = 0.5 * (1.0 - np.cos(np.pi * t / RAMP_TIME))
|
||||||
|
denv = 0.5 * np.pi / RAMP_TIME * np.sin(np.pi * t / RAMP_TIME)
|
||||||
|
else:
|
||||||
|
env = 1.0
|
||||||
|
denv = 0.0
|
||||||
|
q_ref = cfg["amplitude"] * np.sin(w * t) * env
|
||||||
|
qd_ref = cfg["amplitude"] * (w * np.cos(w * t) * env + np.sin(w * t) * denv)
|
||||||
|
|
||||||
|
tau_in = cfg["kp"] * (q_ref - d.qpos[in_dof]) + cfg["kd"] * (qd_ref - d.qvel[in_dof])
|
||||||
|
d.ctrl[in_m] = tau_in
|
||||||
|
d.ctrl[load_m] = cfg["load_torque"]
|
||||||
|
mujoco.mj_step(m, d)
|
||||||
|
|
||||||
|
t_arr[k] = t
|
||||||
|
q_in[k] = d.qpos[in_dof]; qd_in[k] = d.qvel[in_dof]; qacc_in[k] = d.qacc[in_dof]
|
||||||
|
q_out[k] = d.qpos[out_dof]; qd_out[k] = d.qvel[out_dof]; qacc_out[k] = d.qacc[out_dof]
|
||||||
|
if has_planet:
|
||||||
|
q_p[k] = d.qpos[p_dof]; qd_p[k] = d.qvel[p_dof]
|
||||||
|
q_ref_arr[k] = q_ref; qd_ref_arr[k] = qd_ref
|
||||||
|
tau_in_arr[k] = d.actuator_force[in_m]
|
||||||
|
tau_out_arr[k] = d.qfrc_constraint[out_dof]
|
||||||
|
power_arr[k] = tau_out_arr[k] * qd_out[k]
|
||||||
|
|
||||||
|
if viewer is not None:
|
||||||
|
viewer.sync()
|
||||||
|
if not args.fast:
|
||||||
|
elapsed = time.perf_counter() - t_start
|
||||||
|
sim_t = (k + 1) * DT
|
||||||
|
if elapsed < sim_t:
|
||||||
|
time.sleep(sim_t - elapsed)
|
||||||
|
if not viewer.is_running():
|
||||||
|
print("窗口已关闭，提前结束仿真")
|
||||||
|
nsteps = k + 1
|
||||||
|
break
|
||||||
|
finally:
|
||||||
|
if viewer is not None:
|
||||||
|
viewer.close()
|
||||||
|
|
||||||
|
t_arr = t_arr[:nsteps]; q_in = q_in[:nsteps]; qd_in = qd_in[:nsteps]; qacc_in = qacc_in[:nsteps]
|
||||||
|
q_out = q_out[:nsteps]; qd_out = qd_out[:nsteps]; qacc_out = qacc_out[:nsteps]
|
||||||
|
q_p = q_p[:nsteps]; qd_p = qd_p[:nsteps]
|
||||||
|
q_ref_arr = q_ref_arr[:nsteps]; qd_ref_arr = qd_ref_arr[:nsteps]
|
||||||
|
tau_in_arr = tau_in_arr[:nsteps]; tau_out_arr = tau_out_arr[:nsteps]; power_arr = power_arr[:nsteps]
|
||||||
|
|
||||||
|
# ------------------------------ 写 CSV（写到 MJCF 所在目录） ------------------------------
|
||||||
|
cols = [t_arr, q_in, qd_in, qacc_in, q_ref_arr, qd_ref_arr,
|
||||||
|
q_out, qd_out, qacc_out, tau_in_arr, tau_out_arr, power_arr]
|
||||||
|
header = ("time,input_q,input_qd,input_qacc,q_ref,qd_ref,"
|
||||||
|
"output_q,output_qd,output_qacc,tau_in,tau_out,power_out")
|
||||||
|
if has_planet:
|
||||||
|
cols.insert(9, q_p); cols.insert(10, qd_p)
|
||||||
|
header = header.replace("tau_in", "planet_q,planet_qd,tau_in")
|
||||||
|
data = np.column_stack(cols)
|
||||||
|
csv_path = os.path.join(cfg["out_dir"], "timeseries.csv")
|
||||||
|
np.savetxt(csv_path, data, delimiter=",", header=header, comments="", fmt="%.8f")
|
||||||
|
|
||||||
|
# ------------------------------ 汇总报告 ------------------------------
|
||||||
|
half = nsteps // 2
|
||||||
|
tau_in_ss = np.abs(tau_in_arr[half:]).mean()
|
||||||
|
tau_out_ss = np.abs(tau_out_arr[half:]).mean()
|
||||||
|
N = cfg["gear_ratio"]
|
||||||
|
efficiency = (tau_out_ss / (N * tau_in_ss)) * 100.0 if tau_in_ss > 1e-9 else float("nan")
|
||||||
|
|
||||||
|
# ------------------------------ 汇总报告（打印到终端 + 写 report.txt） ------------------------------
|
||||||
|
rm = np.sqrt(np.mean((q_in - q_ref_arr) ** 2))
|
||||||
|
vm = np.sqrt(np.mean((qd_in - qd_ref_arr) ** 2))
|
||||||
|
# 传动比实测：用带截距的最小二乘斜率估计 q_out = k·q_in + b 的 k。
|
||||||
|
# 不能逐点 q_out/q_in 再取均值——q_in 过零处软约束相位滞后会让比值爆表甚至变号，
|
||||||
|
# 把均值带偏（如 0.112368 vs 真实 0.111111）。带截距斜率对相位滞后与负载静偏置
|
||||||
|
# （q_out 恒滞后一个常数角）都不敏感，能精确还原 1/N。
|
||||||
|
ratio_measured = float(np.polyfit(q_in[half:], q_out[half:], 1)[0])
|
||||||
|
mgn_pos = margin(pos_lim, abs(q_in).max())
|
||||||
|
mgn_trq = margin(trq_lim, abs(tau_in_arr).max())
|
||||||
|
|
||||||
|
rep = []
|
||||||
|
rep.append("=" * 64)
|
||||||
|
rep.append("关节模组仿真报告")
|
||||||
|
rep.append("=" * 64)
|
||||||
|
rep.append(f"模型 : {os.path.basename(cfg['xml'])}")
|
||||||
|
rep.append(f"输入端 : {cfg['input_joint']} 输出端: {cfg['output_joint']}")
|
||||||
|
rep.append(f"标称减速比 1 : {N:.4f}（输出 = 输入/{N:.4f}）")
|
||||||
|
rep.append(f"仿真模式 : {cfg['mode']}")
|
||||||
|
rep.append(f"负载力矩 : {cfg['load_torque']:.4f} N·m 阻尼: {cfg['damping']}")
|
||||||
|
rep.append("")
|
||||||
|
rep.append("[1] 运动学（跟踪精度）")
|
||||||
|
rep.append(f" 输入位置峰值 : {abs(q_in).max():.4f} rad (参考 {cfg['amplitude']:.4f})")
|
||||||
|
rep.append(f" 位置跟踪误差 (RMS) : {rm:.4f} rad")
|
||||||
|
rep.append(f" 速度跟踪误差 (RMS) : {vm:.4f} rad/s")
|
||||||
|
rep.append(f" 输出位置峰值 : {abs(q_out).max():.4f} rad (应为 {cfg['amplitude']/N:.4f})")
|
||||||
|
rep.append(f" 传动比实测 : {ratio_measured:.6f} (期望 {1/N:.6f})")
|
||||||
|
rep.append("")
|
||||||
|
rep.append("[2] 动力学（力矩与功率）")
|
||||||
|
rep.append(f" 输入力矩 (稳态均值) : {tau_in_ss:.4f} N·m")
|
||||||
|
rep.append(f" 输出力矩 (稳态均值) : {tau_out_ss:.4f} N·m")
|
||||||
|
rep.append(f" 理想输出 = 输入×{N:.4f} : {N*tau_in_ss:.4f} N·m")
|
||||||
|
rep.append(f" 力矩损失 : {N*tau_in_ss - tau_out_ss:.4f} N·m")
|
||||||
|
rep.append(f" 效率 η = 输出/(输入×{N:.4f}) : {efficiency:.2f} %")
|
||||||
|
rep.append(f" 输入力矩峰值 (瞬态) : {abs(tau_in_arr).max():.4f} N·m")
|
||||||
|
rep.append(f" 输出功率峰值 : {abs(power_arr).max():.4f} W")
|
||||||
|
rep.append("")
|
||||||
|
rep.append("[3] 安全性（安全余量）")
|
||||||
|
rep.append(f" 位置余量 (限位 ±{pos_lim:.4f} rad) : {mgn_pos:.2f} %")
|
||||||
|
rep.append(f" 力矩余量 (限位 ±{trq_lim:.4f} N·m) : {mgn_trq:.2f} %")
|
||||||
|
rep.append(f" 过载判定 : {'⚠ 余量 < 20%，存在过载风险' if mgn_trq < 20 else '✓ 余量充足'}")
|
||||||
|
rep.append("")
|
||||||
|
rep.append("=" * 64)
|
||||||
|
rep.append(f"已写出 {csv_path}")
|
||||||
|
rep.append("=" * 64)
|
||||||
|
|
||||||
|
report_text = "\n".join(rep) + "\n"
|
||||||
|
print("\n" + report_text)
|
||||||
|
report_path = os.path.join(cfg["out_dir"], "report.txt")
|
||||||
|
with open(report_path, "w", encoding="utf-8") as f:
|
||||||
|
f.write(report_text)
|
||||||
|
print(f"已写出报告 {report_path}")
|
||||||
|
|
||||||
|
# overload 模式：额外产出一份过载报告
|
||||||
|
if cfg["mode"] == "overload":
|
||||||
|
tau_peak = float(np.abs(tau_in_arr).max())
|
||||||
|
if mgn_trq <= 0:
|
||||||
|
verdict = "⚠ 已过载（输入力矩达到额定限位）"
|
||||||
|
elif mgn_trq < 20.0:
|
||||||
|
verdict = "⚠ 接近过载（力矩余量 < 20%）"
|
||||||
|
else:
|
||||||
|
verdict = "✓ 未过载"
|
||||||
|
orep = [
|
||||||
|
"=" * 48,
|
||||||
|
"过载仿真报告",
|
||||||
|
"=" * 48,
|
||||||
|
f"模型 : {os.path.basename(cfg['xml'])}",
|
||||||
|
f"仿真模式 : {cfg['mode']}",
|
||||||
|
f"额定力矩限位 : ±{trq_lim:.4f} N·m",
|
||||||
|
f"施加负载力矩 : {cfg['load_torque']:.4f} N·m",
|
||||||
|
f"输入力矩峰值 : {tau_peak:.4f} N·m",
|
||||||
|
f"力矩余量 : {mgn_trq:.2f} %",
|
||||||
|
f"位置跟踪误差 : {rm:.4f} rad (RMS)",
|
||||||
|
f"过载判定 : {verdict}",
|
||||||
|
"=" * 48,
|
||||||
|
]
|
||||||
|
orep_text = "\n".join(orep) + "\n"
|
||||||
|
orep_path = os.path.join(cfg["out_dir"], "overload_report.txt")
|
||||||
|
with open(orep_path, "w", encoding="utf-8") as f:
|
||||||
|
f.write(orep_text)
|
||||||
|
print(f"\n已写出过载报告 {orep_path}")
|
||||||
|
print(orep_text)
|
||||||
|
|
||||||
|
if args.plot:
|
||||||
|
try:
|
||||||
|
import matplotlib
|
||||||
|
matplotlib.use("Agg")
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
_setup_cjk_font(plt)
|
||||||
|
fig, ax = plt.subplots(3, 1, figsize=(9, 10), sharex=True)
|
||||||
|
ax[0].plot(t_arr, q_in, label="input")
|
||||||
|
ax[0].plot(t_arr, q_out, label="output")
|
||||||
|
if has_planet:
|
||||||
|
ax[0].plot(t_arr, q_p, label="planet")
|
||||||
|
ax[0].set_ylabel("位置 [rad]"); ax[0].legend()
|
||||||
|
ax[1].plot(t_arr, tau_in_arr, label="tau_in")
|
||||||
|
ax[1].plot(t_arr, tau_out_arr, label="tau_out")
|
||||||
|
ax[1].set_ylabel("力矩 [N·m]"); ax[1].legend()
|
||||||
|
ax[2].plot(t_arr, power_arr, label="power_out")
|
||||||
|
ax[2].set_ylabel("功率 [W]"); ax[2].set_xlabel("时间 [s]"); ax[2].legend()
|
||||||
|
fig.tight_layout()
|
||||||
|
png_path = os.path.join(cfg["out_dir"], "report_curves.png")
|
||||||
|
fig.savefig(png_path, dpi=120)
|
||||||
|
print(f"已写出 {png_path}")
|
||||||
|
except ImportError:
|
||||||
|
print("未安装 matplotlib，跳过绘图")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,600 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
关节模组 URDF → MJCF 转换小工具（专用版）
|
||||||
|
=====================================================================
|
||||||
|
|
||||||
|
把一个「关节模组」的 URDF 转成 MuJoCo 可用的 MJCF（.xml），**不改变任何
|
||||||
|
运动学 / 动力学 / 传动关系**，只补上原 URDF 里缺失、但仿真必需的部分：
|
||||||
|
|
||||||
|
1. 质量 / 重心 / 惯性张量 —— 从每个 link 的 STL 网格做体积分（trimesh），
|
||||||
|
按材料密度（脚本顶部的常数表）算出，多网格用平行轴定理合成。
|
||||||
|
2. <actuator> —— 输入电机 + 输出负载（原 URDF 没有）。
|
||||||
|
3. 渲染网格覆盖 —— 个别 STL 面数超过 MuJoCo 上限(20万)，渲染时换降采样版。
|
||||||
|
|
||||||
|
已严格保真的部分（直接照搬 URDF，绝不动）：
|
||||||
|
- 关节层级：<joint><parent>/<child> 决定 body 的父子关系；
|
||||||
|
<joint><origin xyz> → 子 <body pos>（不是 <joint pos>！）
|
||||||
|
<joint><origin rpy> → 子 <body euler>（rpy 反转，z-y-x）
|
||||||
|
- 转轴：<axis> → <joint axis>（子 link 系内，直接映射）
|
||||||
|
- 限位：<limit lower/upper> → <joint range>
|
||||||
|
- 减速比：<mimic> → <equality> polycoef（joint1 = offset + multiplier·joint2）
|
||||||
|
- 视觉：<visual><origin xyz> → <geom pos>（仅视觉，contype/conaffinity=0）
|
||||||
|
|
||||||
|
关键 MuJoCo 语义（易错点，务必保持）：
|
||||||
|
- MuJoCo 的 <joint pos> 是转轴相对 **body 帧** 的偏移，默认 0 即轴过 body 原点。
|
||||||
|
URDF 的 <joint><origin> 是「子 link 系相对父系」的位姿，对应子 <body pos>。
|
||||||
|
两者不是一回事 —— 填错会把行星轮挂到体外轴上公转。
|
||||||
|
- <mimic multiplier=M> 等价 <equality> joint1=本关节 joint2=被 mimic 关节
|
||||||
|
polycoef="offset M 0 0 0"。
|
||||||
|
|
||||||
|
用法（在 scripts/ 目录下运行）：
|
||||||
|
python3 urdf_to_mjcf.py # 默认转换本案例
|
||||||
|
python3 urdf_to_mjcf.py --urdf ../urdf/foo.urdf --out foo.xml
|
||||||
|
python3 urdf_to_mjcf.py --input <关节名> --output <关节名> # 手动指定输入/输出端
|
||||||
|
|
||||||
|
依赖：numpy, trimesh（pip install trimesh）
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import os
|
||||||
|
import shutil
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
try:
|
||||||
|
import trimesh
|
||||||
|
except ImportError as e: # pragma: no cover
|
||||||
|
raise SystemExit("缺少依赖 trimesh，请先 `pip install trimesh`") from e
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 材料密度常数 (kg/m³) —— 在这里给定 / 修改
|
||||||
|
# =============================================================================
|
||||||
|
# 默认按钢处理；个别混合/轻质零件单独覆盖。这些就是「关节模组」需要标定的常数。
|
||||||
|
DEFAULT_DENSITY = 7850.0 # 钢（太阳轮、行星架、行星轮）
|
||||||
|
DENSITIES = {
|
||||||
|
"fixed_structure": 2700.0, # 固定结构 —— 铝合金
|
||||||
|
"motor_stator": 7200.0, # 定子 —— 硅钢+铜绕组，工程估算等效密度
|
||||||
|
"motor_rotor": 7600.0, # 转子 —— 硅钢+磁钢，工程估算等效密度
|
||||||
|
# 其余（sun_drive / carrier_link / planet_*_link）走默认钢 7850
|
||||||
|
}
|
||||||
|
|
||||||
|
# STL 单位换算：URDF 里 mesh 的 scale 通常是 0.001（毫米 → 米）。
|
||||||
|
# 质量/惯量按「缩放后（米）」的体积分计算，保证 SI 单位（kg / kg·m²）；
|
||||||
|
# 渲染 scale 逐 mesh 从 URDF <mesh scale> 读取（见 build_mjcf 的 asset 生成）。
|
||||||
|
|
||||||
|
# 渲染网格覆盖：某 STL 原始面数超过 MuJoCo 顶点上限(20万)，渲染时用降采样版；
|
||||||
|
# 但质量/惯量仍按 **原始网格** 计算，不改变动力学。{mesh 名: 渲染用文件名}
|
||||||
|
# 不在此表里的超面数网格，由 render_file_for() 在转换时自动降采样。
|
||||||
|
MESH_ASSET_OVERRIDE = {
|
||||||
|
"motor_stator": "motor_stator_decimated.stl",
|
||||||
|
}
|
||||||
|
|
||||||
|
# MuJoCo 单个 STL 网格的面数上限（超出会报错）；自动降采样的目标面数。
|
||||||
|
MAX_MJC_FACES = 200000
|
||||||
|
DECIMATE_TARGET_FACES = 100000
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 仿真参数（关节模组默认；不影响运动学/传动，仅数值/安全相关）
|
||||||
|
# =============================================================================
|
||||||
|
TIMESTEP = 0.001 # [s]
|
||||||
|
GRAVITY = (0.0, 0.0, -9.81)
|
||||||
|
JOINT_DAMPING = 0.01 # 数值阻尼（避免刚性齿轮约束震荡）
|
||||||
|
JOINT_ARMATURE = 0.0005 # 电枢惯量
|
||||||
|
TORQUE_LIMIT = 10.0 # 输入力矩限位 [N·m]（占位，应填真实电机额定值）
|
||||||
|
LOAD_CTRLRANGE = 1.0e6 # 负载电机 ctrlrange：测试边界条件，不限流（可施加任意负载做过载仿真）
|
||||||
|
SOLREF = (0.002, 1.0) # 齿轮约束软约束 solref
|
||||||
|
SOLIMP = (0.9, 0.95, 0.0001) # 齿轮约束 solimp
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 小工具函数
|
||||||
|
# =============================================================================
|
||||||
|
def _vec(el, attr, default):
|
||||||
|
if el is None or attr not in el.attrib:
|
||||||
|
return np.array(default, dtype=float)
|
||||||
|
return np.array([float(x) for x in el.get(attr).split()], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
|
def parse_origin(origin_el):
|
||||||
|
"""返回 (xyz, rpy)，缺省为零。"""
|
||||||
|
if origin_el is None:
|
||||||
|
return np.zeros(3), np.zeros(3)
|
||||||
|
xyz = _vec(origin_el, "xyz", [0, 0, 0])
|
||||||
|
rpy = _vec(origin_el, "rpy", [0, 0, 0])
|
||||||
|
return xyz, rpy
|
||||||
|
|
||||||
|
|
||||||
|
def parse_axis(axis_el):
|
||||||
|
"""URDF 转轴（子 link 系内），缺省 (1,0,0)。"""
|
||||||
|
return _vec(axis_el, "xyz", [1, 0, 0])
|
||||||
|
|
||||||
|
|
||||||
|
def parse_limit(limit_el):
|
||||||
|
"""返回 (lower, upper)，无 <limit> 返回 None。"""
|
||||||
|
if limit_el is None:
|
||||||
|
return None
|
||||||
|
return (float(limit_el.get("lower")), float(limit_el.get("upper")))
|
||||||
|
|
||||||
|
|
||||||
|
def parse_mimic(mimic_el):
|
||||||
|
"""返回 dict(joint, multiplier, offset)，无 <mimic> 返回 None。"""
|
||||||
|
if mimic_el is None:
|
||||||
|
return None
|
||||||
|
return {
|
||||||
|
"joint": mimic_el.get("joint"),
|
||||||
|
"multiplier": float(mimic_el.get("multiplier", "1")),
|
||||||
|
"offset": float(mimic_el.get("offset", "0")),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def parse_mesh_scale(mesh_el):
|
||||||
|
"""返回 mesh 的 scale（默认 1 1 1）。"""
|
||||||
|
if mesh_el is None:
|
||||||
|
return np.array([1.0, 1.0, 1.0])
|
||||||
|
return _vec(mesh_el, "scale", [1, 1, 1])
|
||||||
|
|
||||||
|
|
||||||
|
def parse_color(material_el, global_materials):
|
||||||
|
"""提取材质 RGBA（前 3 通道），找不到用默认灰。"""
|
||||||
|
rgba = None
|
||||||
|
if material_el is not None:
|
||||||
|
color_el = material_el.find("color")
|
||||||
|
if color_el is not None:
|
||||||
|
rgba = color_el.get("rgba")
|
||||||
|
elif material_el.get("name") in global_materials:
|
||||||
|
rgba = global_materials[material_el.get("name")]
|
||||||
|
if rgba is None:
|
||||||
|
return (0.7, 0.7, 0.7)
|
||||||
|
vals = [float(x) for x in rgba.split()]
|
||||||
|
return tuple(vals[:3])
|
||||||
|
|
||||||
|
|
||||||
|
def rpy_to_euler(rpy):
|
||||||
|
"""URDF rpy(固定轴 x-y-z, R=Rz·Ry·Rx) → MuJoCo euler(体轴 x-y-z, R=Rx·Ry·Rz)。
|
||||||
|
两者不是简单重排（Rx·Ry·Rz ≠ Rz·Ry·Rx），必须由旋转矩阵解出 MuJoCo 的 x-y-z 欧拉角。"""
|
||||||
|
R = rpy_to_mat(rpy)
|
||||||
|
ey = np.arcsin(np.clip(R[0, 2], -1.0, 1.0))
|
||||||
|
ez = np.arctan2(-R[0, 1], R[0, 0])
|
||||||
|
ex = np.arctan2(-R[1, 2], R[2, 2])
|
||||||
|
return np.array([ex, ey, ez])
|
||||||
|
|
||||||
|
|
||||||
|
def rpy_to_mat(rpy):
|
||||||
|
"""rpy → 旋转矩阵 R = Rz(rz)·Ry(ry)·Rx(rx)。"""
|
||||||
|
cx, sx = np.cos(rpy[0]), np.sin(rpy[0])
|
||||||
|
cy, sy = np.cos(rpy[1]), np.sin(rpy[1])
|
||||||
|
cz, sz = np.cos(rpy[2]), np.sin(rpy[2])
|
||||||
|
Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
|
||||||
|
Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
|
||||||
|
Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
|
||||||
|
return Rz @ Ry @ Rx
|
||||||
|
|
||||||
|
|
||||||
|
def fmt(x):
|
||||||
|
"""浮点 → 字符串（6 位有效数字，紧凑科学计数）。"""
|
||||||
|
return f"{float(x):.6g}"
|
||||||
|
|
||||||
|
|
||||||
|
def fmt_vec(v):
|
||||||
|
return " ".join(fmt(x) for x in v)
|
||||||
|
|
||||||
|
|
||||||
|
def fmt_urdf(x):
|
||||||
|
"""URDF 原值：最短精确表示（整数值去 .0），保证与源 URDF 逐位一致。
|
||||||
|
用于「照搬不改」的字段：限位、减速比、原点、轴、mesh scale。"""
|
||||||
|
v = float(x)
|
||||||
|
if v == int(v) and abs(v) < 1e15:
|
||||||
|
return str(int(v))
|
||||||
|
return repr(v)
|
||||||
|
|
||||||
|
|
||||||
|
def fmt_vec_urdf(v):
|
||||||
|
return " ".join(fmt_urdf(x) for x in v)
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 质量 / 重心 / 惯性 计算（STL 体积分）
|
||||||
|
# =============================================================================
|
||||||
|
def mesh_mass_props(path, density, scale):
|
||||||
|
"""加载 STL，返回 (mass, com, inertia)：
|
||||||
|
mass [kg]、com [m，网格自身坐标系]、inertia [kg·m²，关于自身质心，网格系]。"""
|
||||||
|
mesh = trimesh.load(path, force="mesh")
|
||||||
|
if isinstance(mesh, trimesh.Scene):
|
||||||
|
# 多物体场景：合并几何
|
||||||
|
mesh = trimesh.util.concatenate(list(mesh.geometry.values()))
|
||||||
|
# 均匀缩放（毫米→米）。非均匀 scale 这里按体积近似，误差可忽略（本案例均 0.001）
|
||||||
|
s = float(scale[0])
|
||||||
|
mesh.apply_scale(s)
|
||||||
|
mass = density * abs(mesh.volume)
|
||||||
|
com = np.asarray(mesh.center_mass, dtype=float)
|
||||||
|
inertia = np.asarray(mesh.moment_inertia, dtype=float) * density
|
||||||
|
return mass, com, inertia
|
||||||
|
|
||||||
|
|
||||||
|
def link_mass_props(visuals, mesh_dir):
|
||||||
|
"""把一个 link 的多个视觉网格，按各自 origin 合成质量/重心/惯性（link 系）。"""
|
||||||
|
items = []
|
||||||
|
for vis in visuals:
|
||||||
|
if vis["mesh_filename"] is None:
|
||||||
|
continue # 非 mesh 几何（本案例没有）跳过
|
||||||
|
base = vis["mesh_base"]
|
||||||
|
src = os.path.join(mesh_dir, base + ".stl") # 原始网格算质量
|
||||||
|
origin, rpy = vis["origin"]
|
||||||
|
density = DENSITIES.get(base, DEFAULT_DENSITY)
|
||||||
|
mass, com_m, inertia_m = mesh_mass_props(src, density, vis["scale"])
|
||||||
|
R = rpy_to_mat(rpy)
|
||||||
|
com_link = origin + R @ com_m # 网格质心 → link 系
|
||||||
|
inertia_link = R @ inertia_m @ R.T # 惯性张量旋到 link 系
|
||||||
|
items.append((mass, com_link, inertia_link))
|
||||||
|
|
||||||
|
if not items:
|
||||||
|
return 0.0, np.zeros(3), np.zeros((3, 3))
|
||||||
|
|
||||||
|
total_mass = sum(m for m, _, _ in items)
|
||||||
|
com = sum(m * c for m, c, _ in items) / total_mass
|
||||||
|
inertia = np.zeros((3, 3))
|
||||||
|
for mass, c, I in items:
|
||||||
|
d = c - com
|
||||||
|
inertia += I + mass * (np.dot(d, d) * np.eye(3) - np.outer(d, d))
|
||||||
|
return total_mass, com, inertia
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 网格降采样（仅用于渲染；质量/惯量始终按原始网格，见 link_mass_props）
|
||||||
|
# =============================================================================
|
||||||
|
def stl_face_count(path):
|
||||||
|
"""读二进制 STL 的三角面数（offset 80 的 uint32）。非二进制返回 0。"""
|
||||||
|
with open(path, "rb") as f:
|
||||||
|
if f.read(5).lower().startswith(b"solid"):
|
||||||
|
return 0
|
||||||
|
f.seek(80)
|
||||||
|
return int(np.frombuffer(f.read(4), dtype="<u4")[0])
|
||||||
|
|
||||||
|
|
||||||
|
def decimate_stl(src, dst, target_faces):
|
||||||
|
"""把 STL 降采样到约 target_faces 个三角面，写出二进制 STL。"""
|
||||||
|
try:
|
||||||
|
import fast_simplification
|
||||||
|
except ImportError:
|
||||||
|
raise SystemExit(
|
||||||
|
f"网格 {os.path.basename(src)} 面数超 MuJoCo 上限，且缺少 fast_simplification："
|
||||||
|
"请 `pip install fast_simplification`")
|
||||||
|
mesh = trimesh.load(src, force="mesh")
|
||||||
|
if isinstance(mesh, trimesh.Scene):
|
||||||
|
mesh = trimesh.util.concatenate(list(mesh.geometry.values()))
|
||||||
|
faces = mesh.faces.astype("int64")
|
||||||
|
if len(faces) <= MAX_MJC_FACES:
|
||||||
|
shutil.copy2(src, dst)
|
||||||
|
return
|
||||||
|
reduction = max(0.0, 1.0 - target_faces / len(faces))
|
||||||
|
v, f = fast_simplification.simplify(
|
||||||
|
mesh.vertices.astype("float64"), faces, target_reduction=reduction, agg=7.0)
|
||||||
|
trimesh.Trimesh(vertices=v, faces=f, process=False).export(dst)
|
||||||
|
|
||||||
|
|
||||||
|
def render_file_for(base, src_mesh_dir, out_mesh_dir):
|
||||||
|
"""返回 base 网格用于渲染的文件名：显式覆盖 > 自动降采样 > 原文件。"""
|
||||||
|
if base in MESH_ASSET_OVERRIDE:
|
||||||
|
# 显式覆盖仅当覆盖文件真实存在于源网格目录时才生效；否则退回通用降采样路径，
|
||||||
|
# 让新模组里同样超面数的同名网格自动生成 *_decimated.stl（否则会引用一个不存在的文件）。
|
||||||
|
override = MESH_ASSET_OVERRIDE[base]
|
||||||
|
if os.path.isfile(os.path.join(src_mesh_dir, override)):
|
||||||
|
return override
|
||||||
|
src = os.path.join(src_mesh_dir, base + ".stl")
|
||||||
|
if os.path.isfile(src) and stl_face_count(src) > MAX_MJC_FACES:
|
||||||
|
dec = base + "_decimated.stl"
|
||||||
|
dst = os.path.join(out_mesh_dir, dec)
|
||||||
|
if not os.path.isfile(dst):
|
||||||
|
os.makedirs(out_mesh_dir, exist_ok=True)
|
||||||
|
decimate_stl(src, dst, DECIMATE_TARGET_FACES)
|
||||||
|
return dec
|
||||||
|
return base + ".stl"
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# URDF 解析
|
||||||
|
# =============================================================================
|
||||||
|
def parse_urdf(path):
|
||||||
|
tree = ET.parse(path)
|
||||||
|
robot = tree.getroot()
|
||||||
|
|
||||||
|
# 顶层 <material>（本案例用的是视觉内联 <material>，这里留作兜底）
|
||||||
|
global_materials = {}
|
||||||
|
for mat in robot.findall("material"):
|
||||||
|
color_el = mat.find("color")
|
||||||
|
if color_el is not None:
|
||||||
|
global_materials[mat.get("name")] = color_el.get("rgba")
|
||||||
|
|
||||||
|
links = {}
|
||||||
|
for link in robot.findall("link"):
|
||||||
|
name = link.get("name")
|
||||||
|
visuals = []
|
||||||
|
for vis in link.findall("visual"):
|
||||||
|
origin = parse_origin(vis.find("origin"))
|
||||||
|
geom_el = vis.find("geometry")
|
||||||
|
mesh_el = geom_el.find("mesh") if geom_el is not None else None
|
||||||
|
mesh_filename = mesh_el.get("filename") if mesh_el is not None else None
|
||||||
|
mesh_base = (os.path.splitext(os.path.basename(mesh_filename))[0]
|
||||||
|
if mesh_filename else None)
|
||||||
|
visuals.append({
|
||||||
|
"origin": origin,
|
||||||
|
"mesh_filename": mesh_filename,
|
||||||
|
"mesh_base": mesh_base,
|
||||||
|
"scale": parse_mesh_scale(mesh_el),
|
||||||
|
"color": parse_color(vis.find("material"), global_materials),
|
||||||
|
})
|
||||||
|
links[name] = {"name": name, "visuals": visuals}
|
||||||
|
|
||||||
|
joints = []
|
||||||
|
for j in robot.findall("joint"):
|
||||||
|
joints.append({
|
||||||
|
"name": j.get("name"),
|
||||||
|
"type": j.get("type", "revolute"),
|
||||||
|
"parent": j.find("parent").get("link"),
|
||||||
|
"child": j.find("child").get("link"),
|
||||||
|
"origin": parse_origin(j.find("origin")),
|
||||||
|
"axis": parse_axis(j.find("axis")),
|
||||||
|
"limit": parse_limit(j.find("limit")),
|
||||||
|
"mimic": parse_mimic(j.find("mimic")),
|
||||||
|
})
|
||||||
|
|
||||||
|
return robot.get("name"), links, joints
|
||||||
|
|
||||||
|
|
||||||
|
def build_tree(links, joints):
|
||||||
|
"""由关节 parent/child 建立 body 树，返回 (roots, children_by_parent)。"""
|
||||||
|
parent_of = {} # child link -> joint
|
||||||
|
children = {} # parent link -> [joints]
|
||||||
|
for link in links:
|
||||||
|
children[link] = []
|
||||||
|
for j in joints:
|
||||||
|
parent_of[j["child"]] = j
|
||||||
|
children.setdefault(j["parent"], []).append(j)
|
||||||
|
|
||||||
|
roots = [name for name in links if name not in parent_of]
|
||||||
|
return roots, children
|
||||||
|
|
||||||
|
|
||||||
|
def detect_input_output(joints):
|
||||||
|
"""关节模组自动识别输入/输出端：
|
||||||
|
输入 = 唯一没有 <mimic> 的关节（独立驱动源）；
|
||||||
|
输出 = 以正 multiplier 跟随输入的关节（减速输出）。
|
||||||
|
也可用 --input/--output 手动指定。"""
|
||||||
|
no_mimic = [j for j in joints if j["mimic"] is None]
|
||||||
|
input_j = no_mimic[0] if len(no_mimic) == 1 else None
|
||||||
|
output_j = None
|
||||||
|
if input_j is not None:
|
||||||
|
for j in joints:
|
||||||
|
m = j["mimic"]
|
||||||
|
if m is not None and m["joint"] == input_j["name"] and m["multiplier"] > 0:
|
||||||
|
output_j = j
|
||||||
|
break
|
||||||
|
return input_j, output_j
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# MJCF 生成
|
||||||
|
# =============================================================================
|
||||||
|
# URDF 关节类型 → MuJoCo 关节类型。URDF 的 fixed 关节（0 自由度，刚体固连）在
|
||||||
|
# MJCF 里用「子 body 不写 <joint>」表达（子 body 的 pos/euler 已含其位姿），故映射为 None。
|
||||||
|
_URDF_TO_MJC_JOINT = {
|
||||||
|
"revolute": "hinge",
|
||||||
|
"continuous": "hinge",
|
||||||
|
"prismatic": "slide",
|
||||||
|
"floating": "free",
|
||||||
|
"fixed": None,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def joint_xml(j):
|
||||||
|
jtype = _URDF_TO_MJC_JOINT.get(j["type"], j["type"])
|
||||||
|
if jtype is None: # fixed：固连，不生成 <joint>
|
||||||
|
return None
|
||||||
|
attrs = [f'name="{j["name"]}"', f'type="{jtype}"', f'axis="{fmt_vec_urdf(j["axis"])}"']
|
||||||
|
if j["limit"] is not None:
|
||||||
|
attrs.append(f'range="{fmt_urdf(j["limit"][0])} {fmt_urdf(j["limit"][1])}"')
|
||||||
|
return " ".join(attrs)
|
||||||
|
|
||||||
|
|
||||||
|
def inertial_xml(mass, com, inertia):
|
||||||
|
if mass <= 0:
|
||||||
|
return None
|
||||||
|
# fullinertia 顺序：ixx iyy izz ixy ixz iyz
|
||||||
|
fi = (inertia[0, 0], inertia[1, 1], inertia[2, 2],
|
||||||
|
inertia[0, 1], inertia[0, 2], inertia[1, 2])
|
||||||
|
return (f'<inertial pos="{fmt_vec(com)}" mass="{fmt(mass)}" '
|
||||||
|
f'fullinertia="{fmt_vec(fi)}"/>')
|
||||||
|
|
||||||
|
|
||||||
|
def geom_xml(vis, asset_scales):
|
||||||
|
base = vis["mesh_base"]
|
||||||
|
if base is None:
|
||||||
|
return None
|
||||||
|
# 记录该 mesh 的渲染 scale（来自 URDF <mesh scale>）。同一 mesh 若被多处引用且 scale
|
||||||
|
# 不同，以最后一次为准——本语料库所有 mesh scale 统一为 0.001，不涉及此边界情况。
|
||||||
|
asset_scales[base] = vis["scale"]
|
||||||
|
parts = [f'type="mesh"', f'mesh="{base}"',
|
||||||
|
f'rgba="{fmt(vis["color"][0])} {fmt(vis["color"][1])} {fmt(vis["color"][2])} 1"']
|
||||||
|
origin, rpy = vis["origin"]
|
||||||
|
if np.any(np.abs(origin) > 1e-12):
|
||||||
|
parts.append(f'pos="{fmt_vec_urdf(origin)}"')
|
||||||
|
if np.any(np.abs(rpy) > 1e-12):
|
||||||
|
parts.append(f'euler="{fmt_vec_urdf(rpy_to_euler(rpy))}"')
|
||||||
|
return "<geom " + " ".join(parts) + "/>"
|
||||||
|
|
||||||
|
|
||||||
|
def emit_body(lines, link, links, joints, children, joint_map, mesh_dir, asset_scales, depth):
|
||||||
|
ind = " " * depth
|
||||||
|
link_name = link["name"]
|
||||||
|
|
||||||
|
# 该 link 作为子 body 的关节（非根）
|
||||||
|
j = joint_map.get(link_name)
|
||||||
|
pos_attr = ""
|
||||||
|
if j is not None:
|
||||||
|
xyz, rpy = j["origin"]
|
||||||
|
if np.any(np.abs(xyz) > 1e-12):
|
||||||
|
pos_attr = f' pos="{fmt_vec_urdf(xyz)}"'
|
||||||
|
euler = rpy_to_euler(rpy)
|
||||||
|
if np.any(np.abs(euler) > 1e-12):
|
||||||
|
pos_attr += f' euler="{fmt_vec_urdf(euler)}"'
|
||||||
|
|
||||||
|
lines.append(f'{ind}<body name="{link_name}"{pos_attr}>')
|
||||||
|
|
||||||
|
# 惯性（link 系内）
|
||||||
|
mass, com, inertia = link_mass_props(link["visuals"], mesh_dir)
|
||||||
|
iner = inertial_xml(mass, com, inertia)
|
||||||
|
if iner is not None:
|
||||||
|
lines.append(ind + " " + iner)
|
||||||
|
|
||||||
|
# 关节（fixed 关节固连不生成 <joint>，仅靠 body 的 pos/euler 定位）
|
||||||
|
if j is not None:
|
||||||
|
jxml = joint_xml(j)
|
||||||
|
if jxml is not None:
|
||||||
|
lines.append(ind + " <joint " + jxml + "/>")
|
||||||
|
|
||||||
|
# 视觉几何
|
||||||
|
for vis in link["visuals"]:
|
||||||
|
g = geom_xml(vis, asset_scales)
|
||||||
|
if g is not None:
|
||||||
|
lines.append(ind + " " + g)
|
||||||
|
|
||||||
|
# 子 body
|
||||||
|
for cj in children.get(link_name, []):
|
||||||
|
child_link = links[cj["child"]]
|
||||||
|
emit_body(lines, child_link, links, joints, children, joint_map,
|
||||||
|
mesh_dir, asset_scales, depth + 1)
|
||||||
|
|
||||||
|
lines.append(f"{ind}</body>")
|
||||||
|
|
||||||
|
|
||||||
|
def build_mjcf(robot_name, links, joints, mesh_dir, out_meshdir, input_j, output_j, damping, torque_limit):
|
||||||
|
roots, children = build_tree(links, joints)
|
||||||
|
joint_map = {j["child"]: j for j in joints}
|
||||||
|
asset_scales = {}
|
||||||
|
|
||||||
|
lines = []
|
||||||
|
lines.append(f'<mujoco model="{robot_name}">')
|
||||||
|
lines.append(' <!-- 由 urdf_to_mjcf.py 自动生成：只补质量/惯量/作动器，'
|
||||||
|
'不改变运动学/动力学/传动关系 -->')
|
||||||
|
lines.append(' <compiler angle="radian" meshdir="meshes"/>')
|
||||||
|
lines.append('')
|
||||||
|
lines.append(f' <option timestep="{fmt(TIMESTEP)}" gravity="{fmt_vec(GRAVITY)}"/>')
|
||||||
|
lines.append('')
|
||||||
|
lines.append(' <default>')
|
||||||
|
lines.append(f' <joint damping="{fmt(damping)}" armature="{fmt(JOINT_ARMATURE)}"/>')
|
||||||
|
lines.append(' <geom contype="0" conaffinity="0"/>')
|
||||||
|
lines.append(' </default>')
|
||||||
|
lines.append('')
|
||||||
|
lines.append(' <asset>')
|
||||||
|
|
||||||
|
# 先收集所有 mesh 引用（base 名 → 渲染 scale），再确定渲染文件名（覆盖 / 自动降采样 / 原文件）
|
||||||
|
body_lines = []
|
||||||
|
for root in roots:
|
||||||
|
emit_body(body_lines, links[root], links, joints, children, joint_map,
|
||||||
|
mesh_dir, asset_scales, 2)
|
||||||
|
|
||||||
|
render_file = {base: render_file_for(base, mesh_dir, out_meshdir)
|
||||||
|
for base in sorted(asset_scales)}
|
||||||
|
for base, file in sorted(render_file.items()):
|
||||||
|
scale = asset_scales[base]
|
||||||
|
lines.append(f' <mesh name="{base}" file="{file}" scale="{fmt_vec(scale)}"/>')
|
||||||
|
lines.append(' </asset>')
|
||||||
|
lines.append('')
|
||||||
|
lines.append(' <worldbody>')
|
||||||
|
lines.extend(body_lines)
|
||||||
|
lines.append(' </worldbody>')
|
||||||
|
lines.append('')
|
||||||
|
|
||||||
|
# mimic → equality
|
||||||
|
lines.append(' <equality>')
|
||||||
|
for j in joints:
|
||||||
|
m = j["mimic"]
|
||||||
|
if m is not None:
|
||||||
|
poly = f'{fmt_urdf(m["offset"])} {fmt_urdf(m["multiplier"])} 0 0 0'
|
||||||
|
lines.append(f' <joint joint1="{j["name"]}" joint2="{m["joint"]}" '
|
||||||
|
f'polycoef="{poly}" '
|
||||||
|
f'solref="{fmt(SOLREF[0])} {fmt(SOLREF[1])}" '
|
||||||
|
f'solimp="{fmt(SOLIMP[0])} {fmt(SOLIMP[1])} {fmt(SOLIMP[2])}"/>')
|
||||||
|
lines.append(' </equality>')
|
||||||
|
lines.append('')
|
||||||
|
|
||||||
|
# actuator
|
||||||
|
lines.append(' <actuator>')
|
||||||
|
if input_j is not None:
|
||||||
|
lines.append(f' <motor name="input_motor" joint="{input_j["name"]}" '
|
||||||
|
f'gear="1" ctrlrange="-{fmt(torque_limit)} {fmt(torque_limit)}"/>')
|
||||||
|
if output_j is not None:
|
||||||
|
lines.append(f' <motor name="load_motor" joint="{output_j["name"]}" '
|
||||||
|
f'gear="1" ctrlrange="-{fmt(LOAD_CTRLRANGE)} {fmt(LOAD_CTRLRANGE)}"/>')
|
||||||
|
lines.append(' </actuator>')
|
||||||
|
lines.append('</mujoco>')
|
||||||
|
|
||||||
|
return "\n".join(lines) + "\n", render_file
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# 主流程
|
||||||
|
# =============================================================================
|
||||||
|
def main():
|
||||||
|
here = os.path.dirname(os.path.abspath(__file__))
|
||||||
|
ap = argparse.ArgumentParser(description="关节模组 URDF → MJCF 转换")
|
||||||
|
ap.add_argument("--urdf", default=os.path.join(here, "..", "urdf",
|
||||||
|
"planetary_joint_split_motor_demo.urdf"))
|
||||||
|
ap.add_argument("--out", default=os.path.join(here,
|
||||||
|
"planetary_joint_split_motor_demo_generated.xml"))
|
||||||
|
ap.add_argument("--meshdir", default=os.path.join(here, "meshes"),
|
||||||
|
help="网格输出目录（把 URDF 引用的 STL 拷进来，默认 urdf/meshes 即 URDF 同目录）")
|
||||||
|
ap.add_argument("--input", default=None, help="手动指定输入关节名")
|
||||||
|
ap.add_argument("--output", default=None, help="手动指定输出关节名")
|
||||||
|
ap.add_argument("--damping", type=float, default=JOINT_DAMPING,
|
||||||
|
help=f"关节粘性阻尼（默认 {JOINT_DAMPING}）")
|
||||||
|
ap.add_argument("--torque-limit", type=float, default=TORQUE_LIMIT,
|
||||||
|
help=f"输入力矩限位 [N·m]（默认 {TORQUE_LIMIT}）")
|
||||||
|
ap.add_argument("--no-copy", action="store_true", help="不拷贝网格文件")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
urdf_path = os.path.abspath(args.urdf)
|
||||||
|
robot_name, links, joints = parse_urdf(urdf_path)
|
||||||
|
mesh_dir = os.path.join(os.path.dirname(urdf_path), "meshes")
|
||||||
|
|
||||||
|
input_j, output_j = detect_input_output(joints)
|
||||||
|
if args.input:
|
||||||
|
input_j = next((j for j in joints if j["name"] == args.input), None) or input_j
|
||||||
|
if args.output:
|
||||||
|
output_j = next((j for j in joints if j["name"] == args.output), None) or output_j
|
||||||
|
|
||||||
|
os.makedirs(args.meshdir, exist_ok=True)
|
||||||
|
xml_text, render_file = build_mjcf(robot_name, links, joints, mesh_dir,
|
||||||
|
args.meshdir, input_j, output_j, args.damping,
|
||||||
|
args.torque_limit)
|
||||||
|
|
||||||
|
with open(args.out, "w", encoding="utf-8") as f:
|
||||||
|
f.write(xml_text)
|
||||||
|
|
||||||
|
# 拷贝网格（降采样网格已由 render_file_for 直接写进 meshdir，这里只拷原文件/覆盖文件）
|
||||||
|
if not args.no_copy:
|
||||||
|
for base, file in render_file.items():
|
||||||
|
src = os.path.join(mesh_dir, file)
|
||||||
|
dst = os.path.join(args.meshdir, file)
|
||||||
|
if os.path.exists(src):
|
||||||
|
shutil.copy2(src, dst)
|
||||||
|
|
||||||
|
# 汇总
|
||||||
|
print(f"已生成 {os.path.abspath(args.out)}")
|
||||||
|
print(f" links : {len(links)} joints: {len(joints)}")
|
||||||
|
print(f" 阻尼 : {args.damping}")
|
||||||
|
if input_j is not None:
|
||||||
|
print(f" 输入端 : {input_j['name']}")
|
||||||
|
if output_j is not None:
|
||||||
|
m = output_j["mimic"]
|
||||||
|
ratio = 1.0 / m["multiplier"] if m and m["multiplier"] != 0 else float("nan")
|
||||||
|
print(f" 输出端 : {output_j['name']} 减速比 ≈ 1:{ratio:.4f}")
|
||||||
|
for link_name, link in sorted(links.items()):
|
||||||
|
mass, com, _ = link_mass_props(link["visuals"], mesh_dir)
|
||||||
|
print(f" [{link_name}] 质量 {mass*1000:.1f} g 重心 {fmt_vec(com)}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,134 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
关节模组一键验证入口（编排 urdf_to_mjcf → generate_schema → simulate_report）
|
||||||
|
====================================================================
|
||||||
|
|
||||||
|
把「URDF → MJCF → schema → 仿真报告」整条流水线串起来，一条命令跑完。
|
||||||
|
这相当于后端处理「用户上传一份 URDF + mesh」时干的事：你拿任意一份关节模组 URDF
|
||||||
|
丢进来，只要显式给出输入/输出关节，就能得到自包含 MJCF、schema JSON 和仿真报告。
|
||||||
|
|
||||||
|
三个步骤各自都能单独跑（见 docs/architecture.md 的「分步运行」），本脚本只是把它们按顺序编排，
|
||||||
|
并把产物统一放到同一个工作目录（默认 = URDF 所在目录）。
|
||||||
|
|
||||||
|
用法（在 scripts/ 目录下运行）：
|
||||||
|
python3 validate_module.py \
|
||||||
|
--urdf /path/to/joint_module.urdf \
|
||||||
|
--input sun_input_joint --output carrier_output_joint
|
||||||
|
|
||||||
|
常用可选参数：
|
||||||
|
--work-dir DIR 产物输出目录（默认 URDF 所在目录）
|
||||||
|
--ratio N 减速比（URDF 输出关节无 <mimic> 时手动指定）
|
||||||
|
--torque-limit T 输入力矩限位 [N·m]（默认 ±10，占位）
|
||||||
|
--position-limit P 输入关节无 <limit> 时的位置限位 ±rad（默认 ±12π）
|
||||||
|
--show 弹可视化窗口（默认无头模式）
|
||||||
|
--plot 追加 report_curves.png 曲线图
|
||||||
|
|
||||||
|
产物（都在 work-dir 下）：
|
||||||
|
<module_id>.xml 自包含 MJCF（meshdir="meshes"，网格已拷到 meshes/）
|
||||||
|
<module_id>.json schema JSON
|
||||||
|
report.txt 仿真报告
|
||||||
|
timeseries.csv 逐时间步观测
|
||||||
|
report_curves.png 曲线图（--plot）
|
||||||
|
"""
|
||||||
|
import argparse
|
||||||
|
import os
|
||||||
|
import re
|
||||||
|
import subprocess
|
||||||
|
import sys
|
||||||
|
|
||||||
|
|
||||||
|
SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
|
||||||
|
|
||||||
|
|
||||||
|
def robot_name_of(urdf):
|
||||||
|
"""从 URDF 根元素 <robot name="..."> 取机器名，作为默认 module_id。"""
|
||||||
|
with open(urdf, encoding="utf-8") as f:
|
||||||
|
head = f.read(2048)
|
||||||
|
m = re.search(r'<robot[^>]*\bname="([^"]+)"', head)
|
||||||
|
return m.group(1) if m else os.path.splitext(os.path.basename(urdf))[0]
|
||||||
|
|
||||||
|
|
||||||
|
def run(cmd, what):
|
||||||
|
print(f"\n== {what} ==")
|
||||||
|
print(" " + " ".join(cmd))
|
||||||
|
r = subprocess.run(cmd, cwd=SCRIPTS_DIR)
|
||||||
|
if r.returncode != 0:
|
||||||
|
sys.exit(f"[失败] {what}（退出码 {r.returncode}）")
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser(description="关节模组一键验证（URDF→MJCF→schema→报告）")
|
||||||
|
ap.add_argument("--urdf", required=True, help="输入 URDF 路径")
|
||||||
|
ap.add_argument("--input", required=True, help="输入关节名（电机端，显式指定）")
|
||||||
|
ap.add_argument("--output", required=True, help="输出关节名（模组末端，显式指定）")
|
||||||
|
ap.add_argument("--work-dir", default=None, help="产物输出目录（默认 URDF 所在目录）")
|
||||||
|
ap.add_argument("--module-id", default=None, help="模组唯一标识（默认 URDF 的 robot 名）")
|
||||||
|
ap.add_argument("--ratio", type=float, default=None, help="手动指定减速比")
|
||||||
|
ap.add_argument("--torque-limit", type=float, default=10.0, help="输入力矩限位 [N·m]")
|
||||||
|
ap.add_argument("--position-limit", type=float, default=None, help="位置限位 ±rad")
|
||||||
|
ap.add_argument("--load-torque", type=float, default=-3.0,
|
||||||
|
help="输出端恒值负载 [N·m]（负=阻力，默认 -3.0）")
|
||||||
|
ap.add_argument("--damping", type=float, default=0.01, help="关节粘性阻尼（默认 0.01）")
|
||||||
|
ap.add_argument("--mode", choices=["normal", "overload"], default="normal",
|
||||||
|
help="仿真模式（normal / overload，默认 normal）")
|
||||||
|
ap.add_argument("--show", action="store_true", help="弹可视化窗口（默认无头）")
|
||||||
|
ap.add_argument("--plot", action="store_true", help="导出曲线 PNG")
|
||||||
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
urdf = os.path.abspath(args.urdf)
|
||||||
|
if not os.path.isfile(urdf):
|
||||||
|
sys.exit(f"找不到 URDF：{urdf}")
|
||||||
|
|
||||||
|
work_dir = os.path.abspath(args.work_dir) if args.work_dir else os.path.dirname(urdf)
|
||||||
|
os.makedirs(work_dir, exist_ok=True)
|
||||||
|
|
||||||
|
module_id = args.module_id or robot_name_of(urdf)
|
||||||
|
xml_path = os.path.join(work_dir, module_id + ".xml")
|
||||||
|
json_path = os.path.join(work_dir, module_id + ".json")
|
||||||
|
mesh_dir = os.path.join(work_dir, "meshes")
|
||||||
|
|
||||||
|
py = sys.executable
|
||||||
|
|
||||||
|
# 1) URDF → MJCF
|
||||||
|
cmd1 = [py, os.path.join(SCRIPTS_DIR, "urdf_to_mjcf.py"),
|
||||||
|
"--urdf", urdf, "--out", xml_path, "--meshdir", mesh_dir,
|
||||||
|
"--input", args.input, "--output", args.output,
|
||||||
|
"--damping", str(args.damping),
|
||||||
|
"--torque-limit", str(args.torque_limit)]
|
||||||
|
run(cmd1, "① URDF → MJCF")
|
||||||
|
|
||||||
|
# 2) URDF → schema
|
||||||
|
cmd2 = [py, os.path.join(SCRIPTS_DIR, "generate_schema.py"),
|
||||||
|
"--urdf", urdf, "--input", args.input, "--output", args.output,
|
||||||
|
"--model", os.path.basename(xml_path), "--out", json_path,
|
||||||
|
"--module-id", module_id,
|
||||||
|
"--torque-limit", str(args.torque_limit),
|
||||||
|
"--load-torque", str(args.load_torque),
|
||||||
|
"--damping", str(args.damping),
|
||||||
|
"--mode", args.mode]
|
||||||
|
if args.ratio is not None:
|
||||||
|
cmd2 += ["--ratio", str(args.ratio)]
|
||||||
|
if args.position_limit is not None:
|
||||||
|
cmd2 += ["--position-limit", str(args.position_limit)]
|
||||||
|
run(cmd2, "② URDF → schema")
|
||||||
|
|
||||||
|
# 3) schema → 仿真报告
|
||||||
|
cmd3 = [py, os.path.join(SCRIPTS_DIR, "simulate_report.py"),
|
||||||
|
"--schema", json_path, "--headless"]
|
||||||
|
if args.plot:
|
||||||
|
cmd3 += ["--plot"]
|
||||||
|
if args.show:
|
||||||
|
cmd3.remove("--headless")
|
||||||
|
run(cmd3, "③ 仿真 + 报告")
|
||||||
|
|
||||||
|
print("\n全部完成。产物：")
|
||||||
|
print(f" MJCF : {xml_path}")
|
||||||
|
print(f" schema : {json_path}")
|
||||||
|
print(f" 报告 : {os.path.join(work_dir, 'report.txt')}")
|
||||||
|
print(f" 观测 : {os.path.join(work_dir, 'timeseries.csv')}")
|
||||||
|
if args.plot:
|
||||||
|
print(f" 曲线 : {os.path.join(work_dir, 'report_curves.png')}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,126 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<robot name="planetary_joint_split_motor_demo">
|
||||||
|
<link name="base_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/fixed_structure.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="material_fixed_structure.stl">
|
||||||
|
<color rgba="0.62 0.68 0.72 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/motor_stator.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="material_motor_stator.stl">
|
||||||
|
<color rgba="0.30 0.34 0.38 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="sun_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/sun_drive.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="material_sun_drive.stl">
|
||||||
|
<color rgba="0.95 0.62 0.16 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/motor_rotor.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="material_motor_rotor.stl">
|
||||||
|
<color rgba="0.80 0.33 0.16 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="carrier_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/carrier_link.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="material_carrier_link.stl">
|
||||||
|
<color rgba="0.28 0.56 0.82 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="planet_0_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="-0.0535 0.0239016371622 0.00217065450165" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/planet_0_link.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="planet_steel">
|
||||||
|
<color rgba="0.76 0.78 0.80 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="planet_1_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="-0.0535 -0.0138306605224 0.0196140977237" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/planet_1_link.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="planet_steel">
|
||||||
|
<color rgba="0.76 0.78 0.80 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<link name="planet_2_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="-0.0535 -0.0100709766398 -0.0217847522253" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/planet_2_link.stl" scale="0.001 0.001 0.001" />
|
||||||
|
</geometry>
|
||||||
|
<material name="planet_steel">
|
||||||
|
<color rgba="0.76 0.78 0.80 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="sun_input_joint" type="revolute">
|
||||||
|
<parent link="base_link" />
|
||||||
|
<child link="sun_link" />
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-37.6991118431" upper="37.6991118431" effort="1" velocity="6.28318530718" />
|
||||||
|
</joint>
|
||||||
|
<joint name="carrier_output_joint" type="revolute">
|
||||||
|
<parent link="base_link" />
|
||||||
|
<child link="carrier_link" />
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-37.6991118431" upper="37.6991118431" effort="1" velocity="6.28318530718" />
|
||||||
|
<mimic joint="sun_input_joint" multiplier="0.166666666666667" offset="0" />
|
||||||
|
</joint>
|
||||||
|
<joint name="planet_0_spin_joint" type="revolute">
|
||||||
|
<parent link="carrier_link" />
|
||||||
|
<child link="planet_0_link" />
|
||||||
|
<origin xyz="0.0535 -0.0239016371622 -0.00217065450165" rpy="0 0 0" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-37.6991118431" upper="37.6991118431" effort="1" velocity="6.28318530718" />
|
||||||
|
<mimic joint="sun_input_joint" multiplier="-0.416666666666667" offset="0" />
|
||||||
|
</joint>
|
||||||
|
<joint name="planet_1_spin_joint" type="revolute">
|
||||||
|
<parent link="carrier_link" />
|
||||||
|
<child link="planet_1_link" />
|
||||||
|
<origin xyz="0.0535 0.0138306605224 -0.0196140977237" rpy="0 0 0" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-37.6991118431" upper="37.6991118431" effort="1" velocity="6.28318530718" />
|
||||||
|
<mimic joint="sun_input_joint" multiplier="-0.416666666666667" offset="0" />
|
||||||
|
</joint>
|
||||||
|
<joint name="planet_2_spin_joint" type="revolute">
|
||||||
|
<parent link="carrier_link" />
|
||||||
|
<child link="planet_2_link" />
|
||||||
|
<origin xyz="0.0535 0.0100709766398 0.0217847522253" rpy="0 0 0" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-37.6991118431" upper="37.6991118431" effort="1" velocity="6.28318530718" />
|
||||||
|
<mimic joint="sun_input_joint" multiplier="-0.416666666666667" offset="0" />
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,93 @@
|
|||||||
|
{
|
||||||
|
"schema": "simplecad.temporary_split_motor_urdf_demo.v1",
|
||||||
|
"temporary_demo_only": true,
|
||||||
|
"project_sources_modified": false,
|
||||||
|
"formal_motor_definition_remains": "single immutable 3500 COTS assembly",
|
||||||
|
"split_motor": {
|
||||||
|
"stator_step": "/Users/jerry/Downloads/3500-motor/3500-Motor_part2-定子.STEP",
|
||||||
|
"rotor_step": "/Users/jerry/Downloads/3500-motor/3500-Motor_part2-转子.STEP",
|
||||||
|
"stator_sha256": "91cc6a3d9033e74ff4d78b9802ceae32def15d5222c9537d50e7f3dfebab937f",
|
||||||
|
"rotor_sha256": "15c15bd056593d9672b4b7c331446e76c24ebb241ae59c61734b467d10d24e3b",
|
||||||
|
"stator_solid_count": 37,
|
||||||
|
"rotor_solid_count": 9,
|
||||||
|
"original_motor_solid_count": 46,
|
||||||
|
"motor_placement": {
|
||||||
|
"origin": [
|
||||||
|
2.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"x_axis": [
|
||||||
|
0.0,
|
||||||
|
-0.9883811652466841,
|
||||||
|
-0.15199563212673947
|
||||||
|
],
|
||||||
|
"y_axis": [
|
||||||
|
1.0,
|
||||||
|
0.0,
|
||||||
|
0.0
|
||||||
|
],
|
||||||
|
"z_axis": [
|
||||||
|
0.0,
|
||||||
|
-0.15199563212673947,
|
||||||
|
0.9883811652466841
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"rotor_motion_group": "sun_link"
|
||||||
|
},
|
||||||
|
"kinematics": {
|
||||||
|
"driver": "sun_input_joint",
|
||||||
|
"axis": [
|
||||||
|
1,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"rotor_sun_multiplier": 1.0,
|
||||||
|
"carrier_multiplier": 0.16666666666666666,
|
||||||
|
"planet_relative_multiplier": -0.4166666666666667,
|
||||||
|
"ring_fixed": true
|
||||||
|
},
|
||||||
|
"meshes": [
|
||||||
|
{
|
||||||
|
"path": "meshes/fixed_structure.stl",
|
||||||
|
"bytes": 1538584,
|
||||||
|
"sha256": "48754d3f41032aa1b43cd2ddb3d48442c70c558f85458135184200aa2e45dc9c"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/motor_stator.stl",
|
||||||
|
"bytes": 28572384,
|
||||||
|
"sha256": "fa61093b5a1b97f0f6db070d05f1376492c449b068bac957b52a6201733fc58e"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/sun_drive.stl",
|
||||||
|
"bytes": 112484,
|
||||||
|
"sha256": "dff39aab0a1a6df379c53459df2961ba4ad60682568693a6490a0cce3cae065e"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/motor_rotor.stl",
|
||||||
|
"bytes": 526184,
|
||||||
|
"sha256": "5f0a58304dbfd785fc3fdc0aa3d2c2557fbc151e81adbd5f11da1ad9ee8686be"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/carrier_link.stl",
|
||||||
|
"bytes": 508184,
|
||||||
|
"sha256": "eca27acef8eefe282f92c7042db02dcf0a65516c9a97b0aa7668dabc9110a8ff"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/planet_0_link.stl",
|
||||||
|
"bytes": 159284,
|
||||||
|
"sha256": "6fb3cff6c91186f39f098c1e3e619f88af9be56fe31c81ed327b5e5f243ceaa5"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/planet_1_link.stl",
|
||||||
|
"bytes": 159284,
|
||||||
|
"sha256": "53474223ba8cdc31d9353e2a237823e4ce93473c9c85135cd769db126452571d"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "meshes/planet_2_link.stl",
|
||||||
|
"bytes": 159284,
|
||||||
|
"sha256": "1dd79f134d39b79350d9f39663f2b786002496289e3b8936793ac7a34d693864"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"urdf_sha256": "d5e3bf1625a639346d616334fd8677c6af97d86f8cc58dde082a7a9dd988801f"
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user