#!/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()