#!/usr/bin/env python3 """ 关节模组 URDF → MJCF 转换小工具（专用版） ===================================================================== 把一个「关节模组」的 URDF 转成 MuJoCo 可用的 MJCF（.xml），**不改变任何运动学 / 动力学 / 传动关系**，只补上原 URDF 里缺失、但仿真必需的部分： 1. 质量 / 重心 / 惯性张量 —— 从每个 link 的 STL 网格做体积分（trimesh），按材料密度（脚本顶部的常数表）算出，多网格用平行轴定理合成。 2. —— 输入电机 + 输出负载（原 URDF 没有）。 3. 渲染网格覆盖 —— 个别 STL 面数超过 MuJoCo 上限(20万)，渲染时换降采样版。已严格保真的部分（直接照搬 URDF，绝不动）： - 关节层级：/ 决定 body 的父子关系； → 子（不是！） → 子（rpy 反转，z-y-x） - 转轴： → （子 link 系内，直接映射） - 限位： → - 减速比： → polycoef（joint1 = offset + multiplier·joint2） - 视觉： → （仅视觉，contype/conaffinity=0）关键 MuJoCo 语义（易错点，务必保持）： - MuJoCo 的是转轴相对 **body 帧** 的偏移，默认 0 即轴过 body 原点。 URDF 的是「子 link 系相对父系」的位姿，对应子。两者不是一回事 —— 填错会把行星轮挂到体外轴上公转。 - 等价 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 读取（见 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)，无返回 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)，无返回 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=" 自动降采样 > 原文件。""" 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() # 顶层（本案例用的是视觉内联，这里留作兜底） 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): """关节模组自动识别输入/输出端：输入 = 唯一没有的关节（独立驱动源）；输出 = 以正 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 不写」表达（子 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：固连，不生成 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'') def geom_xml(vis, asset_scales): base = vis["mesh_base"] if base is None: return None # 记录该 mesh 的渲染 scale（来自 URDF ）。同一 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 "" 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}') # 惯性（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 关节固连不生成，仅靠 body 的 pos/euler 定位） if j is not None: jxml = joint_xml(j) if jxml is not None: lines.append(ind + " ") # 视觉几何 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}") 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'') lines.append(' ') lines.append(' ') lines.append('') lines.append(f' ') 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()