chenshijue
4240bb889a
- 三接口契约：自包含 MJCF / 配置 schema / 报告计算规范 - Python 流水线：urdf_to_mjcf → generate_schema → simulate_report（validate_module 一键编排） - 输入案例 urdf + 生成产物 output（自包含 MJCF/schema/报告/网格副本） - 详细架构说明 docs/architecture.md
601 lines
25 KiB
Python
601 lines
25 KiB
Python
#!/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()
|