chenshijue/JointModule
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始提交：关节模组仿真平台

Browse Source 
- 三接口契约：自包含 MJCF / 配置 schema / 报告计算规范
- Python 流水线：urdf_to_mjcf → generate_schema → simulate_report（validate_module 一键编排）
- 输入案例 urdf + 生成产物 output（自包含 MJCF/schema/报告/网格副本）
- 详细架构说明 docs/architecture.md
This commit is contained in:
chenshijue
2026-08-28 11:37:47 +08:00
commit 4240bb889a
34 changed files with 6294 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/generate_schema.py
+189
View File
@@ -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()
scripts/simulate_report.py
+396
View File
@@ -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()
scripts/urdf_to_mjcf.py
+600
View File
@@ -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()
scripts/validate_module.py
+134
View File
@@ -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()