初始提交：关节模组仿真平台
- 三接口契约：自包含 MJCF / 配置 schema / 报告计算规范 - Python 流水线：urdf_to_mjcf → generate_schema → simulate_report（validate_module 一键编排） - 输入案例 urdf + 生成产物 output（自包含 MJCF/schema/报告/网格副本） - 详细架构说明 docs/architecture.md
This commit is contained in:
@@ -0,0 +1,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()
|
||||
Reference in New Issue
Block a user