diff --git a/.gitignore b/.gitignore
index 917491c..b050463 100644
--- a/.gitignore
+++ b/.gitignore
@@ -10,3 +10,6 @@ old/
# 本地脚本(内网地址,不随 SDK 发布)
run_02_movej.sh
+
+# tau_eval 采集/分析产物(不入库)
+runs/
diff --git a/tests/test_tau_eval.py b/tests/test_tau_eval.py
new file mode 100644
index 0000000..4fe4343
--- /dev/null
+++ b/tests/test_tau_eval.py
@@ -0,0 +1,224 @@
+"""Offline tests for tools/tau_eval — no hardware, no numpy/matplotlib."""
+from __future__ import annotations
+
+import json
+import math
+import os
+import re
+import sys
+import xml.etree.ElementTree as ET
+
+import pytest
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_TOOLS = os.path.join(os.path.dirname(_HERE), "tools")
+if _TOOLS not in sys.path:
+ sys.path.insert(0, _TOOLS)
+
+from tau_eval import filters, rnea # noqa: E402
+from tau_eval.analyze import AnalyzeOptions, analyze_session # noqa: E402
+from tau_eval.collect import (Sample, ScenarioSpec, Session, # noqa: E402
+ validate_scenario)
+from tau_eval.model import factory_snapshot # noqa: E402
+from tau_eval.plots import generate_all # noqa: E402
+from tau_eval.report import write_report # noqa: E402
+
+
+# ---------------------------------------------------------------------------
+# RNEA: firmware golden probe (dyn.c MODEL_PROBE_Q / G_FACTORY_PROBE)
+# ---------------------------------------------------------------------------
+
+@pytest.mark.parametrize("q, expected_max", [
+ ([0.0, -0.22, 0.94, 0.0, 0.0, 0.0, 0.0], 2.1340647),
+ ([0.0, 0.62, -1.60, 0.0, 0.0, 0.0, 0.0], 7.5729609),
+])
+def test_rnea_matches_firmware_gravity_probe(q, expected_max):
+ snap = factory_snapshot()
+ g = rnea.gravity_torque(q, snap.effective_bodies(), (0.0, 0.0, -9.81))
+ assert max(abs(v) for v in g) == pytest.approx(expected_max, abs=1.5e-4)
+
+
+def test_mass_matrix_is_consistent_with_rnea():
+ snap = factory_snapshot()
+ bodies = snap.effective_bodies()
+ q = [0.1, -0.2, 0.3, -0.4, 0.2, 0.1, -0.1]
+ ddq = [0.5, -0.3, 0.2, 0.1, -0.2, 0.3, -0.1]
+ M = rnea.mass_matrix(q, bodies)
+ tau = rnea.inertia_torque(q, ddq, bodies)
+ pred = [sum(M[i][j] * ddq[j] for j in range(7)) for i in range(7)]
+ for a, b in zip(tau, pred):
+ assert a == pytest.approx(b, abs=1e-9)
+
+
+def test_theory_torques_total_is_sum_of_parts():
+ snap = factory_snapshot()
+ q = [0.05, -0.1, 0.2, -0.15, 0.1, 0.05, -0.05]
+ dq = [0.1, -0.2, 0.15, 0.05, -0.1, 0.2, -0.05]
+ ddq = [0.3, -0.1, 0.2, 0.05, -0.15, 0.1, -0.2]
+ d = rnea.theory_torques(snap, q, dq, ddq)
+ for i in range(7):
+ assert d["total"][i] == pytest.approx(
+ d["gravity"][i] + d["coriolis"][i] + d["inertia"][i]
+ + d["jm"][i] + d["friction"][i], abs=1e-12)
+
+
+# ---------------------------------------------------------------------------
+# Filters / resampling / metrics
+# ---------------------------------------------------------------------------
+
+def test_uniform_grid_and_linear_resample():
+ grid = filters.uniform_grid(0.0, 1.0, 10.0)
+ assert len(grid) == 11
+ assert grid[-1] == pytest.approx(1.0)
+ out = filters.linear_resample([0.0, 1.0], [0.0, 2.0], [0.0, 0.5, 1.0])
+ assert out == [0.0, 1.0, 2.0]
+ # 端点外钳住
+ assert filters.linear_resample([0.0, 1.0], [3.0, 4.0], [-1.0, 2.0]) == [3.0, 4.0]
+
+
+def test_zero_phase_lowpass_preserves_dc_and_phase():
+ fs = 100.0
+ n = 1000
+ x = [1.0] * n
+ y = filters.lowpass_zero_phase(x, fs, 5.0)
+ # 稳态直流增益必须为 1 (边缘反射填充的过渡段会有 ~1e-6 残差, 跳过)
+ assert min(y[100:-100]) == pytest.approx(1.0, abs=1e-12)
+
+ t = [k / fs for k in range(n)]
+ s = [math.sin(2 * math.pi * 1.0 * ti) for ti in t]
+ y = filters.lowpass_zero_phase(s, fs, 10.0)
+ assert filters.pearson(s[50:-50], y[50:-50]) > 0.9999
+ # 零相位: 峰值时刻不移动 (普通因果滤波会右移)
+ # 1 Hz 信号每 100 点一个峰, 取窗口中段的单个峰以避免周期重复取到别的波峰
+ lo, hi = 400, 450
+ i_s = max(range(lo, hi), key=lambda i: s[i])
+ i_y = max(range(lo, hi), key=lambda i: y[i])
+ assert abs(i_y - i_s) <= 1
+
+
+def test_central_diff_matches_analytic_derivative():
+ fs = 200.0
+ f = 2.0
+ t = [k / fs for k in range(400)]
+ x = [math.sin(2 * math.pi * f * ti) for ti in t]
+ dx = filters.central_diff(x, 1.0 / fs)
+ want = [2 * math.pi * f * math.cos(2 * math.pi * f * ti) for ti in t]
+ err = max(abs(dx[i] - want[i]) for i in range(2, len(t) - 2))
+ # 中心差分截断误差上界 = |x'''| dt^2/6 = (2*pi*f)^3 dt^2/6
+ bound = (2 * math.pi * f) ** 3 * (1.0 / fs) ** 2 / 6.0
+ assert err < bound * 1.05
+
+
+def test_metrics_helpers():
+ x = [1.0, 2.0, 3.0, 4.0]
+ assert filters.mean(x) == 2.5
+ assert filters.pstdev(x) == pytest.approx(math.sqrt(5.0 / 3.0))
+ assert filters.quantile([0.0, 10.0], 0.5) == 5.0
+ assert filters.pearson(x, [2.0, 4.0, 6.0, 8.0]) == pytest.approx(1.0)
+ a, b, r = filters.linear_fit(x, [3.0, 5.0, 7.0, 9.0])
+ assert (a, b) == pytest.approx((1.0, 2.0))
+ assert r == pytest.approx(1.0)
+ assert filters.r2_score(x, x) == 1.0
+
+
+# ---------------------------------------------------------------------------
+# Session I/O
+# ---------------------------------------------------------------------------
+
+def _jittered_session(n_samples=300, n_joints=7):
+ out = []
+ t = 0.0
+ for k in range(n_samples):
+ t += 0.01 if k % 3 else 0.013
+ out.append(Sample(
+ t=t, seq=k, mode=3, mode_name="POS", flags=0x200, joint_fault=0,
+ q=[0.1 * math.sin(t) + 0.01 * j for j in range(n_joints)],
+ dq=[0.1 * math.cos(t) for _ in range(n_joints)],
+ tau=[0.2 * j for j in range(n_joints)],
+ t_mos=[30.0 + j for j in range(n_joints)],
+ t_coil=[31.0 + j for j in range(n_joints)],
+ err=[0] * n_joints))
+ return Session(out, {"fs": 100.0, "scenario": "unit"})
+
+
+def test_session_resample_is_uniform(tmp_path):
+ sess = _jittered_session()
+ r = sess.resample(50.0)
+ dts = [b.t - a.t for a, b in zip(r.samples, r.samples[1:])]
+ assert max(abs(d - 0.02) for d in dts) < 1e-9
+ assert r.n == 7
+ paths = r.dump(str(tmp_path))
+ assert os.path.exists(paths["status_csv"])
+ back = Session.load(str(tmp_path))
+ assert len(back.samples) == len(r.samples)
+ assert back.samples[10].q[3] == pytest.approx(r.samples[10].q[3], abs=1e-9)
+
+
+# ---------------------------------------------------------------------------
+# Scenario safety
+# ---------------------------------------------------------------------------
+
+def test_scenario_limit_margin_is_enforced():
+ limits = [(-3.0, 3.0)] * 7
+ with pytest.raises(ValueError, match="余量"):
+ validate_scenario(ScenarioSpec(kind="hold", q_center=[2.95] + [0.0] * 6,
+ duration=1.0), limits)
+ with pytest.raises(ValueError, match="amp"):
+ validate_scenario(ScenarioSpec(kind="sine", q_center=[0.0] * 7,
+ amp=0.0, freq=0.2, duration=1.0), limits)
+
+
+# ---------------------------------------------------------------------------
+# Synthetic end-to-end: generator model == analyzer model
+# ---------------------------------------------------------------------------
+
+def _synthetic_session(fs=100.0, duration=2.0):
+ snap = factory_snapshot()
+ snap.firmware = "synthetic-test"
+ samples = []
+ amp, f = 0.15, 0.3
+ for k in range(int(duration * fs) + 1):
+ t = k / fs
+ q, dq, ddq = [], [], []
+ for j in range(7):
+ ph = 2 * math.pi * f * t + 0.3 * j
+ q.append(amp * math.sin(ph))
+ dq.append(amp * 2 * math.pi * f * math.cos(ph))
+ ddq.append(-amp * (2 * math.pi * f) ** 2 * math.sin(ph))
+ d = rnea.theory_torques(snap, q, dq, ddq)
+ samples.append(Sample(
+ t=t, seq=k, mode=3, mode_name="POS", flags=0x200, joint_fault=0,
+ q=q, dq=dq, tau=list(d["total"]),
+ t_mos=[30.0] * 7, t_coil=[31.0] * 7, err=[0] * 7))
+ return snap, Session(samples, {"fs": fs, "scenario": "sine"})
+
+
+def test_analysis_and_outputs_offline(tmp_path):
+ snap, sess = _synthetic_session()
+ result = analyze_session(sess, snap, AnalyzeOptions())
+ agg = result.summary["metrics"]["raw"]["aggregate"]
+ assert agg["n_joints"] == 7
+ assert agg["mean_rel_rmse"] < 1e-3 # 同一模型回放, 只应有数值噪声
+ assert result.summary["diagnosis"][0]["source"] == "模型与实测高度一致"
+
+ d = tmp_path / "session"
+ paths = sess.dump(str(d))
+ (d / "model.json").write_text(json.dumps(snap.to_dict()), encoding="utf-8")
+ assert os.path.exists(paths["meta_json"])
+
+ figs = generate_all(result, str(d / "figures"))
+ assert len(figs) >= 20
+ for p in figs.values():
+ ET.parse(p) # 每个 SVG 都必须是合法 XML
+
+ # x 轴必须覆盖整个绘图区 (曾因 xhi 取了首点的 y 值, 曲线被压到左边缘)
+ svg = open(figs["j1_tau"], encoding="utf-8").read()
+ xs = [float(m.group(1)) for m in re.finditer(r"[ML](-?[\d.]+),(-?[\d.]+)", svg)]
+ assert min(xs) >= 77.5 and max(xs) <= 936.5
+ assert max(xs) - min(xs) > 800
+
+ report = write_report(result, figs, str(d), command="pytest")
+ text = open(report, encoding="utf-8").read()
+ assert "电流环力矩估计精度评估报告" in text
+ assert "逐轴指标" in text
+ assert "figures/joint1_tau.svg" in text
diff --git a/tools/tau_eval/README.md b/tools/tau_eval/README.md
new file mode 100644
index 0000000..62940c7
--- /dev/null
+++ b/tools/tau_eval/README.md
@@ -0,0 +1,201 @@
+# tau_eval — current-loop torque estimation accuracy evaluation
+
+Python implementation of issue
+[#44](https://github.com/nexform-tech/litearm-python/issues/44):
+collect → inverse dynamics → error visualization.
+
+The tool records every `q / dq / tau` status frame from the arm, replays a
+rigid-body inverse dynamics model (the same world-frame RNEA the firmware
+runs), and reports per-joint error metrics, SVG figures and a Markdown
+report. Everything is reproducible from the persisted session files and the
+options recorded in `metrics.json`.
+
+```
+collect (hardware) ──► status.csv + meta.json + model.json
+ │
+ ▼
+analyze (offline) ──► metrics.json + timeseries.csv + figures/*.svg + report.md
+```
+
+## Requirements
+
+- Python 3.9+ (the SDK's minimum), standard library only for the analysis path.
+- Hardware collection uses the `litearm` SDK (pyserial). Optional PyArrow
+ enables Parquet output (`--parquet`).
+- No numpy / matplotlib; figures are emitted as plain SVG.
+
+## Quick start
+
+```bash
+# 1. Dry run: validate the plan against the firmware soft limits, no motion.
+python3 tools/tau_eval/run.py
+
+# 2. Real run: enable, execute three scenarios, analyze, plot, report.
+python3 tools/tau_eval/run.py --go
+
+# 3. Re-analyze a recorded run with different filter settings.
+python3 tools/tau_eval/run.py analyze runs/20261029_153000_tau_eval \
+ --cutoff-ddq 3 --gravity 0,9.81,0
+```
+
+`run.py` can be launched from any working directory. `--help` on every
+sub-command lists the options.
+
+## Scenarios
+
+`--scenario all` (default) runs these in order:
+
+| kind | excitation | purpose |
+|---|---|---|
+| `hold` | static hold at the centre pose, `move_js` streamed at `--command-hz` | gravity model + static bias |
+| `sine` | low-speed sine `q0 + amp·sin(2πft)` with analytic `dq`, amplitudes automatically narrowed until inside the soft-limit margin | inertia / Coriolis / friction, delay |
+| `poses` | several slow point-to-point moves, each streamed as a jerk-continuous `move_js` reference with a dwell at the target | gravity across the workspace |
+
+Defaults: 3 s hold, 10 s sine (0.12 rad, 0.20 Hz), then up to five poses at
+1.0/0.7/0.5/0.35/0.25 of the usable range with a 1.2 s dwell each (targets that
+fail the margin check are dropped).
+
+Every scenario is driven exclusively through `move_js` re-sent at
+`--command-hz` (default 50 Hz). Two reasons:
+
+- The firmware has a 0.1 s command watchdog; a silent hold fails soft (lower
+ stiffness, gravity feed-forward cut) and the recorded "hold" would be a
+ watchdog-tripped artifact. The hold and the dwells therefore stream a
+ constant `move_js` setpoint for their whole duration.
+- `poses` does **not** use `movej`. Its static hold relies on position error,
+ and the gravity feed-forward bias on a bench with a non-default gravity
+ vector leaves 0.04–0.05 rad of steady-state error — above the SDK arrival
+ tolerance (`q_tol = 0.03 rad`), so `movej` would raise
+ `MotionTimeoutError` and lose the whole segment. Instead each move streams a
+ smoothstep reference (`POSE_SPEED_RAD_S = 0.5 rad/s` × `--speed`, min 0.6 s
+ per move) with its analytic velocity, finishes with an exact target frame at
+ zero velocity, then dwells.
+
+## Collected data
+
+Per scenario directory:
+
+| file | content |
+|---|---|
+| `status.csv` | uniform-grid samples: `t, seq, mode, flags, joint_fault, q0..q6, dq.., tau.., t_mos.., t_coil.., err..` |
+| `meta.json` | timestamps, host, scenario spec/plan/result, fs, firmware version, joint count, geometry hash, payload, gravity, scales, friction model, soft limits, initial/final state, motor and ambient temperatures, static gravity check, warnings |
+| `model.json` | full body / joint model snapshot used for the inverse dynamics (`ModelSnapshot.to_dict()`) |
+| `status.parquet` | optional, identical columns (`--parquet`) |
+
+The recorder hooks `arm._a._on_status` — the single status funnel of the SDK —
+so it never opens a second reader and does not disturb the SDK's own arrival
+logic. Frames are resampled to a uniform grid at save time; discrete fields
+(`seq`, `mode`, `flags`, `err`) take the most recent value, continuous fields
+are linearly interpolated.
+
+## Inverse dynamics
+
+For each sample the tool evaluates
+
+```
+tau = M(q)·ddq + C(q,dq)·dq + G(q) + tau_jm + tau_friction
+```
+
+with the firmware model snapshot (9 bodies, 7 joints), in newtons · metres and
+radians, joint order as reported by the SDK. Gravity is injected as a base
+linear acceleration `−g`, matching the firmware; the default vector is
+`(0, 0, -9.81)` and can be overridden with `--gravity x,y,z` (the test bench
+uses `(0, +9.81, 0)`).
+
+`analyze.rnea.gravity_torque` is verified against the firmware golden probe
+(`MODEL_PROBE_Q` / `G_FACTORY_PROBE`) in `tests/test_tau_eval.py`.
+
+### Processing chain (all options recorded in `metrics.json`)
+
+1. Confirm the uniform sampling grid (re-sample if the jitter exceeds 2 %).
+2. Zero-phase 2nd-order Butterworth low-pass (`filtfilt`): `q/dq` at
+ `--cutoff-dq` 15 Hz, measured `tau` at `--cutoff-tau` 10 Hz.
+3. `ddq` by central difference, then low-pass at `--cutoff-ddq` 5 Hz.
+4. Evaluate the inverse dynamics per sample.
+5. Trim `max(2/cutoff_ddq, 0.1)` s at both ends to drop the reflection
+ transient of the zero-phase filter.
+
+Zero-phase filtering is used deliberately: a causal filter would add a
+frequency-dependent delay between the measured and the model torque. The
+residual phase lag that remains (sensor latency, current-loop bandwidth) is
+measured explicitly by scanning lags in ±`--max-delay-ms`.
+
+## Outputs
+
+- `metrics.json` — aggregate, per-joint (`rmse, mae, bias, std, peak_abs,
+ rel_rmse, r2, corr, meas_rms, model_mean, meas_mean`), speed and pose
+ strata, delay scan, residual correlations, component RMS, static bias,
+ diagnosis.
+- `timeseries.csv` — per joint: `q, dq, ddq, tau_meas, tau_model, tau_scaled,
+ G, C, I, F`.
+- `figures/*.svg` — per-joint torque comparison, residual, histogram and
+ speed-strata bars; RMSE/bias bars; measured-vs-model parity scatter; delay
+ scan; normalized residual histogram (≈ 33 files).
+- `report.md` — summary, diagnosis, method, per-joint tables, limitations,
+ and the exact reproduce command.
+- `summary.md` at the run root — one row per scenario.
+
+## Reading the diagnosis
+
+`analyze.diagnose` inspects the residual structure and attributes the error:
+
+| evidence | likely source |
+|---|---|
+| static hold bias, signed, uniform across poses | gravity model or current-loop gain/offset calibration |
+| residual correlates with `sign(dq)` / `|dq|` | Coulomb / viscous friction model mismatch |
+| residual correlates with `ddq` / inertia term | body inertia (`I`) or COM parameters |
+| negative lag after delay scan shifts the minimum | filter or sensor delay (check `--cutoff-*`) |
+| mean relative RMSE < 2 % | model and measurement agree; residual is numerical noise |
+
+## Safety
+
+- Without `--go` the tool is read-only and only prints the plan.
+- Every target pose and sine envelope is validated against the firmware soft
+ limits with a `MIN_MARGIN = 0.15 rad` margin; invalid amplitudes are narrowed
+ automatically.
+- The arm is enabled before recording and **left enabled afterwards** — at a
+ held pose, disabling lets gravity drop the arm (same policy as
+ `examples/07`). Keep an e-stop within reach; disable deliberately when
+ supported.
+- A static gravity check compares the held `tau` with `model.get_gravity(q)`
+ and is skipped when any joint moves faster than 0.05 rad/s.
+- No torque/stiffness feed-forward is commanded by the sine scenario.
+
+## Limitations
+
+- `tau` is the current-loop estimate, not a joint torque sensor; current-loop
+ calibration errors are indistinguishable from model errors.
+- The factory model is regression-identified: masses are mostly 1, inertias
+ can be negative and the mass matrix is not positive definite. The tool never
+ inverts `M`; it only evaluates the forward product.
+- Gravity is validated only in static holds; inertia/friction identification
+ is out of scope (see `examples/09_payload_identify.py`).
+- Motor winding / MOS temperatures are reported, but ambient temperature must
+ be supplied manually (`--ambient-c`).
+- The default gravity is `(0, 0, -9.81)`; a bench mounted differently must pass
+ `--gravity`.
+- The tool covers a single arm configuration; tool payload has to be described
+ in the model snapshot (or identified separately) before comparing absolute
+ torques.
+
+## Hardware-free example
+
+```bash
+python3 tools/tau_eval/examples/make_synthetic.py
+```
+
+The script synthesises a sine session from the factory model with the same
+RNEA the analyzer uses, then runs analyze → figures → report. The residual is
+pure numerical noise (mean RMSE ≈ 3e-6 N·m), so it is a smoke test for the
+whole pipeline. Output goes to `tools/tau_eval/examples/synthetic_sine/`
+(ignored by git — generated artifacts are never committed).
+
+## Tests
+
+```bash
+python3 -m pytest tests/test_tau_eval.py -q
+```
+
+Offline: firmware gravity golden probe, mass-matrix/RNEA consistency, filter
+and resampling properties, CSV round-trip, soft-limit validation, and a
+synthetic end-to-end analyze/plot/report run.
diff --git a/tools/tau_eval/__init__.py b/tools/tau_eval/__init__.py
new file mode 100644
index 0000000..ce11b9b
--- /dev/null
+++ b/tools/tau_eval/__init__.py
@@ -0,0 +1,4 @@
+# -*- coding: utf-8 -*-
+"""LiteArm 电流环力矩估计精度评估工具。"""
+
+__all__ = ["model", "rnea", "filters", "collect", "analyze", "plots", "report", "cli"]
diff --git a/tools/tau_eval/analyze.py b/tools/tau_eval/analyze.py
new file mode 100644
index 0000000..9f6bbf8
--- /dev/null
+++ b/tools/tau_eval/analyze.py
@@ -0,0 +1,617 @@
+# -*- coding: utf-8 -*-
+"""逆动力学误差分析 —— 实测 τ (电流环) vs 模型 τ = M·ddq + C·dq + G + friction + jm·ddq。
+
+处理链 (每一步都可复现, 参数写进 metrics.json):
+
+ 1. 均匀重采样 会话本就按 fs 存, 这里再确认网格;
+ 2. 零相位低通 q/dq 用 `--cutoff-dq` (默认 15Hz), τ 用 `--cutoff-tau` (默认 10Hz);
+ 3. 中心差分 + 低通 ddq = filtfilt(diff(dq)) (`--cutoff-ddq`, 默认 5Hz);
+ 4. 逆动力学 逐采样调用 rnea.theory_torques (固件同款世界系 RNEA);
+ 5. 边沿裁剪 前后各裁 max(2/cutoff_ddq, 0.1)s, 避开 filtfilt 的反射瞬态;
+ 6. 指标/分层/延迟扫描。
+
+为什么用零相位滤波: 普通因果低通会把 ddq 整体推迟几毫秒, 误差会被误归因成
+'惯量模型错'。filtfilt 在通带内净相位 0, 只在两端留 ~2/fc 的瞬态, 所以第 5 步裁剪。
+
+指标口径:
+ residual = τ_meas − τ_model (N·m)
+ bias = mean(residual)
+ RMSE/MAE/peak 都基于 residual
+ rel_RMSE = RMSE / RMS(τ_meas)
+ R² = 1 − SS_res/SS_tot (对 τ_meas 的方差解释度)
+
+`raw` 用物理量 (不加 gravity_scale/inertia_scale); `scaled` 用固件前馈口径,
+两者都给出 —— 前者回答"模型对不对", 后者回答"固件前馈打得准不准"。
+"""
+from __future__ import annotations
+
+import copy
+import csv
+import json
+import math
+import os
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from . import filters, rnea
+from .collect import Session
+from .model import ModelSnapshot, N_JOINT
+
+
+# ---------------------------------------------------------------------------
+# 配置
+# ---------------------------------------------------------------------------
+
+@dataclass
+class AnalyzeOptions:
+ cutoff_dq: float = 15.0 # Hz
+ cutoff_ddq: float = 5.0 # Hz
+ cutoff_tau: float = 10.0 # Hz
+ trim_s: Optional[float] = None # None = max(2/cutoff_ddq, 0.1), 上限 span/5
+ gravity_override: Optional[Tuple[float, float, float]] = None
+ max_delay_ms: float = 100.0
+ static_dq: float = 0.05 # |dq| 低于此值视为静止
+ speed_edges: Tuple[float, ...] = (0.05, 0.3, 1.0)
+
+ def to_dict(self) -> Dict[str, object]:
+ return {"cutoff_dq_hz": self.cutoff_dq, "cutoff_ddq_hz": self.cutoff_ddq,
+ "cutoff_tau_hz": self.cutoff_tau, "trim_s": self.trim_s,
+ "gravity_override": list(self.gravity_override)
+ if self.gravity_override is not None else None,
+ "max_delay_ms": self.max_delay_ms, "static_dq": self.static_dq,
+ "speed_edges": list(self.speed_edges)}
+
+
+# ---------------------------------------------------------------------------
+# 结果容器
+# ---------------------------------------------------------------------------
+
+@dataclass
+class AnalysisResult:
+ fs: float
+ t: List[float]
+ q: List[List[float]] # [joint][sample]
+ dq: List[List[float]]
+ ddq: List[List[float]]
+ tau_meas: List[List[float]]
+ tau_model: List[List[float]]
+ tau_scaled: List[List[float]]
+ components: Dict[str, List[List[float]]]
+ options: AnalyzeOptions
+ snapshot: ModelSnapshot
+ summary: Dict[str, object] = field(default_factory=dict)
+
+ @property
+ def n(self) -> int:
+ return len(self.q)
+
+ @property
+ def n_samples(self) -> int:
+ return len(self.t)
+
+
+# ---------------------------------------------------------------------------
+# 基础工具
+# ---------------------------------------------------------------------------
+
+def _median(vals: Sequence[float]) -> float:
+ s = sorted(vals)
+ n = len(s)
+ if n == 0:
+ return float("nan")
+ m = n // 2
+ return s[m] if n % 2 else 0.5 * (s[m - 1] + s[m])
+
+
+def infer_fs(session: Session) -> float:
+ """优先用 meta 的 fs, 否则用到达间隔中位数。"""
+ fs = session.meta.get("fs")
+ if isinstance(fs, (int, float)) and fs > 0:
+ return float(fs)
+ ts = session.times()
+ if len(ts) < 2:
+ return 100.0
+ dt = _median([b - a for a, b in zip(ts, ts[1:]) if b > a])
+ return 1.0 / dt if dt > 0 else 100.0
+
+
+def _ensure_uniform(session: Session) -> Tuple[Session, float, bool]:
+ fs = infer_fs(session)
+ ts = session.times()
+ if len(ts) < 3:
+ return session, fs, False
+ dt = [b - a for a, b in zip(ts, ts[1:])]
+ nominal = 1.0 / fs
+ jitter = max(abs(d - nominal) for d in dt) / nominal
+ if jitter <= 0.02:
+ return session, fs, False
+ return session.resample(fs), fs, True
+
+
+def _filtered(x: Sequence[float], fs: float, fc: float) -> List[float]:
+ return filters.lowpass_zero_phase(x, fs, fc) if len(x) >= 4 else list(x)
+
+
+def _metrics(meas: Sequence[float], pred: Sequence[float]) -> Dict[str, object]:
+ n = len(meas)
+ if n == 0:
+ return {"n": 0}
+ res = [meas[i] - pred[i] for i in range(n)]
+ rmse = math.sqrt(sum(v * v for v in res) / n)
+ mae = sum(abs(v) for v in res) / n
+ peak = max(abs(v) for v in res)
+ bias = filters.mean(res)
+ std = filters.pstdev(res)
+ meas_rms = math.sqrt(sum(v * v for v in meas) / n)
+ rel = rmse / meas_rms if meas_rms > 1e-12 else float("nan")
+ return {
+ "n": n,
+ "rmse": round(rmse, 6),
+ "mae": round(mae, 6),
+ "bias": round(bias, 6),
+ "std": round(std, 6),
+ "peak_abs": round(peak, 6),
+ "rel_rmse": round(rel, 6) if rel == rel else None,
+ "r2": round(filters.r2_score(meas, pred), 6),
+ "corr": round(filters.pearson(meas, pred), 6),
+ "meas_rms": round(meas_rms, 6),
+ "model_mean": round(filters.mean(pred), 6),
+ "meas_mean": round(filters.mean(meas), 6),
+ }
+
+
+def _agg_metrics(per_joint: Dict[str, Dict[str, object]]) -> Dict[str, object]:
+ vals = [m for m in per_joint.values() if m.get("n")]
+ if not vals:
+ return {"n_joints": 0}
+ rmse = [m["rmse"] for m in vals]
+ return {
+ "n_joints": len(vals),
+ "mean_rmse": round(filters.mean(rmse), 6),
+ "max_rmse": round(max(rmse), 6),
+ "worst_joint": max(per_joint, key=lambda k: per_joint[k].get("rmse", -1)),
+ "mean_abs_bias": round(filters.mean([abs(m["bias"]) for m in vals]), 6),
+ "max_abs_bias": round(max(abs(m["bias"]) for m in vals), 6),
+ "mean_rel_rmse": round(filters.mean(
+ [m["rel_rmse"] for m in vals if m.get("rel_rmse") is not None]), 6),
+ }
+
+
+# ---------------------------------------------------------------------------
+# 分层
+# ---------------------------------------------------------------------------
+
+def _speed_bins(edges: Sequence[float]) -> List[Tuple[float, float, str]]:
+ """[0, e0) = static, [e0,e1) = slow, ... 最后一段到 ∞。"""
+ out = [(0.0, edges[0], "static")]
+ labels = ["slow", "mid", "fast", "very_fast"]
+ for k in range(len(edges) - 1):
+ out.append((edges[k], edges[k + 1], labels[min(k, len(labels) - 1)]))
+ out.append((edges[-1], float("inf"), labels[min(len(edges) - 1, len(labels) - 1)]))
+ return out
+
+
+def speed_strata(result: AnalysisResult) -> Dict[str, object]:
+ bins = _speed_bins(result.options.speed_edges)
+ out: Dict[str, object] = {}
+ for j in range(result.n):
+ dq = result.dq[j]
+ meas = result.tau_meas[j]
+ model = result.tau_model[j]
+ rows = {}
+ for lo, hi, label in bins:
+ idx = [i for i, v in enumerate(dq) if lo <= abs(v) < hi]
+ if len(idx) < 10:
+ continue
+ rows[f"{label}_[{lo:g},{hi:g})"] = _metrics(
+ [meas[i] for i in idx], [model[i] for i in idx])
+ if rows:
+ out[f"J{j + 1}"] = rows
+ return out
+
+
+def pose_strata(result: AnalysisResult) -> Dict[str, object]:
+ out: Dict[str, object] = {}
+ for j in range(result.n):
+ q = result.q[j]
+ meas = result.tau_meas[j]
+ model = result.tau_model[j]
+ qs = sorted(q)
+ if len(qs) < 30:
+ continue
+ q1, q2 = qs[len(qs) // 3], qs[2 * len(qs) // 3]
+ groups = [("q_low", -float("inf"), q1), ("q_mid", q1, q2),
+ ("q_high", q2, float("inf"))]
+ rows = {}
+ for label, lo, hi in groups:
+ idx = [i for i, v in enumerate(q) if lo <= v < hi]
+ if len(idx) < 10:
+ continue
+ m = _metrics([meas[i] for i in idx], [model[i] for i in idx])
+ m["q_range"] = [round(lo if lo > -1e9 else min(q), 4),
+ round(hi if hi < 1e9 else max(q), 4)]
+ rows[label] = m
+ if rows:
+ out[f"J{j + 1}"] = rows
+ return out
+
+
+# ---------------------------------------------------------------------------
+# 延迟扫描
+# ---------------------------------------------------------------------------
+
+def delay_scan(result: AnalysisResult) -> Dict[str, object]:
+ """在 ±max_delay_ms 内找使 RMSE 最小的时移。
+
+ lag > 0 表示「把实测序列往后挪 lag 个采样才最贴合模型」⇒ 实测相对模型**滞后**。
+ 这能抓住因果滤波/电流环积分导致的群延迟, 也能量化"零相位滤波没有白做"。
+ """
+ fs = result.fs
+ max_lag = max(1, int(round(result.options.max_delay_ms / 1000.0 * fs)))
+ out: Dict[str, object] = {}
+ for j in range(result.n):
+ meas = result.tau_meas[j]
+ model = result.tau_model[j]
+ n = len(meas)
+ if n < 2 * max_lag + 10:
+ continue
+ base = _rmse(meas, model)
+ best = (0, base)
+ curve = []
+ for lag in range(-max_lag, max_lag + 1):
+ if lag >= 0:
+ m, y = model[:n - lag], meas[lag:]
+ else:
+ m, y = model[-lag:], meas[:n + lag]
+ if len(m) < 10:
+ continue
+ r = _rmse(y, m)
+ curve.append([lag, round(r, 6)])
+ if r < best[1]:
+ best = (lag, r)
+ imp = (base - best[1]) / base if base > 1e-12 else 0.0
+ out[f"J{j + 1}"] = {
+ "lag_samples": best[0],
+ "lag_ms": round(best[0] / fs * 1000.0, 3),
+ "rmse_at_zero": round(base, 6),
+ "rmse_at_best": round(best[1], 6),
+ "improvement": round(imp, 6),
+ "curve": curve,
+ }
+ return out
+
+
+def _rmse(a: Sequence[float], b: Sequence[float]) -> float:
+ if not a:
+ return float("nan")
+ return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)) / len(a))
+
+
+# ---------------------------------------------------------------------------
+# 残差归因
+# ---------------------------------------------------------------------------
+
+def residual_analysis(result: AnalysisResult,
+ static_bias: Sequence[float]) -> Dict[str, object]:
+ """把 residual 与各理论项做相关, 给出可核查的归因证据 (不是判决)。"""
+ out: Dict[str, object] = {}
+ comp_names = ("gravity", "coriolis", "inertia", "jm", "friction")
+ for j in range(result.n):
+ res = [result.tau_meas[j][i] - result.tau_model[j][i]
+ for i in range(result.n_samples)]
+ dq = result.dq[j]
+ corr = {}
+ for name in comp_names:
+ c = result.components[name][j]
+ corr[name] = round(filters.pearson(res, c), 4)
+ corr["dq"] = round(filters.pearson(res, dq), 4)
+ corr["abs_dq"] = round(filters.pearson(res, [abs(v) for v in dq]), 4)
+ corr["sign_dq"] = round(filters.pearson(res, [1.0 if v > 0 else -1.0 for v in dq]), 4)
+ corr["ddq"] = round(filters.pearson(res, result.ddq[j]), 4)
+ out[f"J{j + 1}"] = {
+ "static_bias": round(static_bias[j], 6),
+ "correlation": corr,
+ }
+ return out
+
+
+def static_bias(result: AnalysisResult) -> List[float]:
+ """静止采样的平均残差 —— 电流环标定偏置 + 重力模型误差的合并证据。"""
+ out = []
+ for j in range(result.n):
+ idx = [i for i, v in enumerate(result.dq[j])
+ if abs(v) < result.options.static_dq]
+ if not idx:
+ out.append(float("nan"))
+ continue
+ out.append(filters.mean(
+ [result.tau_meas[j][i] - result.tau_model[j][i] for i in idx]))
+ return out
+
+
+# ---------------------------------------------------------------------------
+# 诊断建议
+# ---------------------------------------------------------------------------
+
+def diagnose(result: AnalysisResult, summary: Dict[str, object]) -> List[Dict[str, str]]:
+ diags: List[Dict[str, str]] = []
+ raw = summary["metrics"]["raw"]
+ agg = raw["aggregate"]
+ if agg.get("n_joints", 0) == 0:
+ return diags
+
+ rel = agg.get("mean_rel_rmse")
+ if rel is not None and rel < 0.02:
+ return [{
+ "source": "模型与实测高度一致",
+ "evidence": f"平均相对 RMSE {rel * 100:.2f}% (<2%), 残差已接近滤波/数值噪声",
+ "suggestion": "若为合成/回放数据, 说明采集-逆动力学链路正确; 若为真机数据, "
+ "精度已优于 2%, 结论限定在本组构型与速度范围内即可。",
+ }]
+
+ # 1) 滤波/时序延迟
+ worst_delay = max(
+ (v for v in summary["delay"].values() if "improvement" in v),
+ key=lambda v: v["improvement"], default=None)
+ if worst_delay is not None and worst_delay["improvement"] > 0.05 \
+ and abs(worst_delay["lag_ms"]) >= 5.0:
+ diags.append({
+ "source": "时序/滤波延迟",
+ "evidence": f"最优时移 {worst_delay['lag_ms']:+.1f} ms 时 RMSE 再降 "
+ f"{worst_delay['improvement'] * 100:.1f}% (J 最优轴)",
+ "suggestion": "先看该轴 cutoff_ddq 是否过低 (ddq 相位滞后) 或电流环报数"
+ "有固定延迟; 可把 --cutoff-ddq 提到 8~10Hz 复测, 再把残差"
+ "与延迟解耦。",
+ })
+
+ # 2) 摩擦 (residual 与 sign(dq) / friction 相关)
+ fr_corr = [(name, v["correlation"].get("friction", 0.0))
+ for name, v in summary["residual"].items()]
+ if fr_corr:
+ name, c = max(fr_corr, key=lambda kv: abs(kv[1]))
+ if abs(c) >= 0.3:
+ diags.append({
+ "source": "摩擦模型/低速换向",
+ "evidence": f"最大残差-摩擦相关 |r|={abs(c):.2f} ({name})",
+ "suggestion": "低速段与换向处最容易暴露 Stribeck 参数误差; 用 hold/"
+ "极低速 sine 段单独看 residual vs sign(dq), 必要时重新辨识 "
+ "fv/fc0/fc1 (get_ff_vec 9/10/11)。",
+ })
+
+ # 3) 重力 / 电流环零偏 (静止 bias)
+ biases = [(k, v["static_bias"]) for k, v in summary["residual"].items()
+ if isinstance(v.get("static_bias"), (int, float))
+ and v["static_bias"] == v["static_bias"]]
+ if biases:
+ name, b = max(biases, key=lambda kv: abs(kv[1]))
+ lim = max(0.1, 0.15 * max(m["rmse"] for m in raw["per_joint"].values()))
+ if abs(b) > lim:
+ diags.append({
+ "source": "重力模型/电流环零偏",
+ "evidence": f"最大静止残差 {b:+.3f} N·m ({name}); 静止时 ddq=dq=0, "
+ f"残差只能来自 G(q)、payload 或电流环标定",
+ "suggestion": "先确认台架真实重力向量 (--gravity) 与固件 get_ff_scalar(6) "
+ "一致; 再对静止段做逐轴零偏校核。若偏置随姿态变, 指向重力/payload;"
+ "若各姿态几乎恒定, 指向电流环/减速器零偏。",
+ })
+
+ # 4) 惯量 (运动段残差-惯量相关 + 延迟已排除)
+ in_corr = [(k, v["correlation"].get("inertia", 0.0))
+ for k, v in summary["residual"].items()]
+ if in_corr:
+ name, c = max(in_corr, key=lambda kv: abs(kv[1]))
+ if abs(c) >= 0.3:
+ diags.append({
+ "source": "惯量/加速度项",
+ "evidence": f"最大残差-惯量项相关 |r|={abs(c):.2f} ({name})",
+ "suggestion": "用 sine 段的 ddq 幅值与残差符号核对 jm/惯量缩放 "
+ "(--gravity 之外再看 inertia_scale); 先排除延迟 (上一条) "
+ "再怀疑惯量, 否则相位差会被误读成惯量误差。",
+ })
+
+ if not diags:
+ diags.append({
+ "source": "未见单一主导项",
+ "evidence": f"平均 RMSE {agg['mean_rmse']:.4f} N·m, 平均 |bias| "
+ f"{agg['mean_abs_bias']:.4f} N·m, 无单项相关超过阈值",
+ "suggestion": "看逐轴 metrics 与 residual 分布; 若精度已满足, 把结论限定在"
+ "本组构型/速度范围内。",
+ })
+ return diags
+
+
+# ---------------------------------------------------------------------------
+# 主分析
+# ---------------------------------------------------------------------------
+
+def analyze_session(session: Session, snapshot: ModelSnapshot,
+ options: Optional[AnalyzeOptions] = None) -> AnalysisResult:
+ opts = options or AnalyzeOptions()
+ if snapshot.joint_count != N_JOINT:
+ raise ValueError(f"模型关节数 {snapshot.joint_count} != {N_JOINT}")
+
+ session, fs, resampled = _ensure_uniform(session)
+ n = session.n
+ if n != N_JOINT:
+ raise ValueError(f"会话有 {n} 个关节, 期望 {N_JOINT}")
+ if len(session.samples) < 10:
+ raise ValueError("样本太少, 无法分析 (至少 10 帧)")
+
+ snap = copy.deepcopy(snapshot)
+ if opts.gravity_override is not None:
+ snap.gravity = tuple(float(v) for v in opts.gravity_override)
+
+ t = session.times()
+ t = [v - t[0] for v in t]
+ s0 = session.samples[0]
+ dt = 1.0 / fs
+ q_raw = [[s.q[j] for s in session.samples] for j in range(n)]
+ dq_raw = [[s.dq[j] for s in session.samples] for j in range(n)]
+ tau_raw = [[s.tau[j] for s in session.samples] for j in range(n)]
+
+ cutoff_dq = min(opts.cutoff_dq, 0.45 * fs)
+ cutoff_ddq = min(opts.cutoff_ddq, 0.45 * fs)
+ cutoff_tau = min(opts.cutoff_tau, 0.45 * fs)
+ q = [_filtered(q_raw[j], fs, cutoff_dq) for j in range(n)]
+ dq = [_filtered(dq_raw[j], fs, cutoff_dq) for j in range(n)]
+ ddq = [_filtered(filters.central_diff(dq[j], dt), fs, cutoff_ddq)
+ for j in range(n)]
+ tau_meas = [_filtered(tau_raw[j], fs, cutoff_tau) for j in range(n)]
+
+ n_s = len(t)
+ tau_model = [[0.0] * n_s for _ in range(n)]
+ tau_scaled = [[0.0] * n_s for _ in range(n)]
+ comp: Dict[str, List[List[float]]] = {
+ name: [[0.0] * n_s for _ in range(n)]
+ for name in ("gravity", "coriolis", "inertia", "jm", "friction")}
+ for i in range(n_s):
+ d = rnea.theory_torques(snap, [q[j][i] for j in range(n)],
+ [dq[j][i] for j in range(n)],
+ [ddq[j][i] for j in range(n)])
+ for name in comp:
+ for j in range(n):
+ comp[name][j][i] = d[name][j]
+ for j in range(n):
+ tau_model[j][i] = d["total"][j]
+ tau_scaled[j][i] = d["total_scaled"][j]
+
+ # 边沿裁剪: filtfilt 的反射瞬态约 2/fc 秒
+ trim = opts.trim_s
+ if trim is None:
+ trim = max(2.0 / cutoff_ddq, 0.1)
+ trim = min(trim, 0.2 * t[-1]) if t[-1] > 0 else 0.0
+ i0 = int(math.ceil(trim * fs))
+ i1 = n_s - i0
+ if i1 - i0 < 10:
+ i0, i1 = 0, n_s
+
+ def cut(seq):
+ return seq[i0:i1]
+
+ res = AnalysisResult(
+ fs=fs,
+ t=cut(t),
+ q=[cut(v) for v in q],
+ dq=[cut(v) for v in dq],
+ ddq=[cut(v) for v in ddq],
+ tau_meas=[cut(v) for v in tau_meas],
+ tau_model=[cut(v) for v in tau_model],
+ tau_scaled=[cut(v) for v in tau_scaled],
+ components={name: [cut(v) for v in vals] for name, vals in comp.items()},
+ options=opts,
+ snapshot=snap,
+ )
+
+ per_raw = {}
+ per_scaled = {}
+ for j in range(n):
+ per_raw[f"J{j + 1}"] = _metrics(res.tau_meas[j], res.tau_model[j])
+ per_scaled[f"J{j + 1}"] = _metrics(res.tau_meas[j], res.tau_scaled[j])
+
+ sbias = static_bias(res)
+ opts_dict = opts.to_dict()
+ opts_dict["trim_s"] = round(trim, 6)
+ summary: Dict[str, object] = {
+ "schema": "tau_eval.analysis.v1",
+ "scenario": session.meta.get("scenario"),
+ "fs_hz": fs,
+ "n_samples": res.n_samples,
+ "t_span_s": round(res.t[-1] - res.t[0], 6) if res.t else 0.0,
+ "resampled": resampled,
+ "options": opts_dict,
+ "model": {
+ "firmware": snap.firmware,
+ "geometry_sha16": snap.geometry_sha16,
+ "gravity": list(snap.gravity),
+ "payload": {"mass": snap.payload_mass, "com": list(snap.payload_com)},
+ "friction_model": snap.friction_model,
+ "read_warnings": snap.read_warnings,
+ },
+ "metrics": {
+ "raw": {"per_joint": per_raw, "aggregate": _agg_metrics(per_raw)},
+ "scaled": {"per_joint": per_scaled, "aggregate": _agg_metrics(per_scaled)},
+ },
+ "strata": {"speed": speed_strata(res), "pose": pose_strata(res)},
+ "delay": delay_scan(res),
+ "static_bias": [None if v != v else round(v, 6) for v in sbias],
+ "residual": residual_analysis(res, sbias),
+ "components_rms": {
+ name: [round(math.sqrt(sum(v * v for v in vals[j]) / max(len(vals[j]), 1)), 6)
+ for j in range(n)]
+ for name, vals in res.components.items()},
+ "notes": [
+ "residual = τ_meas − τ_model, 关节侧口径, 单位 N·m。",
+ "raw 为物理量; scaled 按固件 gravity_scale/inertia_scale 口径。",
+ "speed 分层基于滤波后的 |dq|; pose 分层基于 q 的 1/3、2/3 分位。",
+ "delay>0 = 实测相对模型滞后; improvement 是时移后可再降的 RMSE 比例。",
+ ],
+ }
+ summary["diagnosis"] = diagnose(res, summary)
+ res.summary = summary
+ return res
+
+
+# ---------------------------------------------------------------------------
+# 读写
+# ---------------------------------------------------------------------------
+
+def write_timeseries(result: AnalysisResult, path: str) -> None:
+ n = result.n
+ cols = ["t"]
+ for j in range(n):
+ cols += [f"q{j}", f"dq{j}", f"ddq{j}", f"tau_meas{j}",
+ f"tau_model{j}", f"tau_scaled{j}",
+ f"G{j}", f"C{j}", f"I{j}", f"F{j}"]
+ with open(path, "w", newline="", encoding="utf-8") as f:
+ w = csv.writer(f)
+ w.writerow(cols)
+ for i in range(result.n_samples):
+ row = [f"{result.t[i]:.9f}"]
+ for j in range(n):
+ row += [
+ f"{result.q[j][i]:.9g}", f"{result.dq[j][i]:.9g}",
+ f"{result.ddq[j][i]:.9g}", f"{result.tau_meas[j][i]:.9g}",
+ f"{result.tau_model[j][i]:.9g}", f"{result.tau_scaled[j][i]:.9g}",
+ f"{result.components['gravity'][j][i]:.9g}",
+ f"{result.components['coriolis'][j][i]:.9g}",
+ f"{result.components['inertia'][j][i]:.9g}",
+ f"{result.components['friction'][j][i]:.9g}",
+ ]
+ w.writerow(row)
+
+
+def clean_json(obj):
+ """递归把 NaN/±inf 换成 None —— `json.dump` 默认会写出非标准 NaN。"""
+ if isinstance(obj, dict):
+ return {k: clean_json(v) for k, v in obj.items()}
+ if isinstance(obj, (list, tuple)):
+ return [clean_json(v) for v in obj]
+ if isinstance(obj, float) and not math.isfinite(obj):
+ return None
+ return obj
+
+
+def save_analysis(result: AnalysisResult, outdir: str) -> Dict[str, str]:
+ os.makedirs(outdir, exist_ok=True)
+ ts_path = os.path.join(outdir, "timeseries.csv")
+ metrics_path = os.path.join(outdir, "metrics.json")
+ write_timeseries(result, ts_path)
+ with open(metrics_path, "w", encoding="utf-8") as f:
+ json.dump(clean_json(result.summary), f, ensure_ascii=False, indent=2)
+ return {"timeseries_csv": ts_path, "metrics_json": metrics_path}
+
+
+def _load_snapshot(indir: str) -> ModelSnapshot:
+ path = os.path.join(indir, "model.json")
+ with open(path, encoding="utf-8") as f:
+ return ModelSnapshot.from_dict(json.load(f))
+
+
+def analyze_dir(indir: str, outdir: Optional[str] = None,
+ options: Optional[AnalyzeOptions] = None) -> AnalysisResult:
+ """从一个采集目录 (`status.csv` + `model.json`) 分析, 结果写回同目录。"""
+ session = Session.load(indir)
+ snapshot = _load_snapshot(indir)
+ result = analyze_session(session, snapshot, options)
+ save_analysis(result, outdir or indir)
+ return result
diff --git a/tools/tau_eval/cli.py b/tools/tau_eval/cli.py
new file mode 100644
index 0000000..a3ec87a
--- /dev/null
+++ b/tools/tau_eval/cli.py
@@ -0,0 +1,432 @@
+# -*- coding: utf-8 -*-
+"""命令行入口: 一条命令完成 采集 → 逆动力学 → 图 → 报告。
+
+子命令:
+ run 默认。连接真机, 跑 hold/sine/poses 三个场景, 逐个分析出图出报告。
+ collect 只采集一个场景 (仍会分析, 除非 --no-analyze)。
+ analyze 对已有会话目录 (或包含多个会话的 run 目录) 做离线分析。
+
+安全约定与 examples/ 一致: 默认只读打印计划; 任何 enable/运动/采集都要 `--go`。
+采集结束不 disable (该位形失能会因自重坠回), 急停请放手边。
+"""
+from __future__ import annotations
+
+import argparse
+import datetime as _dt
+import os
+import shlex
+import sys
+from typing import List, Optional, Sequence, Tuple
+
+from .analyze import AnalyzeOptions, analyze_dir
+from .collect import (MIN_MARGIN, ScenarioSpec, fetch_from_arm,
+ read_joint_limits, run_scenario)
+from .plots import generate_all
+from .report import write_report
+
+#: 默认三场景的构型偏移模板 (rad)。乘缩放系数后仍要过软限位余量检查。
+_POSE_MAGS = (0.10, 0.10, 0.10, 0.08, 0.10, 0.10, 0.10)
+_POSE_SIGNS = (
+ (1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0),
+ (-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0),
+)
+
+
+def _gravity(s: str) -> Tuple[float, float, float]:
+ parts = [p for p in s.replace(" ", "").split(",") if p]
+ if len(parts) != 3:
+ raise argparse.ArgumentTypeError("重力向量需要 3 个分量, 如 0,9.81,0")
+ return tuple(float(v) for v in parts) # type: ignore[return-value]
+
+
+def _joints(s: str) -> List[int]:
+ out = []
+ for tok in s.replace(" ", "").split(","):
+ if tok:
+ j = int(tok) - 1
+ if j < 0:
+ raise argparse.ArgumentTypeError("关节号从 1 开始")
+ out.append(j)
+ return out
+
+
+def _common_collect_args(ap: argparse.ArgumentParser) -> None:
+ ap.add_argument("--port", default=None, help="串口 (默认自动发现 1d50:606f)")
+ ap.add_argument("--go", action="store_true",
+ help="真正 enable/运动/采集 (默认只读, 只打印计划)")
+ ap.add_argument("--outdir", default=None,
+ help="输出根目录 (默认 runs/<时间戳>_tau_eval)")
+ ap.add_argument("--fs", type=float, default=100.0, help="落盘采样率, 1..100Hz")
+ ap.add_argument("--scenario", default="all",
+ choices=["all", "hold", "sine", "poses"],
+ help="场景: all=hold+sine+poses")
+ ap.add_argument("--hold-s", type=float, default=3.0, help="静态持位时长 (s)")
+ ap.add_argument("--sine-s", type=float, default=10.0, help="正弦段时长 (s)")
+ ap.add_argument("--amp", type=float, default=0.12, help="正弦幅值 (rad), 受软限位自动收窄")
+ ap.add_argument("--freq", type=float, default=0.20, help="正弦频率 (Hz)")
+ ap.add_argument("--command-hz", type=float, default=50.0,
+ help="连续伺服 (move_js) 重发率, ≥10Hz 才不断看门狗")
+ ap.add_argument("--sine-joints", type=_joints, default=None,
+ help="正弦激励的关节, 逗号分隔 1 基 (默认全部)")
+ ap.add_argument("--dwell-s", type=float, default=1.2, help="poses 每姿态停驻 (s)")
+ ap.add_argument("--speed", type=float, default=0.2, help="poses 的 movej speed (0..1)")
+ ap.add_argument("--ambient-c", type=float, default=None,
+ help="手工记录的室温 (°C); 固件只报绕组/MOS 温度")
+ ap.add_argument("--parquet", action="store_true",
+ help="同时写 Parquet (需要 pyarrow, 否则自动跳过)")
+ ap.add_argument("--gravity", type=_gravity, default=None,
+ help="分析用的重力向量, 如台架为 +Y 时写 0,9.81,0")
+ ap.add_argument("--cutoff-dq", type=float, default=15.0, help="q/dq 零相位低通 (Hz)")
+ ap.add_argument("--cutoff-ddq", type=float, default=5.0, help="ddq 零相位低通 (Hz)")
+ ap.add_argument("--cutoff-tau", type=float, default=10.0, help="实测 τ 低通 (Hz)")
+ ap.add_argument("--trim-s", type=float, default=None,
+ help="两端裁剪 (s), 默认 max(2/cutoff_ddq, 0.1)")
+ ap.add_argument("--max-delay-ms", type=float, default=100.0,
+ help="延迟扫描范围 ±ms")
+ ap.add_argument("--no-plot", action="store_true", help="只算指标, 不生成 SVG")
+ ap.add_argument("--no-report", action="store_true", help="不写 report.md")
+
+
+def build_parser() -> argparse.ArgumentParser:
+ ap = argparse.ArgumentParser(
+ prog="tau_eval",
+ description="LiteArm 电流环力矩估计精度评估: 采集 → 逆动力学 → 误差图/报告")
+ sub = ap.add_subparsers(dest="cmd")
+
+ p_run = sub.add_parser("run", help="一条命令跑三场景并出图出报告 (默认)")
+ _common_collect_args(p_run)
+
+ p_col = sub.add_parser("collect", help="只采集 (默认仍分析)")
+ _common_collect_args(p_col)
+ p_col.add_argument("--no-analyze", action="store_true", help="只落盘, 不分析")
+
+ p_an = sub.add_parser("analyze", help="分析已有会话/run 目录")
+ p_an.add_argument("path", help="会话目录 (含 status.csv) 或 run 目录")
+ p_an.add_argument("--outdir", default=None, help="分析输出目录 (默认写回原目录)")
+ p_an.add_argument("--gravity", type=_gravity, default=None,
+ help="重力向量, 如台架为 +Y 时写 0,9.81,0")
+ p_an.add_argument("--cutoff-dq", type=float, default=15.0)
+ p_an.add_argument("--cutoff-ddq", type=float, default=5.0)
+ p_an.add_argument("--cutoff-tau", type=float, default=10.0)
+ p_an.add_argument("--trim-s", type=float, default=None)
+ p_an.add_argument("--max-delay-ms", type=float, default=100.0)
+ p_an.add_argument("--no-plot", action="store_true")
+ p_an.add_argument("--no-report", action="store_true")
+ return ap
+
+
+def _analyze_options(args) -> AnalyzeOptions:
+ return AnalyzeOptions(
+ cutoff_dq=args.cutoff_dq, cutoff_ddq=args.cutoff_ddq,
+ cutoff_tau=args.cutoff_tau, trim_s=args.trim_s,
+ gravity_override=args.gravity, max_delay_ms=args.max_delay_ms)
+
+
+# ---------------------------------------------------------------------------
+# 场景规划
+# ---------------------------------------------------------------------------
+
+def _adaptive_amp(center: Sequence[float], limits: Sequence[Tuple[float, float]],
+ want: float, joints: Sequence[int],
+ margin: float = MIN_MARGIN) -> float:
+ """所有选中关节都留出 margin 时, 同相正弦的最大公共幅值。"""
+ caps = []
+ for j in joints:
+ lo, hi = limits[j]
+ caps.append(min(center[j] - lo, hi - center[j]) - margin)
+ return min([want] + caps)
+
+
+def _build_poses(start: Sequence[float], limits: Sequence[Tuple[float, float]],
+ margin: float = MIN_MARGIN) -> Tuple[List[List[float]], float]:
+ """生成 "起点 -> 正偏移 -> 负偏移 -> 起点" 的慢速序列, 自动收窄幅度。"""
+ for scale in (1.0, 0.7, 0.5, 0.35, 0.25):
+ poses = []
+ for signs in _POSE_SIGNS:
+ pose = [start[j] + scale * signs[j] * _POSE_MAGS[j]
+ for j in range(len(start))]
+ poses.append(pose)
+ poses.append(list(start))
+ ok = True
+ for p in poses:
+ for j, q in enumerate(p):
+ lo, hi = limits[j]
+ if q - lo < margin or hi - q < margin:
+ ok = False
+ break
+ if not ok:
+ break
+ if ok:
+ return poses, scale
+ raise RuntimeError(
+ "当前位形离软限位太近, 生成不出安全的多姿态序列; 先把臂挪到更居中的位形")
+
+
+def _make_specs(arm, args, limits) -> List[ScenarioSpec]:
+ st = arm.get_state().value
+ if st is None:
+ raise RuntimeError("拿不到状态帧")
+ q0 = list(st.q)
+ n = len(q0)
+ joints = args.sine_joints if args.sine_joints is not None else list(range(n))
+ for j in joints:
+ if not 0 <= j < n:
+ raise SystemExit(f"⛔ --sine-joints 里的 {j + 1} 越界 (1..{n})")
+
+ kinds = ["hold", "sine", "poses"] if args.scenario == "all" else [args.scenario]
+ specs: List[ScenarioSpec] = []
+ for kind in kinds:
+ if kind == "hold":
+ specs.append(ScenarioSpec(kind="hold", name="hold", q_center=list(q0),
+ duration=args.hold_s))
+ elif kind == "sine":
+ if not (10.0 <= args.command_hz <= 200.0):
+ raise SystemExit("⛔ --command-hz 必须在 [10, 200]")
+ amp = _adaptive_amp(q0, limits, args.amp, joints)
+ if amp < 0.03:
+ raise SystemExit(
+ f"⛔ 当前位形只剩 {amp:.3f} rad 正弦空间 (<0.03), 先换一个更居中的位形")
+ if amp < args.amp - 1e-9:
+ print(f"[规划] 正弦幅值受软限位限制: {args.amp:.3f} -> {amp:.3f} rad")
+ specs.append(ScenarioSpec(
+ kind="sine", name="sine", q_center=list(q0), duration=args.sine_s,
+ amp=amp, freq=args.freq, joints=list(joints),
+ command_hz=args.command_hz))
+ elif kind == "poses":
+ poses, scale = _build_poses(q0, limits)
+ if scale < 1.0:
+ print(f"[规划] 多姿态幅度收窄到 {scale:.2f}× 以留出限位余量")
+ specs.append(ScenarioSpec(
+ kind="poses", name="poses", q_center=list(q0), poses=poses,
+ dwell=args.dwell_s, speed=args.speed))
+ return specs
+
+
+def _describe(spec: ScenarioSpec) -> str:
+ if spec.kind == "hold":
+ return f"hold: 静止持位 {spec.duration:.1f}s"
+ if spec.kind == "sine":
+ js = spec.joints if spec.joints is not None else []
+ return (f"sine: {spec.duration:.1f}s, {spec.amp:.3f} rad @ {spec.freq:.2f}Hz, "
+ f"关节 {[j + 1 for j in js]}, 伺服 {spec.command_hz:.0f}Hz")
+ return (f"poses: {len(spec.poses or [])} 个位形, dwell {spec.dwell:.1f}s, "
+ f"speed {spec.speed:.2f}")
+
+
+def _print_plan(arm, specs: Sequence[ScenarioSpec]) -> None:
+ st = arm.get_state().value
+ print(f"[连接] 固件 {getattr(arm, 'firmware', '?')} (n={arm.n}); "
+ f"mode={st.mode_name} enabled={st.enabled} flags=0x{st.flags:X}")
+ print(f"[位形] q = {[round(v, 4) for v in st.q]}")
+ print("[计划]")
+ for k, spec in enumerate(specs, 1):
+ print(f" {k}) {_describe(spec)}")
+ print("[安全] 目标均过软限位余量检查; 结束后不 disable (失能会因自重坠回)")
+
+
+# ---------------------------------------------------------------------------
+# 输出与分析
+# ---------------------------------------------------------------------------
+
+def _timestamp_dir(root: Optional[str]) -> str:
+ if root is None:
+ root = os.path.join("runs", _dt.datetime.now().strftime("%Y%m%d_%H%M%S")
+ + "_tau_eval")
+ if os.path.exists(os.path.join(root, "status.csv")) or \
+ (os.path.isdir(root) and any(
+ os.path.exists(os.path.join(root, d, "status.csv"))
+ for d in os.listdir(root) if os.path.isdir(os.path.join(root, d)))):
+ root = root + "_" + _dt.datetime.now().strftime("%H%M%S")
+ os.makedirs(root, exist_ok=True)
+ return root
+
+
+def _analyze_one(indir: str, outdir: str, opts: AnalyzeOptions, command: str,
+ no_plot: bool = False, no_report: bool = False):
+ os.makedirs(outdir, exist_ok=True)
+ result = analyze_dir(indir, outdir, opts)
+ figs = {}
+ if not no_plot:
+ figs = generate_all(result, os.path.join(outdir, "figures"))
+ if not no_report:
+ write_report(result, figs, outdir, command=command)
+ return result
+
+
+def _print_summary(result) -> None:
+ s = result.summary
+ agg = s["metrics"]["raw"]["aggregate"]
+ print(f" [{s.get('scenario') or 'session'}] 平均 RMSE {agg['mean_rmse']:.4f} N·m, "
+ f"最大 {agg['max_rmse']:.4f} N·m ({agg['worst_joint']}), "
+ f"平均 |bias| {agg['mean_abs_bias']:.4f} N·m")
+ for d in s.get("diagnosis", []):
+ print(f" → {d['source']}: {d['evidence']}")
+
+
+def _write_overview(run_dir: str, results: Sequence) -> str:
+ lines = [f"# tau_eval 运行总览 — {os.path.basename(run_dir)}", ""]
+ lines.append("| 场景 | 样本 | 平均 RMSE (N·m) | 最大 RMSE | 最差轴 | 平均 \\|bias\\| | 报告 |")
+ lines.append("|---|---|---|---|---|---|---|")
+ for r in results:
+ s = r.summary
+ agg = s["metrics"]["raw"]["aggregate"]
+ sub = s.get("scenario") or ""
+ lines.append(
+ f"| {sub} | {s['n_samples']} | {agg['mean_rmse']:.4f} | "
+ f"{agg['max_rmse']:.4f} | {agg['worst_joint']} | "
+ f"{agg['mean_abs_bias']:.4f} | [report]({sub}/report.md) |")
+ lines.append("")
+ lines.append("图与逐轴指标在各场景子目录: `metrics.json` / `timeseries.csv` / "
+ "`figures/*.svg`。")
+ path = os.path.join(run_dir, "summary.md")
+ with open(path, "w", encoding="utf-8") as f:
+ f.write("\n".join(lines) + "\n")
+ return path
+
+
+# ---------------------------------------------------------------------------
+# 命令
+# ---------------------------------------------------------------------------
+
+def _connect(args):
+ import litearm as pa
+ return pa.Arm(port=args.port or os.environ.get("LITEARM_PORT") or None).connect()
+
+
+def _command_line(argv: Sequence[str]) -> str:
+ return "python3 tools/tau_eval/run.py " + " ".join(shlex.quote(a) for a in argv)
+
+
+def cmd_run(args, argv: Sequence[str]) -> int:
+ arm = _connect(args)
+ try:
+ st = arm.get_state().value
+ if st is not None and st.faulted:
+ print(f"⛔ 臂有故障位, 拒绝运行: {st.fault_detail}")
+ return 2
+ limits = read_joint_limits(arm)
+ specs = _make_specs(arm, args, limits)
+ _print_plan(arm, specs)
+ if not args.go:
+ print("\n只读模式: 加 --go 才会 enable/运动/采集。")
+ return 0
+
+ root = _timestamp_dir(args.outdir)
+ print(f"[输出] {root}")
+ snapshot = fetch_from_arm(arm)
+ session_dirs = []
+ for k, spec in enumerate(specs, 1):
+ sub = f"{k:02d}_{spec.kind}"
+ d = os.path.join(root, sub)
+ print(f"\n[采集 {k}/{len(specs)}] {_describe(spec)}")
+ run_scenario(arm, spec, d, fs=args.fs, go=True,
+ ambient_c=args.ambient_c, snapshot=snapshot,
+ parquet=args.parquet)
+ session_dirs.append((spec, d))
+ finally:
+ arm.close()
+ print("\n[断开] 已关闭串口 (刻意未 disable: 失能会因自重坠回)")
+
+ opts = _analyze_options(args)
+ cmd = _command_line(argv)
+ results = []
+ print("\n[分析]")
+ for spec, d in session_dirs:
+ results.append(_analyze_one(d, d, opts, cmd, args.no_plot, args.no_report))
+ _print_summary(results[-1])
+ if results:
+ ov = _write_overview(root, results)
+ print(f"\n[完成] 总览 {ov}")
+ return 0
+
+
+def cmd_collect(args, argv: Sequence[str]) -> int:
+ arm = _connect(args)
+ session_dir = None
+ try:
+ st = arm.get_state().value
+ if st is not None and st.faulted:
+ print(f"⛔ 臂有故障位, 拒绝运行: {st.fault_detail}")
+ return 2
+ limits = read_joint_limits(arm)
+ specs = _make_specs(arm, args, limits)
+ _print_plan(arm, specs)
+ if not args.go:
+ print("\n只读模式: 加 --go 才会 enable/运动/采集。")
+ return 0
+ session_dir = _timestamp_dir(args.outdir)
+ print(f"[输出] {session_dir}")
+ snapshot = fetch_from_arm(arm)
+ for k, spec in enumerate(specs, 1):
+ run_scenario(arm, spec, session_dir, fs=args.fs, go=True,
+ ambient_c=args.ambient_c, snapshot=snapshot,
+ parquet=args.parquet)
+ finally:
+ arm.close()
+ print("[断开] 已关闭串口 (刻意未 disable: 失能会因自重坠回)")
+
+ if session_dir and not args.no_analyze:
+ result = _analyze_one(session_dir, session_dir, _analyze_options(args),
+ _command_line(argv), args.no_plot, args.no_report)
+ _print_summary(result)
+ return 0
+
+
+def _session_dirs(path: str) -> List[str]:
+ if os.path.isfile(os.path.join(path, "status.csv")):
+ return [path]
+ out = []
+ if os.path.isdir(path):
+ for d in sorted(os.listdir(path)):
+ p = os.path.join(path, d)
+ if os.path.isfile(os.path.join(p, "status.csv")):
+ out.append(p)
+ return out
+
+
+def cmd_analyze(args, argv: Sequence[str]) -> int:
+ dirs = _session_dirs(args.path)
+ if not dirs:
+ print(f"⛔ {args.path} 下没有找到 status.csv")
+ return 2
+ opts = _analyze_options(args)
+ cmd = _command_line(argv)
+ results = []
+ for d in dirs:
+ out = os.path.join(args.outdir, os.path.basename(d)) if args.outdir else d
+ result = _analyze_one(d, out, opts, cmd, args.no_plot, args.no_report)
+ results.append(result)
+ _print_summary(result)
+ if len(results) > 1 and args.outdir is None:
+ _write_overview(os.path.dirname(dirs[0]), results)
+ return 0
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+ argv = list(sys.argv[1:] if argv is None else argv)
+ if not argv or (argv[0].startswith("-") and argv[0] not in ("-h", "--help")):
+ argv = ["run"] + argv
+ ap = build_parser()
+ args = ap.parse_args(argv)
+ if args.cmd is None:
+ ap.print_help()
+ return 0
+ try:
+ if args.cmd == "run":
+ return cmd_run(args, argv)
+ if args.cmd == "collect":
+ return cmd_collect(args, argv)
+ if args.cmd == "analyze":
+ return cmd_analyze(args, argv)
+ except KeyboardInterrupt:
+ print("\n[中断] 用户 Ctrl-C; 串口已由 finally 关闭。")
+ return 130
+ except Exception as e: # noqa: BLE001 - 顶层收口
+ print(f"⛔ {type(e).__name__}: {e}")
+ return 2
+ ap.print_help()
+ return 2
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/tools/tau_eval/collect.py b/tools/tau_eval/collect.py
new file mode 100644
index 0000000..a833b6f
--- /dev/null
+++ b/tools/tau_eval/collect.py
@@ -0,0 +1,778 @@
+# -*- coding: utf-8 -*-
+"""状态流采集 + 激励场景 —— 只读采集与安全性检查都在这里。
+
+数据路径 (与 issue 的「采集」段一一对应):
+
+ 固件 100Hz RSP_STATUS ──> _Ack._on_status 探头 ──> Session (每帧一条)
+ │
+ ├─ status.csv (可配置 fs 的均匀网格)
+ ├─ meta.json (固件/模型/温度/丢帧账)
+ └─ model.json (ModelSnapshot)
+
+探头挂在 SDK 状态帧的**唯一**漏斗 `_Ack._on_status` 上: 不另开读口、不抢字节流。
+采集期间先按到达时刻存**全部**帧, 落盘时再按 `--fs` 重采样到均匀网格
+(`filters.linear_resample`): 配置 100Hz 时就是原始流; 配置 50Hz 时是 50Hz 的
+均匀重采样, 而不是"碰巧每两帧取一帧" —— 后者在 100Hz 抖动的到达间隔下会变成
+30/10ms 交替的假节拍, 差分求 ddq 时直接编出噪声。
+
+激励场景 (issue 要求至少三类):
+ hold 静态持位 —— 纯重力项, 校核 G(q)
+ sine 低速正弦 —— 连续 q/dq 参考 (move_js ≥10Hz 重发), 激励惯性/摩擦
+ poses 多姿态慢速 movej —— 大范围姿态变化, 激励重力随构型的变化
+
+安全:
+ * 所有目标位形都按固件软限位检查, 余量不足 `MIN_MARGIN` 直接拒绝;
+ * 默认只读 (打印计划); 只有 `go=True` 才 enable / 运动 / 采集;
+ * 采集结束**不 disable** —— 该位形失能会因自重坠回 (见 examples/07 的同一条策略)。
+"""
+from __future__ import annotations
+
+import bisect
+import csv
+import datetime as _dt
+import json
+import math
+import os
+import platform
+import socket
+import time
+from dataclasses import asdict, dataclass
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from . import filters
+from .model import ModelSnapshot, fetch_from_arm
+
+#: 目标位形到软限位的最小余量 (rad)。低于它直接拒绝 —— 位置越限锁存不受使能门控,
+#: 也不是 reset 能清的那类故障 (与 examples/07 的 MIN_MARGIN 同值同理由)。
+MIN_MARGIN = 0.15
+
+#: 正弦幅值/频率的本地护栏 (不是固件限制, 是"这是精度评估不是耐久试验")。
+MAX_SINE_AMP = 0.35
+MAX_SINE_FREQ = 1.0
+
+#: 连续伺服重发率下限。固件 0.1s 命令看门狗 ⇒ 低于 10Hz 会 fail-soft 丢前馈。
+MIN_COMMAND_HZ = 10.0
+MAX_COMMAND_HZ = 200.0
+
+#: 静态重力核查的尾部窗口与"算静止"的速度门限。
+GRAVITY_TAIL_S = 0.5
+STATIC_DQ_MAX = 0.05
+
+#: poses 场景段间参考速度 (rad/s, 再乘 `--speed`)。用 move_js 流式走平滑轨迹,
+#: 不依赖 movej 到位判据 —— 真机上 movej 的静态误差会超过 SDK 的 q_tol。
+POSE_SPEED_RAD_S = 0.5
+
+
+# ---------------------------------------------------------------------------
+# 会话数据
+# ---------------------------------------------------------------------------
+
+@dataclass
+class Sample:
+ """一帧状态 (时刻为相对会话起点的秒)。"""
+
+ t: float
+ seq: int
+ mode: int
+ mode_name: str
+ flags: int
+ joint_fault: int
+ q: List[float]
+ dq: List[float]
+ tau: List[float]
+ t_mos: List[float]
+ t_coil: List[float]
+ err: List[int]
+
+
+class Session:
+ """一段采集: 原始采样 + 元数据。
+
+ `samples` 的 `t` 按到达顺序递增, 但间隔不保证均匀 —— `resample(fs)` 才把它变成
+ 均匀网格。分析层只吃 `resample()` 的结果。
+ """
+
+ def __init__(self, samples: List[Sample], meta: Optional[Dict] = None):
+ self.samples = list(samples)
+ self.meta = dict(meta or {})
+
+ # ---- 基本属性 ----
+ @property
+ def n(self) -> int:
+ return len(self.samples[0].q) if self.samples else 0
+
+ @property
+ def span(self) -> float:
+ return self.samples[-1].t - self.samples[0].t if len(self.samples) > 1 else 0.0
+
+ @property
+ def raw_rate_hz(self) -> float:
+ return (len(self.samples) - 1) / self.span if self.span > 0 else 0.0
+
+ def column(self, name: str, j: int) -> List[float]:
+ return [getattr(s, name)[j] for s in self.samples]
+
+ def times(self) -> List[float]:
+ return [s.t for s in self.samples]
+
+ # ---- 均匀重采样 ----
+ def resample(self, fs: float) -> "Session":
+ """把不均匀到达序列重采样到 `[t0, t1]` 上的 1/fs 网格。"""
+ if not self.samples:
+ return Session([], dict(self.meta, fs=fs))
+ t = self.times()
+ grid = filters.uniform_grid(t[0], t[-1], fs)
+ if len(grid) < 2:
+ return Session(list(self.samples), dict(self.meta, fs=fs))
+
+ cont: Dict[str, List[List[float]]] = {}
+ for name in ("q", "dq", "tau", "t_mos", "t_coil"):
+ cont[name] = [
+ filters.linear_resample(t, self.column(name, j), grid)
+ for j in range(self.n)
+ ]
+ flat_err = [
+ filters.linear_resample(t, [float(s.err[j]) for s in self.samples], grid)
+ for j in range(self.n)
+ ]
+ # 离散字段取"网格时刻之前最近一帧", 不做插值。
+ disc = []
+ for g in grid:
+ k = max(0, bisect.bisect_right(t, g) - 1)
+ disc.append(self.samples[k])
+
+ out = []
+ for k, g in enumerate(grid):
+ src = disc[k]
+ out.append(Sample(
+ t=round(g, 9), seq=src.seq, mode=src.mode,
+ mode_name=src.mode_name, flags=src.flags,
+ joint_fault=src.joint_fault,
+ q=[cont["q"][j][k] for j in range(self.n)],
+ dq=[cont["dq"][j][k] for j in range(self.n)],
+ tau=[cont["tau"][j][k] for j in range(self.n)],
+ t_mos=[cont["t_mos"][j][k] for j in range(self.n)],
+ t_coil=[cont["t_coil"][j][k] for j in range(self.n)],
+ err=[int(round(flat_err[j][k])) for j in range(self.n)],
+ ))
+ return Session(out, dict(self.meta, fs=fs))
+
+ # ---- 落盘 ----
+ def write_csv(self, path: str) -> None:
+ n = self.n
+ cols = (["t", "seq", "mode", "mode_name", "flags", "joint_fault"]
+ + [f"q{i}" for i in range(n)]
+ + [f"dq{i}" for i in range(n)]
+ + [f"tau{i}" for i in range(n)]
+ + [f"tmos{i}" for i in range(n)]
+ + [f"tcoil{i}" for i in range(n)]
+ + [f"err{i}" for i in range(n)])
+ with open(path, "w", newline="", encoding="utf-8") as f:
+ w = csv.writer(f)
+ w.writerow(cols)
+ for s in self.samples:
+ w.writerow([f"{s.t:.9f}", s.seq, s.mode, s.mode_name,
+ s.flags, s.joint_fault]
+ + [f"{v:.12g}" for v in s.q]
+ + [f"{v:.12g}" for v in s.dq]
+ + [f"{v:.12g}" for v in s.tau]
+ + [f"{v:.12g}" for v in s.t_mos]
+ + [f"{v:.12g}" for v in s.t_coil]
+ + list(s.err))
+
+ @classmethod
+ def read_csv(cls, path: str, meta: Optional[Dict] = None) -> "Session":
+ with open(path, newline="", encoding="utf-8") as f:
+ rows = list(csv.DictReader(f))
+ if not rows:
+ return cls([], dict(meta or {}))
+ n = 0
+ while f"q{n}" in rows[0]:
+ n += 1
+
+ def _f(v: str) -> float:
+ return float(v)
+
+ samples = []
+ for r in rows:
+ samples.append(Sample(
+ t=_f(r["t"]), seq=int(r["seq"]), mode=int(r["mode"]),
+ mode_name=r.get("mode_name", ""), flags=int(r["flags"]),
+ joint_fault=int(r["joint_fault"]),
+ q=[_f(r[f"q{j}"]) for j in range(n)],
+ dq=[_f(r[f"dq{j}"]) for j in range(n)],
+ tau=[_f(r[f"tau{j}"]) for j in range(n)],
+ t_mos=[_f(r[f"tmos{j}"]) for j in range(n)],
+ t_coil=[_f(r[f"tcoil{j}"]) for j in range(n)],
+ err=[int(float(r[f"err{j}"])) for j in range(n)],
+ ))
+ return cls(samples, dict(meta or {}))
+
+ def dump(self, outdir: str, extra_meta: Optional[Dict] = None) -> Dict[str, str]:
+ os.makedirs(outdir, exist_ok=True)
+ csv_path = os.path.join(outdir, "status.csv")
+ meta_path = os.path.join(outdir, "meta.json")
+ self.write_csv(csv_path)
+ meta = dict(self.meta)
+ meta.update(extra_meta or {})
+ meta.update({
+ "sample_count": len(self.samples),
+ "t_span_s": round(self.span, 6),
+ "fs": meta.get("fs"),
+ })
+ with open(meta_path, "w", encoding="utf-8") as f:
+ json.dump(meta, f, ensure_ascii=False, indent=2)
+ return {"status_csv": csv_path, "meta_json": meta_path}
+
+ @classmethod
+ def load(cls, outdir: str) -> "Session":
+ meta_path = os.path.join(outdir, "meta.json")
+ meta = {}
+ if os.path.exists(meta_path):
+ with open(meta_path, encoding="utf-8") as f:
+ meta = json.load(f)
+ return cls.read_csv(os.path.join(outdir, "status.csv"), meta)
+
+
+# ---------------------------------------------------------------------------
+# 状态流探头
+# ---------------------------------------------------------------------------
+
+class StatusRecorder:
+ """挂在 `_Ack._on_status` 上的被动探头。
+
+ 只记录, 不判定、不发命令。`arm._a` 是 `_Ack` 实例; 把它替换掉的那个函数在
+ 退出时恢复 (`stop()`), 因此可以安全嵌套于 SDK 自己的到位判定。
+ """
+
+ def __init__(self, arm, clock=time.monotonic):
+ self._arm = arm
+ self._clock = clock
+ self._records: List[Tuple[float, object]] = []
+ self._orig = None
+ self._ack = None
+ self.started_at: Optional[float] = None
+ self.finished_at: Optional[float] = None
+ self.decode_errors = 0
+
+ def start(self) -> "StatusRecorder":
+ from litearm.state import decode_state
+
+ ack = self._arm._a
+ orig = ack._on_status
+ self._ack = ack
+ self._orig = orig
+ self.started_at = self._clock()
+
+ def spy(payload: bytes) -> None:
+ now = self._clock()
+ try:
+ st = decode_state(payload)
+ except ValueError:
+ self.decode_errors += 1
+ return
+ self._records.append((now, st))
+ orig(payload) # 让 SDK 照常更新 state/status_seq
+
+ ack._on_status = spy
+ return self
+
+ def stop(self) -> None:
+ if self._ack is not None and self._orig is not None:
+ self._ack._on_status = self._orig
+ self._ack = None
+ self._orig = None
+ self.finished_at = self._clock()
+
+ def __enter__(self) -> "StatusRecorder":
+ return self.start()
+
+ def __exit__(self, exc_type, exc, tb) -> bool:
+ self.stop()
+ return False
+
+ # ---- 转成 Session ----
+ def to_session(self, meta: Optional[Dict] = None) -> Session:
+ if not self._records:
+ return Session([], dict(meta or {}))
+ t0 = self._records[0][0]
+ samples = []
+ for ts, st in self._records:
+ samples.append(Sample(
+ t=ts - t0, seq=st.seq, mode=st.mode,
+ mode_name=st.mode_name, flags=st.flags,
+ joint_fault=st.joint_fault,
+ q=list(st.q), dq=list(st.dq), tau=list(st.tau),
+ t_mos=[j.t_mos for j in st.joints],
+ t_coil=[j.t_coil for j in st.joints],
+ err=[j.err for j in st.joints],
+ ))
+ m = dict(meta or {})
+ m.setdefault("raw_frames", len(samples))
+ m.setdefault("raw_rate_hz", round(
+ (len(samples) - 1) / max(samples[-1].t, 1e-9), 2))
+ m.setdefault("decode_errors", self.decode_errors)
+ return Session(samples, m)
+
+
+def _seq_gaps(samples: Sequence[Sample]) -> Tuple[int, int]:
+ """返回 (seq 缺失帧数, 最大连续缺失帧数)。"""
+ missing = 0
+ max_gap = 0
+ for a, b in zip(samples, samples[1:]):
+ d = b.seq - a.seq
+ if 0 < d < 100000: # 0/负 = 回绕或重复, 不计
+ missing += d - 1
+ max_gap = max(max_gap, d - 1)
+ return missing, max_gap
+
+
+# ---------------------------------------------------------------------------
+# 场景
+# ---------------------------------------------------------------------------
+
+@dataclass
+class ScenarioSpec:
+ """一个激励场景的参数。`kind` 决定其余字段怎么用。"""
+
+ kind: str # hold | sine | poses
+ name: str = ""
+ duration: float = 3.0 # hold/sine 的总时长 (s)
+ q_center: Optional[List[float]] = None
+ amp: float = 0.15 # sine 幅值 (rad)
+ freq: float = 0.2 # sine 频率 (Hz)
+ joints: Optional[List[int]] = None # sine 激励哪些关节 (0 基); None = 全部
+ poses: Optional[List[List[float]]] = None # poses 场景的目标位形
+ dwell: float = 1.2 # poses 每个位形停驻 (s)
+ speed: float = 0.2 # poses 的 movej speed 倍率
+ command_hz: float = 50.0 # sine 连续伺服重发率
+ pre_roll: float = 0.5 # sine 前的静止段 (s)
+ post_roll: float = 1.0 # sine 后的静止段 (s)
+
+ def label(self) -> str:
+ return self.name or self.kind
+
+
+def read_joint_limits(arm) -> List[Tuple[float, float]]:
+ """从固件读软限位 (0x24)。读不到就直接抛 —— 不会静默用宽松默认值。"""
+ out = []
+ for i in range(arm.n):
+ jp = arm.params.get_joint_param(i).value
+ out.append((float(jp.q_min), float(jp.q_max)))
+ return out
+
+
+def _check_pose(pose: Sequence[float], limits: Sequence[Tuple[float, float]],
+ label: str, margin: float = MIN_MARGIN) -> None:
+ if len(pose) != len(limits):
+ raise ValueError(f"{label}: 位形需要 {len(limits)} 个关节, 给的是 {len(pose)}")
+ for j, (q, (lo, hi)) in enumerate(zip(pose, limits)):
+ if not (math.isfinite(q)):
+ raise ValueError(f"{label}: J{j + 1} 角度非有限 ({q})")
+ left = q - lo
+ right = hi - q
+ if left < margin or right < margin:
+ raise ValueError(
+ f"{label}: J{j + 1} q={q:+.4f} 离软限位 [{lo:+.3f}, {hi:+.3f}] "
+ f"余量 {min(left, right):.3f} < {margin} rad,拒绝执行")
+
+
+def validate_scenario(spec: ScenarioSpec, limits: Sequence[Tuple[float, float]],
+ margin: float = MIN_MARGIN) -> Dict[str, object]:
+ """检查场景是否会越限/参数是否合法, 返回给用户看的计划摘要。"""
+ n = len(limits)
+ if spec.kind == "hold":
+ if spec.q_center is None:
+ raise ValueError("hold 场景需要 q_center")
+ _check_pose(spec.q_center, limits, "hold", margin)
+ if spec.duration <= 0:
+ raise ValueError("hold duration 必须 > 0")
+ return {"kind": "hold", "duration": spec.duration}
+
+ if spec.kind == "sine":
+ if spec.q_center is None:
+ raise ValueError("sine 场景需要 q_center")
+ if not (0.0 < spec.amp <= MAX_SINE_AMP):
+ raise ValueError(f"sine amp 必须在 (0, {MAX_SINE_AMP}] rad")
+ if not (0.0 < spec.freq <= MAX_SINE_FREQ):
+ raise ValueError(f"sine freq 必须在 (0, {MAX_SINE_FREQ}] Hz")
+ if spec.duration <= 0:
+ raise ValueError("sine duration 必须 > 0")
+ if not (MIN_COMMAND_HZ <= spec.command_hz <= MAX_COMMAND_HZ):
+ raise ValueError(f"command_hz 必须在 [{MIN_COMMAND_HZ}, {MAX_COMMAND_HZ}]")
+ joints = list(spec.joints) if spec.joints is not None else list(range(n))
+ for j in joints:
+ if not 0 <= j < n:
+ raise ValueError(f"sine joints 里的 {j} 越界 (0..{n - 1})")
+ # 所有选中关节同相时是最大偏移; 两个极端都查。
+ hi = list(spec.q_center)
+ lo = list(spec.q_center)
+ for j in joints:
+ hi[j] += spec.amp
+ lo[j] -= spec.amp
+ _check_pose(lo, limits, "sine 下极限", margin)
+ _check_pose(hi, limits, "sine 上极限", margin)
+ return {"kind": "sine", "duration": spec.duration, "amp_rad": spec.amp,
+ "freq_hz": spec.freq, "joints": [j + 1 for j in joints],
+ "command_hz": spec.command_hz}
+
+ if spec.kind == "poses":
+ if not spec.poses:
+ raise ValueError("poses 场景需要 poses")
+ if spec.dwell < 0:
+ raise ValueError("poses dwell 不能为负")
+ if not (0.0 < spec.speed <= 1.0):
+ raise ValueError("poses speed 必须在 (0, 1]")
+ for k, p in enumerate(spec.poses):
+ _check_pose(p, limits, f"pose{k}")
+ return {"kind": "poses", "poses": len(spec.poses),
+ "dwell_s": spec.dwell, "speed": spec.speed}
+
+ raise ValueError(f"未知场景 kind={spec.kind!r} (hold|sine|poses)")
+
+
+# ---------------------------------------------------------------------------
+# 使能与运动
+# ---------------------------------------------------------------------------
+
+def wait_enabled(arm, target: bool = True, timeout: float = 2.0) -> bool:
+ """轮询状态帧直到 `enabled` 等于 `target`。
+
+ `enable()` 在状态帧反映之前就返回 (真机实测要 17~48ms), 所以只能轮询。
+ """
+ end = time.monotonic() + timeout
+ while time.monotonic() < end:
+ st = arm.get_state().value
+ if st is not None and st.enabled == target:
+ return True
+ time.sleep(0.02)
+ st = arm.get_state().value
+ return st is not None and st.enabled == target
+
+
+def ensure_enabled(arm, go: bool) -> None:
+ st = arm.get_state().value
+ if st is None:
+ raise RuntimeError("拿不到状态帧, 无法确认使能状态")
+ if st.faulted:
+ raise RuntimeError(f"臂有故障位, 拒绝运动: {st.fault_detail}")
+ if st.enabled:
+ return
+ if not go:
+ raise RuntimeError("臂未使能; 采集需要使能持位 (加 --go 才会 enable)")
+ arm.enable()
+ if not wait_enabled(arm, True):
+ raise RuntimeError("enable() 已返回但状态帧仍未使能")
+
+
+def _hold_stream(arm, q: Sequence[float], duration: float, hz: float) -> int:
+ """以 `hz` 重发 move_js 零速目标, 维持连续伺服看门狗。返回实际发帧数。"""
+ if duration <= 0:
+ return 0
+ period = 1.0 / hz
+ dq = [0.0] * len(q)
+ n = 0
+ t_end = time.monotonic() + duration
+ t_next = time.monotonic()
+ while True:
+ now = time.monotonic()
+ if now >= t_end:
+ break
+ arm.move_js(q, dq)
+ n += 1
+ t_next += period
+ left = t_next - time.monotonic()
+ if left > 0:
+ time.sleep(left)
+ return n
+
+
+def _run_hold(arm, spec: ScenarioSpec) -> Dict[str, object]:
+ """静止持位: 必须持续重发 move_js。
+
+ 固件的 0.1s 命令看门狗超时即 fail-soft (降刚度、断重力前馈)。只 sleep 的话
+ 整个 hold 段录到的是 WD_TRIPPED 下的假持位 —— 真机上实测 flags=0x202。
+ """
+ hz = float(spec.command_hz)
+ sent = _hold_stream(arm, list(spec.q_center), spec.duration, hz)
+ return {"duration_s": spec.duration, "command_frames": sent,
+ "command_rate_hz": round(sent / max(spec.duration, 1e-9), 2)}
+
+
+def _run_sine(arm, spec: ScenarioSpec) -> Dict[str, object]:
+ q0 = list(spec.q_center)
+ joints = list(spec.joints) if spec.joints is not None else list(range(len(q0)))
+ hz = float(spec.command_hz)
+ period = 1.0 / hz
+ w = 2.0 * math.pi * float(spec.freq)
+ amp = float(spec.amp)
+
+ _hold_stream(arm, q0, spec.pre_roll, hz)
+ t_start = time.monotonic()
+ t_end = t_start + spec.duration
+ sent = 0
+ t_next = time.monotonic()
+ while True:
+ now = time.monotonic()
+ if now >= t_end:
+ break
+ ph = w * (now - t_start)
+ q_ref = list(q0)
+ dq_ref = [0.0] * len(q0)
+ s, c = math.sin(ph), math.cos(ph)
+ for j in joints:
+ q_ref[j] = q0[j] + amp * s
+ dq_ref[j] = amp * w * c
+ arm.move_js(q_ref, dq_ref)
+ sent += 1
+ t_next += period
+ left = t_next - time.monotonic()
+ if left > 0:
+ time.sleep(left)
+ _hold_stream(arm, q0, spec.post_roll, hz)
+ return {"sine_s": spec.duration, "command_frames": sent,
+ "command_rate_hz": round(sent / max(spec.duration, 1e-9), 2)}
+
+
+def _run_poses(arm, spec: ScenarioSpec) -> Dict[str, object]:
+ """多个位形的慢速往返: 用 move_js 流式走 smoothstep 参考, 每个位形停驻 `dwell`。
+
+ 为什么不用 `movej`: 固件 movej 的静止保持靠位置误差, 台架重力前馈偏差会留下
+ 0.04~0.05 rad 稳态误差, 超过 SDK 到位判据 `q_tol=0.03` ⇒ 必然 MotionTimeoutError,
+ 整段数据全丢。这里自己生成 jerk/加速度连续的参考, 顺带给 ddq 一条干净来源。
+ """
+ if not spec.poses:
+ return {"poses": 0, "moves": []}
+ hz = float(spec.command_hz)
+ period = 1.0 / hz
+ q = list(spec.q_center)
+ v_ref = max(POSE_SPEED_RAD_S * float(spec.speed or 1.0), 1e-3)
+ moves = []
+
+ _hold_stream(arm, q, spec.pre_roll, hz)
+ for k, target in enumerate(spec.poses):
+ target = [float(v) for v in target]
+ dq_seg = [target[j] - q[j] for j in range(len(q))]
+ dur = max(0.6, max(abs(v) for v in dq_seg) / v_ref)
+ t0 = time.monotonic()
+ t_end = t0 + dur
+ sent = 0
+ t_next = t0
+ while True:
+ now = time.monotonic()
+ if now >= t_end:
+ break
+ u = (now - t0) / dur
+ s = filters.smoothstep(u)
+ sd = filters.smoothstep_deriv(u) / dur
+ arm.move_js([q[j] + dq_seg[j] * s for j in range(len(q))],
+ [dq_seg[j] * sd for j in range(len(q))])
+ sent += 1
+ t_next += period
+ left = t_next - time.monotonic()
+ if left > 0:
+ time.sleep(left)
+ arm.move_js(target, [0.0] * len(q)) # 精确末点 + 零速
+ q = target
+ _hold_stream(arm, q, spec.dwell, hz)
+ moves.append({"pose": k, "move_s": round(time.monotonic() - t0 - spec.dwell, 3),
+ "command_frames": sent})
+ _hold_stream(arm, q, spec.post_roll, hz)
+ return {"poses": len(spec.poses), "moves": moves}
+
+
+# ---------------------------------------------------------------------------
+# 静态重力核查
+# ---------------------------------------------------------------------------
+
+def static_gravity_check(arm, samples: Sequence[Sample],
+ tail_s: float = GRAVITY_TAIL_S) -> Dict[str, object]:
+ """用末尾静止段的实测 τ 对 `model.get_gravity(q)` 做一次静态核查。
+
+ 这是 issue 点名的 «用 model.get_gravity 做对照»。只在尾窗内所有关节
+ |dq| < STATIC_DQ_MAX 时成立; 否则如实标 skipped —— 不拿运动段冒充。
+ """
+ if not samples:
+ return {"skipped": "无采样"}
+ t_end = samples[-1].t
+ tail = [s for s in samples if s.t >= t_end - tail_s]
+ if not tail:
+ return {"skipped": "尾窗为空"}
+ n = len(tail[0].q)
+ dq_max = max(abs(v) for s in tail for v in s.dq)
+ if dq_max > STATIC_DQ_MAX:
+ return {"skipped": f"尾窗不静止 (max|dq|={dq_max:.4f} > {STATIC_DQ_MAX})",
+ "max_abs_dq": round(dq_max, 6)}
+ q = [filters.mean([s.q[j] for s in tail]) for j in range(n)]
+ tau_meas = [filters.mean([s.tau[j] for s in tail]) for j in range(n)]
+ try:
+ tau_model = list(arm.model.get_gravity(q).value)
+ except Exception as e: # noqa: BLE001 - 老固件可能没有 0x39
+ return {"skipped": f"get_gravity 不可用: {type(e).__name__}: {e}"}
+ resid = [tau_meas[j] - tau_model[j] for j in range(n)]
+ rms = math.sqrt(sum(v * v for v in resid) / len(resid)) if resid else float("nan")
+ return {
+ "q": [round(v, 6) for v in q],
+ "tau_meas": [round(v, 6) for v in tau_meas],
+ "tau_model": [round(v, 6) for v in tau_model],
+ "residual": [round(v, 6) for v in resid],
+ "rms": round(rms, 6),
+ "max_abs_dq": round(dq_max, 6),
+ }
+
+
+# ---------------------------------------------------------------------------
+# 元数据
+# ---------------------------------------------------------------------------
+
+def _state_meta(st) -> Optional[Dict[str, object]]:
+ if st is None:
+ return None
+ return {"t": 0.0, "seq": st.seq, "mode": st.mode, "mode_name": st.mode_name,
+ "flags": st.flags, "enabled": st.enabled, "joint_fault": st.joint_fault,
+ "q": [round(v, 6) for v in st.q],
+ "dq": [round(v, 6) for v in st.dq],
+ "tau": [round(v, 6) for v in st.tau],
+ "flag_names": list(st.flag_names)}
+
+
+def _build_meta(arm, spec: ScenarioSpec, snapshot: ModelSnapshot, st0, st_end,
+ limits, plan, fs: float, scenario_result: Dict[str, object],
+ gravity_check: Dict[str, object],
+ ambient_c: Optional[float]) -> Dict[str, object]:
+ now = _dt.datetime.now().astimezone()
+ temps = []
+ if st_end is not None:
+ temps = [j.t_coil for j in st_end.joints]
+ meta: Dict[str, object] = {
+ "tool": "tau_eval",
+ "schema": "tau_eval.session.v1",
+ "created_at": now.isoformat(timespec="seconds"),
+ "hostname": socket.gethostname(),
+ "platform": platform.platform(),
+ "python": platform.python_version(),
+ "scenario": spec.label(),
+ "scenario_spec": asdict(spec),
+ "scenario_plan": plan,
+ "scenario_result": scenario_result,
+ "fs": fs,
+ "firmware": getattr(arm, "firmware", "") or snapshot.firmware,
+ "joint_count": int(getattr(arm, "n", snapshot.joint_count)),
+ "model_geometry_sha16": snapshot.geometry_sha16,
+ "model_file": "model.json",
+ "model_read_warnings": list(snapshot.read_warnings),
+ "payload": {"mass": snapshot.payload_mass,
+ "com": list(snapshot.payload_com)},
+ "gravity": list(snapshot.gravity),
+ "gravity_scale": list(snapshot.gravity_scale),
+ "inertia_scale": list(snapshot.inertia_scale),
+ "friction_model": snapshot.friction_model,
+ "joint_limits": [[lo, hi] for lo, hi in limits],
+ "initial_state": _state_meta(st0),
+ "final_state": _state_meta(st_end),
+ "motor_temperature_c": [round(v, 3) for v in temps],
+ "motor_temperature_max_c": round(max(temps), 3) if temps else None,
+ "ambient_temperature_c": ambient_c,
+ "gravity_check": gravity_check,
+ "notes": [
+ "τ 来自固件电流环估计 (状态帧 joint.tau),口径为关节侧。",
+ "ddq 由实测 dq 中心差分得到,再做零相位低通;见 report 的 filter 段。",
+ "仿真/台架可选 --gravity 覆盖, 但固件 get_gravity 的核查仍按固件参数。",
+ ],
+ }
+ return meta
+
+
+# ---------------------------------------------------------------------------
+# 主入口
+# ---------------------------------------------------------------------------
+
+def run_scenario(arm, spec: ScenarioSpec, outdir: str, fs: float = 100.0,
+ go: bool = False, ambient_c: Optional[float] = None,
+ snapshot: Optional[ModelSnapshot] = None,
+ parquet: bool = False) -> Session:
+ """执行一个场景并落盘 `status.csv` / `meta.json` / `model.json`。
+
+ `go=False` 时只做只读校验并抛 `RuntimeError` 前先返回计划? —— 不, 本函数只负责
+ 真正执行; 规划/打印在 `cli.py`。`go` 只决定是否允许 enable。
+ """
+ if not (1.0 <= fs <= 100.0):
+ raise ValueError("fs 必须在 [1, 100] Hz (固件状态流上限约 100Hz)")
+
+ limits = read_joint_limits(arm)
+ plan = validate_scenario(spec, limits)
+ ensure_enabled(arm, go)
+
+ if snapshot is None:
+ snapshot = fetch_from_arm(arm)
+ st0 = arm.get_state().value
+
+ with StatusRecorder(arm) as rec:
+ if spec.kind == "hold":
+ result = _run_hold(arm, spec)
+ elif spec.kind == "sine":
+ result = _run_sine(arm, spec)
+ elif spec.kind == "poses":
+ result = _run_poses(arm, spec)
+ else: # validate_scenario 已挡住
+ raise ValueError(spec.kind)
+ st_end = arm.get_state().value
+
+ raw = rec.to_session()
+ missing, gap_max = _seq_gaps(raw.samples)
+ raw.meta.update({"seq_missing": missing, "seq_gap_max": gap_max})
+
+ gcheck = static_gravity_check(arm, raw.samples)
+ meta = _build_meta(arm, spec, snapshot, st0, st_end, limits, plan,
+ fs, result, gcheck, ambient_c)
+ meta.update({"seq_missing": missing, "seq_gap_max": gap_max,
+ "raw_frames": len(raw.samples),
+ "raw_rate_hz": round(raw.raw_rate_hz, 2),
+ "t_raw_span_s": round(raw.span, 6)})
+
+ session = raw.resample(fs)
+ session.meta = meta
+
+ paths = session.dump(outdir)
+ model_path = os.path.join(outdir, "model.json")
+ with open(model_path, "w", encoding="utf-8") as f:
+ json.dump(snapshot.to_dict(), f, ensure_ascii=False, indent=2)
+ paths["model_json"] = model_path
+
+ if parquet:
+ pq = _write_parquet(session, os.path.join(outdir, "status.parquet"))
+ if pq:
+ paths["status_parquet"] = pq
+
+ print(f"[tau_eval] {spec.label()}: {len(session.samples)} 帧 / "
+ f"{session.span:.2f}s @ {fs:g}Hz (原始 {len(raw.samples)} 帧, "
+ f"seq 缺 {missing}) -> {outdir}")
+ if gcheck.get("skipped"):
+ print(f"[tau_eval] 静态重力核查: 跳过 ({gcheck['skipped']})")
+ else:
+ print(f"[tau_eval] 静态重力核查: RMS {gcheck['rms']:.4f} N·m, "
+ f"max|res| {max(abs(v) for v in gcheck['residual']):.4f} N·m")
+ return session
+
+
+def _write_parquet(session: Session, path: str) -> Optional[str]:
+ """有 pyarrow 时额外写一份 Parquet; 没有就跳过 (核心路径坚持纯标准库)。"""
+ try:
+ import pyarrow as pa # type: ignore
+ import pyarrow.parquet as pq # type: ignore
+ except Exception: # noqa: BLE001 - 可选依赖
+ print("[tau_eval] 未安装 pyarrow, 跳过 Parquet (CSV 已写)")
+ return None
+ n = session.n
+ data = {"t": [s.t for s in session.samples],
+ "seq": [s.seq for s in session.samples],
+ "mode": [s.mode for s in session.samples],
+ "flags": [s.flags for s in session.samples],
+ "joint_fault": [s.joint_fault for s in session.samples]}
+ for name in ("q", "dq", "tau", "t_mos", "t_coil"):
+ for j in range(n):
+ data[f"{name}{j}"] = [getattr(s, name)[j] for s in session.samples]
+ for j in range(n):
+ data[f"err{j}"] = [s.err[j] for s in session.samples]
+ pq.write_table(pa.table(data), path)
+ return path
diff --git a/tools/tau_eval/examples/.gitignore b/tools/tau_eval/examples/.gitignore
new file mode 100644
index 0000000..b92cf4d
--- /dev/null
+++ b/tools/tau_eval/examples/.gitignore
@@ -0,0 +1,2 @@
+# generated example output — regenerate with make_synthetic.py
+synthetic_sine/
diff --git a/tools/tau_eval/examples/make_synthetic.py b/tools/tau_eval/examples/make_synthetic.py
new file mode 100644
index 0000000..3c63c00
--- /dev/null
+++ b/tools/tau_eval/examples/make_synthetic.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""Generate a hardware-free example session from the factory model itself.
+
+The synthetic `tau` is produced by the same RNEA used for analysis, so the
+residual is pure numerical noise — a useful smoke test for the whole
+collect → analyze → plot → report pipeline.
+
+ python3 tools/tau_eval/examples/make_synthetic.py [--outdir DIR]
+
+Writes status.csv, model.json, meta.json, timeseries.csv, metrics.json,
+figures/*.svg, report.md. Generated files are meant to stay untracked
+(AGENTS.md: never commit generated artifacts).
+"""
+import argparse
+import json
+import math
+import os
+import sys
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_TOOLS = os.path.dirname(os.path.dirname(_HERE))
+if _TOOLS not in sys.path:
+ sys.path.insert(0, _TOOLS)
+
+from tau_eval.analyze import (AnalyzeOptions, analyze_session, # noqa: E402
+ save_analysis)
+from tau_eval.collect import Sample, Session # noqa: E402
+from tau_eval.model import factory_snapshot # noqa: E402
+from tau_eval.plots import generate_all # noqa: E402
+from tau_eval.report import write_report # noqa: E402
+from tau_eval import rnea # noqa: E402
+
+
+def build_session(fs=100.0, duration=2.0, amp=0.15, freq=0.3):
+ snap = factory_snapshot()
+ snap.firmware = "synthetic-example"
+ samples = []
+ for k in range(int(duration * fs) + 1):
+ t = k / fs
+ q, dq, ddq = [], [], []
+ for j in range(7):
+ ph = 2 * math.pi * freq * t + 0.3 * j
+ q.append(amp * math.sin(ph))
+ dq.append(amp * 2 * math.pi * freq * math.cos(ph))
+ ddq.append(-amp * (2 * math.pi * freq) ** 2 * math.sin(ph))
+ d = rnea.theory_torques(snap, q, dq, ddq)
+ samples.append(Sample(
+ t=t, seq=k, mode=3, mode_name="POS", flags=0x200, joint_fault=0,
+ q=q, dq=dq, tau=list(d["total"]),
+ t_mos=[30.0] * 7, t_coil=[31.0] * 7, err=[0] * 7))
+ return snap, Session(samples, {"fs": fs, "scenario": "synthetic-sine"})
+
+
+def main(argv=None):
+ ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
+ ap.add_argument("--outdir", default=os.path.join(_HERE, "synthetic_sine"),
+ help="output directory (default: examples/synthetic_sine)")
+ ap.add_argument("--fs", type=float, default=100.0)
+ ap.add_argument("--duration", type=float, default=2.0)
+ ap.add_argument("--amp", type=float, default=0.15)
+ ap.add_argument("--freq", type=float, default=0.3)
+ args = ap.parse_args(argv)
+
+ snap, sess = build_session(args.fs, args.duration, args.amp, args.freq)
+ outdir = os.path.abspath(args.outdir)
+ os.makedirs(outdir, exist_ok=True)
+ sess.dump(outdir)
+ with open(os.path.join(outdir, "model.json"), "w", encoding="utf-8") as f:
+ json.dump(snap.to_dict(), f, ensure_ascii=False, indent=2)
+
+ result = analyze_session(sess, snap, AnalyzeOptions())
+ save_analysis(result, outdir)
+ figs = generate_all(result, os.path.join(outdir, "figures"))
+ report = write_report(
+ result, figs, outdir,
+ command="python3 tools/tau_eval/examples/make_synthetic.py")
+
+ agg = result.summary["metrics"]["raw"]["aggregate"]
+ print(f"session {os.path.join(outdir, 'status.csv')}")
+ print(f"mean RMSE {agg['mean_rmse']:.3e} N·m "
+ f"max RMSE {agg['max_rmse']:.3e} N·m ({agg['worst_joint']})")
+ print(f"figures {len(figs)}")
+ print(f"report {report}")
+ return 0
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/tools/tau_eval/filters.py b/tools/tau_eval/filters.py
new file mode 100644
index 0000000..bf3f027
--- /dev/null
+++ b/tools/tau_eval/filters.py
@@ -0,0 +1,189 @@
+# -*- coding: utf-8 -*-
+"""重采样、零相位低通与差分 —— 纯标准库,无 numpy。
+
+为什么坚持零相位:100 Hz 流里由 dq 差分得到的 ddq 噪声很大,必须低通;
+但普通因果滤波会把 ddq 整体推迟若干毫秒,误差分析会把「相位延迟」误判成
+「惯量模型错」。前后各滤一遍 (filtfilt) 在通带内净相位为 0,只在两端留下
+约 `2/fc` 秒的边沿瞬态,分析时按边沿裁剪。
+"""
+from __future__ import annotations
+
+import bisect
+import math
+from typing import Iterable, List, Sequence, Tuple
+
+
+def uniform_grid(t0: float, t1: float, fs: float) -> List[float]:
+ """[t0, t1] 上等间隔采样时刻 (含两端)。"""
+ if fs <= 0:
+ raise ValueError("fs 必须 > 0")
+ n = int(math.floor((t1 - t0) * fs + 1e-9)) + 1
+ return [t0 + k / fs for k in range(max(n, 1))]
+
+
+def linear_resample(t: Sequence[float], x: Sequence[float],
+ grid: Sequence[float]) -> List[float]:
+ """对一维序列做线性插值;grid 超出 [t0,t1] 时用端点值钳住。"""
+ if len(t) != len(x):
+ raise ValueError("t 与 x 长度不一致")
+ if not t:
+ return [float("nan")] * len(grid)
+ out = []
+ for g in grid:
+ if g <= t[0]:
+ out.append(float(x[0]))
+ continue
+ if g >= t[-1]:
+ out.append(float(x[-1]))
+ continue
+ j = bisect.bisect_right(t, g)
+ t0, t1 = t[j - 1], t[j]
+ x0, x1 = float(x[j - 1]), float(x[j])
+ if t1 <= t0:
+ out.append(x1)
+ else:
+ a = (g - t0) / (t1 - t0)
+ out.append(x0 + a * (x1 - x0))
+ return out
+
+
+def _butter2_coeffs(fs: float, fc: float) -> Tuple[List[float], List[float]]:
+ """二阶 Butterworth 低通 (双线性变换 + 预畸变)。"""
+ if not (0.0 < fc < fs / 2.0):
+ raise ValueError(f"截止频率 fc={fc} 必须在 (0, fs/2={fs/2}) 内")
+ K = math.tan(math.pi * fc / fs)
+ norm = 1.0 + math.sqrt(2.0) * K + K * K
+ b0 = K * K / norm
+ b = [b0, 2.0 * b0, b0]
+ a = [1.0, 2.0 * (K * K - 1.0) / norm, (1.0 - math.sqrt(2.0) * K + K * K) / norm]
+ return b, a
+
+
+def _lfilter(b: Sequence[float], a: Sequence[float], x: Sequence[float]) -> List[float]:
+ # a[0] 已归一化为 1;输入输出都用零初始状态。
+ b0, b1, b2 = b
+ a1, a2 = a[1], a[2]
+ x1 = x2 = y1 = y2 = 0.0
+ out = []
+ for v in x:
+ y = b0 * v + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2
+ x2, x1 = x1, v
+ y2, y1 = y1, y
+ out.append(y)
+ return out
+
+
+def _reflect_pad(x: Sequence[float], pad: int) -> List[float]:
+ if pad <= 0 or len(x) < 2:
+ return list(x)
+ pad = min(pad, len(x) - 1)
+ left = [x[i] for i in range(pad, 0, -1)]
+ right = [x[i] for i in range(len(x) - 2, len(x) - pad - 2, -1)]
+ return left + list(x) + right
+
+
+def lowpass_zero_phase(x: Sequence[float], fs: float, fc: float) -> List[float]:
+ """二阶 Butterworth 前后向滤波 (净相位 0)。过短序列原样返回。"""
+ n = len(x)
+ if n < 4 or not (fc > 0.0):
+ return [float(v) for v in x]
+ b, a = _butter2_coeffs(fs, fc)
+ pad = max(9, int(math.ceil(2.0 * fs / fc)))
+ y = _lfilter(b, a, _reflect_pad(x, pad))
+ y.reverse()
+ y = _lfilter(b, a, y)
+ y.reverse()
+ core = y[pad:pad + n] if len(y) >= pad + n else y[:n]
+ return list(core)
+
+
+def central_diff(x: Sequence[float], dt: float) -> List[float]:
+ """二阶中心差分;端点用单侧一阶差分。"""
+ n = len(x)
+ if n == 0:
+ return []
+ if n == 1:
+ return [0.0]
+ out = [0.0] * n
+ for i in range(1, n - 1):
+ out[i] = (x[i + 1] - x[i - 1]) / (2.0 * dt)
+ out[0] = (x[1] - x[0]) / dt
+ out[-1] = (x[-1] - x[-2]) / dt
+ return out
+
+
+def smoothstep(u: float) -> float:
+ """0..1 上的五次平滑窗 (端点一阶、二阶导均为 0)。"""
+ if u <= 0.0:
+ return 0.0
+ if u >= 1.0:
+ return 1.0
+ return u * u * u * (u * (u * 6.0 - 15.0) + 10.0)
+
+
+def smoothstep_deriv(u: float) -> float:
+ if u <= 0.0 or u >= 1.0:
+ return 0.0
+ return 30.0 * u * u * (1.0 - u) * (1.0 - u)
+
+
+def mean(vals: Sequence[float]) -> float:
+ return sum(vals) / len(vals) if vals else float("nan")
+
+
+def pstdev(vals: Sequence[float]) -> float:
+ if len(vals) < 2:
+ return 0.0
+ m = mean(vals)
+ return math.sqrt(sum((v - m) ** 2 for v in vals) / (len(vals) - 1))
+
+
+def quantile(vals: Sequence[float], q: float) -> float:
+ """线性插值分位数 (与 numpy 默认一致)。"""
+ if not vals:
+ return float("nan")
+ s = sorted(float(v) for v in vals)
+ if len(s) == 1:
+ return s[0]
+ pos = (len(s) - 1) * min(max(q, 0.0), 1.0)
+ lo = int(math.floor(pos))
+ hi = min(lo + 1, len(s) - 1)
+ a = pos - lo
+ return s[lo] * (1.0 - a) + s[hi] * a
+
+
+def pearson(x: Sequence[float], y: Sequence[float]) -> float:
+ if len(x) != len(y) or len(x) < 2:
+ return float("nan")
+ mx, my = mean(x), mean(y)
+ sxx = sum((a - mx) ** 2 for a in x)
+ syy = sum((b - my) ** 2 for b in y)
+ if sxx <= 0.0 or syy <= 0.0:
+ return float("nan")
+ sxy = sum((a - mx) * (b - my) for a, b in zip(x, y))
+ return sxy / math.sqrt(sxx * syy)
+
+
+def linear_fit(x: Sequence[float], y: Sequence[float]) -> Tuple[float, float, float]:
+ """最小二乘 y ≈ a + b·x,返回 (a, b, r)。"""
+ if len(x) != len(y) or len(x) < 2:
+ return (float("nan"), float("nan"), float("nan"))
+ mx, my = mean(x), mean(y)
+ sxx = sum((v - mx) ** 2 for v in x)
+ if sxx <= 0.0:
+ return (float("nan"), float("nan"), float("nan"))
+ sxy = sum((a - mx) * (b - my) for a, b in zip(x, y))
+ b = sxy / sxx
+ a = my - b * mx
+ return (a, b, pearson(x, y))
+
+
+def r2_score(meas: Sequence[float], pred: Sequence[float]) -> float:
+ if len(meas) != len(pred) or not meas:
+ return float("nan")
+ m = mean(meas)
+ ss_tot = sum((v - m) ** 2 for v in meas)
+ ss_res = sum((a - b) ** 2 for a, b in zip(meas, pred))
+ if ss_tot <= 0.0:
+ return float("nan")
+ return 1.0 - ss_res / ss_tot
diff --git a/tools/tau_eval/model.py b/tools/tau_eval/model.py
new file mode 100644
index 0000000..2a69bf0
--- /dev/null
+++ b/tools/tau_eval/model.py
@@ -0,0 +1,529 @@
+# -*- coding: utf-8 -*-
+"""动力学模型快照 —— 固件模型参数 (0x30/0x33/0x34) 或 URDF 的纯 Python 表示。
+
+本模块只做数据:刚体 (m/com/I)、jm、payload、重力、缩放系数与摩擦参数。
+递推在 `rnea.py`;采集在 `collect.py`;分析在 `analyze.py`。
+
+几何常量来自固件 `tools/export_model.py` 从
+`assets/litearm/urdf/litearm_id.urdf` 生成的 `dyn_model_data.h`
+(sha256[:16] = 640bf04bf532e395)。固件只在线暴露惯性参数与 jm,
+**运动学链 (固定位姿/关节轴) 不提供在线读取**,所以这里内嵌同一份常量,
+并用 `from_urdf()` 支持从 URDF 重算核对。
+
+⚠ 固件模型是辨识得到的**回归量式**参数:link mass 多为 1、惯量含负值、
+M 不是正定矩阵。τ = M·q̈ + C·q̇ + G 仍然正确,但**不要对 M 求逆**。
+本工具不含任何求逆路径。
+"""
+from __future__ import annotations
+
+import hashlib
+import math
+import os
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Sequence, Tuple
+
+# ---------------------------------------------------------------------------
+# 运动学几何 (dyn_model_data.h)
+# ---------------------------------------------------------------------------
+
+#: `tool/geometry.py` 内嵌常量的来源指纹;`from_urdf()` 会重算并记录实际文件的 sha。
+GEOMETRY_SHA16 = "640bf04bf532e395"
+
+DYN_PARENT = (-1, 0, 1, 2, 3, 4, 5, 6, 7)
+#: -1 = base, 0 = revolute (有 DOF), 1 = fixed
+DYN_JTYPE = (-1, 0, 0, 0, 0, 0, 0, 0, 1)
+DYN_AXIS = (
+ (0.0, 0.0, 0.0),
+ (0.0, 0.0, 1.0),
+ (0.0, 0.0, 1.0),
+ (0.0, 0.0, 1.0),
+ (0.0, 0.0, -1.0),
+ (0.0, 0.0, 1.0),
+ (0.0, 0.0, -1.0),
+ (0.0, 0.0, -1.0),
+ (0.0, 0.0, 0.0),
+)
+DYN_FIX_XYZ = (
+ (0.0, 0.0, 0.0),
+ (-2.23694604e-07, 7.53497322e-07, 0.13324993),
+ (0.0, 0.0, 0.0),
+ (0.0, -0.2995, 0.0),
+ (0.038, 0.0, 0.0),
+ (-0.038, -0.2225, 0.0),
+ (0.0, 0.0, 0.0),
+ (-0.074, 0.0, 0.0),
+ (-0.053749853, 6.97627774e-07, 1.77373939e-06),
+)
+DYN_FIX_RPY = (
+ (0.0, 0.0, 0.0),
+ (0.0, 0.0, 1.57079265),
+ (-1.57079633, 0.0, 0.0),
+ (1.57079633, 0.0, 0.0),
+ (-1.57079633, 0.0, 0.0),
+ (1.57079633, 0.0, 0.0),
+ (0.0, 1.57079633, 1.57079633),
+ (1.57079633, 0.0, 0.0),
+ (0.0, -1.5707881, -3.46291658e-06),
+)
+DYN_MASS = (0.364380717, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0)
+DYN_COM = (
+ (2.81752473e-05, -6.21980479e-07, 0.0355224095),
+ (0.00387153474, 0.0, 0.0),
+ (0.0527914515, 0.648081281, 0.0),
+ (-0.0681886379, -0.00784773695, 0.0),
+ (0.0320031263, 0.304352812, 0.0),
+ (4.775927e-05, -3.12025358e-05, 0.0),
+ (0.0242589492, 0.000115404839, 0.0),
+ (-0.0033098347, 9.95219448e-05, 0.0),
+ (0.0, 0.0, 0.0),
+)
+DYN_INERTIA = (
+ (0.000273641151, 1.67098714e-08, -1.09565995e-08, 0.000251821003, -3.3686319e-09, 0.000377759326),
+ (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+ (-1.17575709, 0.0255899977, -0.0223129781, -0.00278693735, 0.0211291862, -1.22121557),
+ (-0.0884669606, -0.00697576894, -0.0269419248, -0.00464969033, 0.0382667169, 0.0568640218),
+ (-0.411442517, 0.0141869443, -1.53041911e-05, -0.00102420009, -0.0134242531, -0.439410942),
+ (-0.0203443616, 0.00585590416, -0.00562145578, -2.28094787e-09, 6.44438452e-06, -0.0360883733),
+ (0.0357078987, -0.000649135069, -0.000377149847, -0.000588496614, 5.6738306e-05, -0.0191189771),
+ (-0.0028817802, -0.000974346793, -0.000418232835, -1.09550057e-05, -0.000162816495, 0.00673921812),
+ (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+)
+#: 出厂 jm (dyn_jm_default.h)。同样是辨识基系数,不是物理转子惯量。
+DYN_JM_DEFAULT = (0.0, 0.0, -0.0188174839, -0.0236409545,
+ -0.00612815024, -0.0196070617, -0.0142279878)
+
+N_BODY = 9
+N_JOINT = 7
+
+
+# ---------------------------------------------------------------------------
+# 小型线性代数 / 旋转
+# ---------------------------------------------------------------------------
+
+def rpy_to_matrix(rpy: Sequence[float]) -> List[List[float]]:
+ """URDF rpy -> R = Rz(y)·Ry(p)·Rx(r)。"""
+ r, p, y = (float(v) for v in rpy)
+ cr, sr = math.cos(r), math.sin(r)
+ cp, sp = math.cos(p), math.sin(p)
+ cy, sy = math.cos(y), math.sin(y)
+ return [
+ [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+ [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+ [-sp, cp * sr, cp * cr],
+ ]
+
+
+def sym6_to_matrix(i6: Sequence[float]) -> List[List[float]]:
+ ixx, ixy, ixz, iyy, iyz, izz = (float(v) for v in i6)
+ return [[ixx, ixy, ixz], [ixy, iyy, iyz], [ixz, iyz, izz]]
+
+
+def matrix_to_sym6(m: Sequence[Sequence[float]]) -> Tuple[float, ...]:
+ return (m[0][0], m[0][1], m[0][2], m[1][1], m[1][2], m[2][2])
+
+
+def rotate_inertia(i6: Sequence[float], rpy: Sequence[float]) -> Tuple[float, ...]:
+ """把惯量张量从 inertial 系旋到 link 系:I' = R·I·Rᵀ。"""
+ if not any(abs(float(v)) > 1e-15 for v in rpy):
+ return tuple(float(v) for v in i6)
+ R = rpy_to_matrix(rpy)
+ I = sym6_to_matrix(i6)
+ IR = [[sum(I[i][k] * R[j][k] for k in range(3)) for j in range(3)] for i in range(3)]
+ out = [[sum(R[i][k] * IR[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
+ return matrix_to_sym6(out)
+
+
+# ---------------------------------------------------------------------------
+# 模型快照
+# ---------------------------------------------------------------------------
+
+@dataclass
+class Body:
+ """单个刚体的惯性参数 (link 系)。"""
+
+ mass: float = 0.0
+ com: Tuple[float, float, float] = (0.0, 0.0, 0.0)
+ inertia: Tuple[float, float, float, float, float, float] = (0.0,) * 6
+
+ def as_list(self) -> List[float]:
+ return [self.mass, *self.com, *self.inertia]
+
+ @classmethod
+ def from_vals(cls, vals: Sequence[float]) -> "Body":
+ if len(vals) != 10:
+ raise ValueError("body 参数必须是 10 个 (m, cx, cy, cz, ixx, ixy, ixz, iyy, iyz, izz)")
+ v = [float(x) for x in vals]
+ return cls(v[0], (v[1], v[2], v[3]), (v[4], v[5], v[6], v[7], v[8], v[9]))
+
+
+@dataclass
+class ModelSnapshot:
+ """一份可落盘、可离线复用的动力学模型。"""
+
+ bodies: List[Body]
+ jm: List[float]
+ payload_mass: float = 0.0
+ payload_com: Tuple[float, float, float] = (0.0, 0.0, 0.0)
+ gravity: Tuple[float, float, float] = (0.0, 0.0, -9.81)
+ gravity_scale: List[float] = field(default_factory=lambda: [1.0] * N_JOINT)
+ inertia_scale: List[float] = field(default_factory=lambda: [1.0] * N_JOINT)
+ friction_model: int = 1
+ friction_v: List[float] = field(default_factory=lambda: [0.0] * N_JOINT)
+ friction_fc0: List[float] = field(default_factory=lambda: [0.0] * N_JOINT)
+ friction_fc1: List[float] = field(default_factory=lambda: [0.0] * N_JOINT)
+ fric_v2_eps: float = 0.05
+ ff_mask: Optional[int] = None
+ firmware: str = ""
+ joint_count: int = N_JOINT
+ geometry_sha16: str = GEOMETRY_SHA16
+ model_status: Dict[str, object] = field(default_factory=dict)
+ read_warnings: List[str] = field(default_factory=list)
+
+ def validate(self) -> None:
+ if len(self.bodies) != N_BODY:
+ raise ValueError(f"需要 {N_BODY} 个刚体, 实际 {len(self.bodies)}")
+ if len(self.jm) != N_JOINT:
+ raise ValueError(f"jm 需要 {N_JOINT} 个, 实际 {len(self.jm)}")
+ for name in ("gravity_scale", "inertia_scale",
+ "friction_v", "friction_fc0", "friction_fc1"):
+ if len(getattr(self, name)) != N_JOINT:
+ raise ValueError(f"{name} 需要 {N_JOINT} 个")
+
+ def effective_bodies(self) -> List[Body]:
+ """bank + payload -> 生效层,与固件 `load_eff()` 同式。
+
+ payload 只复合进 body8 (ee 固定体),com 在 **ee_link 系**。
+ body8 的 bank 质量必须为 0;和为 0/负时固件直接放弃复合,这里照做。
+ """
+ out = [Body(b.mass, b.com, b.inertia) for b in self.bodies]
+ m1 = float(self.payload_mass)
+ if not (m1 > 0.0):
+ return out
+ k = N_BODY - 1
+ b = out[k]
+ m0 = b.mass
+ if not (m0 + m1 > 0.0):
+ return out
+ c0 = b.com
+ c1 = self.payload_com
+ c = tuple((m0 * c0[i] + m1 * c1[i]) / (m0 + m1) for i in range(3))
+ I = list(b.inertia)
+ for mass, r in ((m0, tuple(c0[i] - c[i] for i in range(3))),
+ (m1, tuple(c1[i] - c[i] for i in range(3)))):
+ r2 = sum(v * v for v in r)
+ I[0] += mass * (r2 - r[0] * r[0])
+ I[1] += mass * (-r[0] * r[1])
+ I[2] += mass * (-r[0] * r[2])
+ I[3] += mass * (r2 - r[1] * r[1])
+ I[4] += mass * (-r[1] * r[2])
+ I[5] += mass * (r2 - r[2] * r[2])
+ out[k] = Body(m0 + m1, c, tuple(I))
+ return out
+
+ def friction_torque(self, dq: Sequence[float]) -> List[float]:
+ """固件 v2 摩擦前馈的稳态值 (以**实测** dq 为输入)。
+
+ 公式 (law.c `law_friction_v2`):
+ th = tanh(dq/eps)
+ tau = fv·dq + (fc0−fc1)/2·th + (fc0+fc1)/2·th²
+ 忽略 slew (稳态评估);friction_model != 1 时返回 0 并由调用方在
+ 报告里标注「摩擦项未纳入」。固件对参数的钳制在这里同样执行。
+ """
+ if self.friction_model != 1:
+ return [0.0] * N_JOINT
+ eps = max(1e-3, float(self.fric_v2_eps))
+ out = []
+ for i in range(N_JOINT):
+ fv = max(0.0, float(self.friction_v[i]))
+ fc0 = max(0.0, float(self.friction_fc0[i]))
+ fc1 = min(0.0, float(self.friction_fc1[i]))
+ if fv == 0.0 and fc0 == 0.0 and fc1 == 0.0:
+ out.append(0.0)
+ continue
+ th = math.tanh(float(dq[i]) / eps)
+ out.append(fv * float(dq[i]) + 0.5 * (fc0 - fc1) * th
+ + 0.5 * (fc0 + fc1) * th * th)
+ return out
+
+ # ---- 序列化 ----
+ def to_dict(self) -> Dict[str, object]:
+ return {
+ "geometry_sha16": self.geometry_sha16,
+ "firmware": self.firmware,
+ "joint_count": self.joint_count,
+ "bodies": [b.as_list() for b in self.bodies],
+ "jm": [float(v) for v in self.jm],
+ "payload": {"mass": float(self.payload_mass),
+ "com": [float(v) for v in self.payload_com]},
+ "gravity": [float(v) for v in self.gravity],
+ "gravity_scale": [float(v) for v in self.gravity_scale],
+ "inertia_scale": [float(v) for v in self.inertia_scale],
+ "friction": {"model": int(self.friction_model),
+ "v": [float(v) for v in self.friction_v],
+ "fc0": [float(v) for v in self.friction_fc0],
+ "fc1": [float(v) for v in self.friction_fc1],
+ "v2_eps": float(self.fric_v2_eps)},
+ "ff_mask": None if self.ff_mask is None else int(self.ff_mask),
+ "model_status": self.model_status,
+ "read_warnings": list(self.read_warnings),
+ }
+
+ @classmethod
+ def from_dict(cls, d: Dict[str, object]) -> "ModelSnapshot":
+ fr = d.get("friction") or {}
+ pay = d.get("payload") or {}
+ snap = cls(
+ bodies=[Body.from_vals(v) for v in d["bodies"]],
+ jm=[float(v) for v in d["jm"]],
+ payload_mass=float(pay.get("mass", 0.0)),
+ payload_com=tuple(float(v) for v in pay.get("com", (0.0, 0.0, 0.0))),
+ gravity=tuple(float(v) for v in d.get("gravity", (0.0, 0.0, -9.81))),
+ gravity_scale=[float(v) for v in d.get("gravity_scale", [1.0] * N_JOINT)],
+ inertia_scale=[float(v) for v in d.get("inertia_scale", [1.0] * N_JOINT)],
+ friction_model=int(fr.get("model", 1)),
+ friction_v=[float(v) for v in fr.get("v", [0.0] * N_JOINT)],
+ friction_fc0=[float(v) for v in fr.get("fc0", [0.0] * N_JOINT)],
+ friction_fc1=[float(v) for v in fr.get("fc1", [0.0] * N_JOINT)],
+ fric_v2_eps=float(fr.get("v2_eps", 0.05)),
+ ff_mask=None if d.get("ff_mask") is None else int(d["ff_mask"]),
+ firmware=str(d.get("firmware", "")),
+ joint_count=int(d.get("joint_count", N_JOINT)),
+ geometry_sha16=str(d.get("geometry_sha16", GEOMETRY_SHA16)),
+ model_status=dict(d.get("model_status") or {}),
+ read_warnings=list(d.get("read_warnings") or []),
+ )
+ snap.validate()
+ return snap
+
+
+def factory_snapshot() -> ModelSnapshot:
+ """编译期默认模型 (与 `dyn_model_data.h` / `dyn_jm_default.h` 同值)。
+
+ 只在离线自测 / URDF 不可用且固件读不到时使用;正常采集一律以固件读回为准。
+ """
+ bodies = [Body(DYN_MASS[i], DYN_COM[i], DYN_INERTIA[i]) for i in range(N_BODY)]
+ return ModelSnapshot(bodies=bodies, jm=list(DYN_JM_DEFAULT),
+ firmware="factory-defaults")
+
+
+# ---------------------------------------------------------------------------
+# 在线读取固件模型
+# ---------------------------------------------------------------------------
+
+def _read(arm, label: str, fn, default, warnings: List[str]):
+ try:
+ return fn()
+ except Exception as e: # noqa: BLE001 - 读不到就降级并记账
+ warnings.append(f"{label}: {type(e).__name__}: {e}")
+ return default
+
+
+def fetch_from_arm(arm, timeout: float = 1.0) -> ModelSnapshot:
+ """把固件里的模型、payload、重力、缩放与摩擦参数读成快照。
+
+ 读取项全部是只读查询 (0x34/0x33/0x2B/0x2C/0x39 相关);不写任何参数。
+ 固件版本过旧导致某项读不到时,用默认值并写进 `read_warnings`。
+ """
+ warnings: List[str] = []
+ bodies = []
+ for i in range(N_BODY):
+ vals = _read(arm, f"model.get_body({i})",
+ lambda i=i: arm.model.get_body(i, timeout=timeout).value,
+ [0.0] * 10, warnings)
+ if len(vals) != 10:
+ vals = [0.0] * 10
+ bodies.append(Body.from_vals(vals))
+ jm = _read(arm, "model.get_jm",
+ lambda: arm.model.get_jm(timeout=timeout).value,
+ [0.0] * N_JOINT, warnings)
+
+ payload_mass = _read(arm, "ff_scalar(4)",
+ lambda: arm.get_ff_scalar(4, 0, timeout=timeout).value,
+ 0.0, warnings)
+ payload_com = tuple(
+ _read(arm, f"ff_scalar(5,{k})",
+ lambda k=k: arm.get_ff_scalar(5, k, timeout=timeout).value,
+ 0.0, warnings) for k in range(3))
+ gravity = tuple(
+ _read(arm, f"ff_scalar(6,{k})",
+ lambda k=k: arm.get_ff_scalar(6, k, timeout=timeout).value,
+ 0.0, warnings) for k in range(3))
+ if all(abs(v) < 1e-9 for v in gravity):
+ # 固件默认 s_g = (0,0,-9.81);读回全 0 说明参数区没配置,按固件默认补上并记账。
+ warnings.append("gravity 读回全 0,按固件默认 (0,0,-9.81) 处理")
+ gravity = (0.0, 0.0, -9.81)
+
+ gs = _read(arm, "ff_vec(7) gravity_scale",
+ lambda: list(arm.get_ff_vec(7, timeout=timeout).value),
+ [1.0] * N_JOINT, warnings)
+ isc = _read(arm, "ff_vec(8) inertia_scale",
+ lambda: list(arm.get_ff_vec(8, timeout=timeout).value),
+ [1.0] * N_JOINT, warnings)
+ fv = _read(arm, "ff_vec(9) friction_v",
+ lambda: list(arm.get_ff_vec(9, timeout=timeout).value),
+ [0.0] * N_JOINT, warnings)
+ fc0 = _read(arm, "ff_vec(10) friction_fc0",
+ lambda: list(arm.get_ff_vec(10, timeout=timeout).value),
+ [0.0] * N_JOINT, warnings)
+ fc1 = _read(arm, "ff_vec(11) friction_fc1",
+ lambda: list(arm.get_ff_vec(11, timeout=timeout).value),
+ [0.0] * N_JOINT, warnings)
+ fmodel = _read(arm, "ff_scalar(7) friction_model",
+ lambda: int(round(arm.get_ff_scalar(7, 0, timeout=timeout).value)),
+ 1, warnings)
+ feps = _read(arm, "ff_scalar(8) fric_v2_eps",
+ lambda: float(arm.get_ff_scalar(8, 0, timeout=timeout).value),
+ 0.05, warnings)
+ ff_mask = _read(arm, "get_ff_mask", lambda: arm.get_ff_mask(timeout=timeout),
+ None, warnings)
+ status = _read(arm, "model.status", lambda: arm.model.status(timeout=timeout).value,
+ None, warnings)
+
+ snap = ModelSnapshot(
+ bodies=bodies, jm=[float(v) for v in jm],
+ payload_mass=float(payload_mass), payload_com=payload_com,
+ gravity=gravity, gravity_scale=[float(v) for v in gs],
+ inertia_scale=[float(v) for v in isc],
+ friction_model=int(fmodel),
+ friction_v=[float(v) for v in fv],
+ friction_fc0=[float(v) for v in fc0],
+ friction_fc1=[float(v) for v in fc1],
+ fric_v2_eps=float(feps),
+ ff_mask=None if ff_mask is None else int(ff_mask),
+ firmware=str(getattr(arm, "firmware", "") or ""),
+ joint_count=int(getattr(arm, "n", N_JOINT)),
+ model_status=({"override": status.override,
+ "staged_mask": status.staged_mask,
+ "dirty": status.dirty} if status is not None else {}),
+ read_warnings=warnings,
+ )
+ snap.validate()
+ return snap
+
+
+# ---------------------------------------------------------------------------
+# URDF 解析 (离线模型 / 几何核对)
+# ---------------------------------------------------------------------------
+
+def _parse_floats(text: Optional[str], n: int, default: float = 0.0) -> List[float]:
+ if not text:
+ return [default] * n
+ parts = [float(v) for v in text.replace(",", " ").split()]
+ if len(parts) != n:
+ raise ValueError(f"需要 {n} 个数, 得到 {text!r}")
+ return parts
+
+
+def _link_inertial(link) -> Body:
+ ine = link.find("inertial")
+ if ine is None:
+ return Body()
+ origin = ine.find("origin")
+ xyz = _parse_floats(origin.get("xyz") if origin is not None else None, 3)
+ rpy = _parse_floats(origin.get("rpy") if origin is not None else None, 3)
+ mass_el = ine.find("mass")
+ mass = float(mass_el.get("value", 0.0)) if mass_el is not None else 0.0
+ inel = ine.find("inertia")
+ if inel is None:
+ i6 = (0.0,) * 6
+ else:
+ i6 = tuple(float(inel.get(k, 0.0)) for k in
+ ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
+ return Body(mass, tuple(xyz), rotate_inertia(i6, rpy))
+
+
+def from_urdf(path: str, jm: Optional[Sequence[float]] = None) -> ModelSnapshot:
+ """从 URDF 串行链构造快照。
+
+ 要求结构与本臂一致:root + 7 个 revolute + 1 个 fixed 末端体。关节轴只允许
+ 沿局部 ±z (固件递推的约定);轴位于 x/y 时直接报错,而不是静默算错。
+ """
+ with open(path, "rb") as f:
+ raw = f.read()
+ sha16 = hashlib.sha256(raw).hexdigest()[:16]
+ root = ET.fromstring(raw)
+ links = {l.get("name"): l for l in root.findall("link")}
+ joints = root.findall("joint")
+ child_of = {}
+ joints_by_parent = {}
+ for j in joints:
+ p = j.find("parent").get("link")
+ c = j.find("child").get("link")
+ child_of[c] = p
+ joints_by_parent.setdefault(p, []).append(j)
+ roots = [name for name in links if name not in child_of]
+ if len(roots) != 1:
+ raise ValueError(f"URDF 需要单一根 link, 找到 {roots}")
+ bodies = [_link_inertial(links[roots[0]])]
+ cur = roots[0]
+ while cur in joints_by_parent:
+ children = joints_by_parent[cur]
+ if len(children) != 1:
+ raise ValueError(f"link {cur!r} 有 {len(children)} 个子关节,本工具只支持串行链")
+ j = children[0]
+ jtype = j.get("type")
+ if jtype not in ("revolute", "continuous", "fixed"):
+ raise ValueError(f"关节 {j.get('name')!r} 类型 {jtype!r} 不支持")
+ origin = j.find("origin")
+ xyz = _parse_floats(origin.get("xyz") if origin is not None else None, 3)
+ rpy = _parse_floats(origin.get("rpy") if origin is not None else None, 3)
+ axis_el = j.find("axis")
+ axis = _parse_floats(axis_el.get("xyz") if axis_el is not None else None, 3,
+ default=0.0) if axis_el is not None else [0.0, 0.0, 1.0]
+ if abs(axis[0]) > 1e-9 or abs(axis[1]) > 1e-9 or abs(abs(axis[2]) - 1.0) > 1e-6:
+ raise ValueError(f"关节 {j.get('name')!r} 轴 {axis} 不是局部 ±z,固件递推不支持")
+ child = j.find("child").get("link")
+ bodies.append(_link_inertial(links[child]))
+ cur = child
+ if len(bodies) != N_BODY:
+ raise ValueError(f"URDF 链得到 {len(bodies)} 个刚体,期望 {N_BODY}")
+ if sum(1 for j in joints if j.get("type") != "fixed") != N_JOINT:
+ raise ValueError(f"URDF 可动关节数不是 {N_JOINT}")
+ snap = ModelSnapshot(bodies=bodies,
+ jm=list(jm) if jm is not None else [0.0] * N_JOINT,
+ firmware=f"urdf:{os.path.basename(path)}",
+ geometry_sha16=sha16)
+ snap.validate()
+ return snap
+
+
+def compare_geometry(urdf_path: str) -> List[str]:
+ """把 URDF 的固定位姿/轴与内嵌常量逐项比对,返回差异描述。
+
+ 用于回答「这份 URDF 是否和固件那套几何同源」。差异为空表示一致。
+ """
+ root = ET.fromstring(open(urdf_path, "rb").read())
+ joints = root.findall("joint")
+ child_of = {j.find("child").get("link"): j.find("parent").get("link") for j in joints}
+ by_parent = {}
+ for j in joints:
+ by_parent.setdefault(j.find("parent").get("link"), []).append(j)
+ roots = [l.get("name") for l in root.findall("link") if l.get("name") not in child_of]
+ if len(roots) != 1:
+ return [f"根 link 不唯一: {roots}"]
+ diffs: List[str] = []
+ cur = roots[0]
+ idx = 1
+ while cur in by_parent and idx < N_BODY:
+ j = by_parent[cur][0]
+ origin = j.find("origin")
+ xyz = _parse_floats(origin.get("xyz") if origin is not None else None, 3)
+ rpy = _parse_floats(origin.get("rpy") if origin is not None else None, 3)
+ axis_el = j.find("axis")
+ axis = _parse_floats(axis_el.get("xyz"), 3) if axis_el is not None else [0.0, 0.0, 1.0]
+ for name, got, want in (
+ ("xyz", xyz, DYN_FIX_XYZ[idx]), ("rpy", rpy, DYN_FIX_RPY[idx])):
+ for k in range(3):
+ if abs(got[k] - want[k]) > 5e-6:
+ diffs.append(f"body{idx} {name}[{k}]: urdf={got[k]:.9g} "
+ f"firmware={want[k]:.9g}")
+ sgn_urdf = 1.0 if axis[2] >= 0 else -1.0
+ sgn_fw = 1.0 if DYN_AXIS[idx][2] >= 0 else -1.0
+ if j.get("type") != "fixed" and sgn_urdf != sgn_fw:
+ diffs.append(f"body{idx} axis 符号: urdf={sgn_urdf:+.0f} firmware={sgn_fw:+.0f}")
+ cur = j.find("child").get("link")
+ idx += 1
+ if idx != N_BODY:
+ diffs.append(f"URDF 链只走到 body{idx},未覆盖全部 {N_BODY} 个刚体")
+ return diffs
diff --git a/tools/tau_eval/plots.py b/tools/tau_eval/plots.py
new file mode 100644
index 0000000..1c1627b
--- /dev/null
+++ b/tools/tau_eval/plots.py
@@ -0,0 +1,389 @@
+# -*- coding: utf-8 -*-
+"""纯 SVG 绘图 —— 不依赖 matplotlib/numpy。
+
+为什么手写 SVG: 本仓库的 CI 只装 `.[dev]` (pytest), 而真机上 matplotlib 与
+numpy 2.2.6 有 ABI 冲突。评测工具的核心路径坚持纯标准库, 图必须能在任何
+有 Python 的机器上生成。输出是普通 `.svg`, 浏览器/VS Code/Markdown 都能直接看。
+
+提供的最小图元:
+ line 多序列折线 (时间序列 / 延迟扫描)
+ bars 分组柱状 (逐轴 RMSE / bias / 分层)
+ scatter 散点 + y=x 参考线 (实测 vs 模型)
+ hist 直方图 (残差分布)
+"""
+from __future__ import annotations
+
+import html
+import math
+import os
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from .analyze import AnalysisResult
+
+PALETTE = ["#1f77b4", "#d62728", "#2ca02c", "#ff7f0e",
+ "#9467bd", "#8c564b", "#17becf"]
+
+
+def _esc(s: object) -> str:
+ return html.escape(str(s), quote=True)
+
+
+def _finite(v: float) -> bool:
+ return isinstance(v, (int, float)) and math.isfinite(v)
+
+
+def _nice_ticks(lo: float, hi: float, count: int = 5) -> List[float]:
+ if not (_finite(lo) and _finite(hi)) or hi <= lo:
+ return [lo]
+ span = hi - lo
+ raw = span / max(count, 1)
+ mag = 10 ** math.floor(math.log10(raw))
+ for m in (1, 2, 2.5, 5, 10):
+ step = m * mag
+ if span / step <= count + 1:
+ break
+ start = math.floor(lo / step) * step
+ ticks = []
+ v = start
+ while v <= hi + step * 0.5:
+ if v >= lo - step * 0.5:
+ ticks.append(v)
+ v += step
+ return ticks
+
+
+def _fmt(v: float) -> str:
+ if not _finite(v):
+ return ""
+ if v == 0:
+ return "0"
+ a = abs(v)
+ if a >= 1000 or a < 0.001:
+ return f"{v:.2e}"
+ if a >= 10:
+ return f"{v:.1f}"
+ if a >= 1:
+ return f"{v:.2f}"
+ return f"{v:.3f}"
+
+
+def _downsample(xs: Sequence[float], ys: Sequence[float], limit: int = 1500):
+ n = len(xs)
+ if n <= limit:
+ return xs, ys
+ step = max(1, n // limit)
+ idx = list(range(0, n, step))
+ if idx[-1] != n - 1:
+ idx.append(n - 1)
+ return [xs[i] for i in idx], [ys[i] for i in idx]
+
+
+def _canvas(body: List[str], w: int, h: int, title: str) -> str:
+ return "\n".join([
+ f'",
+ ]) + "\n"
+
+
+def _write(path: str, text: str) -> str:
+ os.makedirs(os.path.dirname(os.path.abspath(path)), exist_ok=True)
+ with open(path, "w", encoding="utf-8") as f:
+ f.write(text)
+ return path
+
+
+def _axes(body: List[str], x0: float, y0: float, x1: float, y1: float,
+ xlo: float, xhi: float, ylo: float, yhi: float,
+ xlabel: str, ylabel: str) -> None:
+ body.append(f'')
+ for v in _nice_ticks(xlo, xhi):
+ px = x0 + (v - xlo) / (xhi - xlo) * (x1 - x0)
+ body.append(f'')
+ body.append(f'{_fmt(v)}')
+ for v in _nice_ticks(ylo, yhi):
+ py = y1 - (v - ylo) / (yhi - ylo) * (y1 - y0)
+ body.append(f'')
+ body.append(f'{_fmt(v)}')
+ body.append(f'{_esc(xlabel)}')
+ body.append(f'{_esc(ylabel)}')
+
+
+def _legend(body: List[str], labels: Sequence[str], x0: float, y0: float,
+ colors: Optional[Sequence[str]] = None,
+ dashed: Optional[Sequence[bool]] = None) -> None:
+ x = x0
+ for k, lab in enumerate(labels):
+ c = (colors or PALETTE)[k % len(colors or PALETTE)]
+ dash = " stroke-dasharray=\"6 4\"" if dashed and dashed[k] else ""
+ body.append(f'')
+ body.append(f'{_esc(lab)}')
+ x += 27 + 9 * len(str(lab)) + 18
+
+
+# ---------------------------------------------------------------------------
+# 图元
+# ---------------------------------------------------------------------------
+
+def svg_line(path: str, series: Sequence[Tuple[str, Sequence[float], Sequence[float]]],
+ title: str, xlabel: str = "t (s)", ylabel: str = "",
+ width: int = 960, height: int = 520,
+ y_zero: bool = False,
+ colors: Optional[Sequence[str]] = None,
+ dashed: Optional[Sequence[bool]] = None) -> str:
+ """series = [(label, xs, ys), ...]; 自动取所有序列的并集范围。"""
+ clean = []
+ for lab, xs, ys in series:
+ pts = [(x, y) for x, y in zip(xs, ys) if _finite(x) and _finite(y)]
+ if pts:
+ clean.append((lab, pts))
+ if not clean:
+ return _write(path, _canvas([], width, height, title))
+ xlo = min(x for _, p in clean for x, _ in p)
+ xhi = max(x for _, p in clean for x, _ in p)
+ if xhi - xlo < 1e-12:
+ xhi = xlo + 1.0
+ ylo = min(min(y for _, y in p) for _, p in clean)
+ yhi = max(max(y for _, y in p) for _, p in clean)
+ if y_zero:
+ ylo, yhi = min(ylo, 0.0), max(yhi, 0.0)
+ if yhi - ylo < 1e-9:
+ ylo, yhi = ylo - 1.0, yhi + 1.0
+ pad = 0.05 * (yhi - ylo)
+ ylo, yhi = ylo - pad, yhi + pad
+
+ x0, y0, x1, y1 = 78, 52, width - 24, height - 58
+ body: List[str] = []
+ _axes(body, x0, y0, x1, y1, xlo, xhi, ylo, yhi, xlabel, ylabel)
+ if ylo <= 0.0 <= yhi:
+ py = y1 - (0.0 - ylo) / (yhi - ylo) * (y1 - y0)
+ body.append(f'')
+ for k, (lab, pts) in enumerate(clean):
+ c = (colors or PALETTE)[k % len(colors or PALETTE)]
+ d = " stroke-dasharray=\"6 4\"" if dashed and k < len(dashed) and dashed[k] else ""
+ pts = _downsample([p[0] for p in pts], [p[1] for p in pts])
+ dpath = " ".join(
+ f'{"M" if i == 0 else "L"}{x0 + (x - xlo) / (xhi - xlo) * (x1 - x0):.1f},'
+ f'{y1 - (y - ylo) / (yhi - ylo) * (y1 - y0):.1f}'
+ for i, (x, y) in enumerate(zip(*pts)))
+ body.append(f'')
+ _legend(body, [lab for lab, _ in clean], x0, y0 - 12, colors, dashed)
+ return _write(path, _canvas(body, width, height, title))
+
+
+def svg_bars(path: str, categories: Sequence[str],
+ series: Sequence[Tuple[str, Sequence[float]]], title: str,
+ ylabel: str = "", width: int = 900, height: int = 480,
+ colors: Optional[Sequence[str]] = None) -> str:
+ vals = [v for _, vs in series for v in vs if _finite(v)]
+ if not vals or not categories:
+ return _write(path, _canvas([], width, height, title))
+ ylo, yhi = min(vals + [0.0]), max(vals + [0.0])
+ if yhi - ylo < 1e-9:
+ yhi += 1.0
+ pad = 0.08 * (yhi - ylo)
+ ylo, yhi = ylo - pad, yhi + pad
+ x0, y0, x1, y1 = 78, 52, width - 24, height - 58
+ body: List[str] = []
+ _axes(body, x0, y0, x1, y1, 0.0, float(len(categories)), ylo, yhi, "", ylabel)
+ group_w = (x1 - x0) / len(categories)
+ bar_w = group_w * 0.75 / max(len(series), 1)
+ for ci, cat in enumerate(categories):
+ gx = x0 + ci * group_w + group_w * 0.5
+ body.append(f'{_esc(cat)}')
+ for si, (lab, vs) in enumerate(series):
+ v = vs[ci] if ci < len(vs) else float("nan")
+ if not _finite(v):
+ continue
+ bx = gx - (len(series) * bar_w) / 2 + si * bar_w
+ py = y1 - (v - ylo) / (yhi - ylo) * (y1 - y0)
+ py0 = y1 - (0.0 - ylo) / (yhi - ylo) * (y1 - y0)
+ top, hgt = min(py, py0), abs(py0 - py)
+ c = (colors or PALETTE)[si % len(colors or PALETTE)]
+ body.append(f'')
+ _legend(body, [lab for lab, _ in series], x0, y0 - 12, colors)
+ return _write(path, _canvas(body, width, height, title))
+
+
+def svg_scatter(path: str,
+ groups: Sequence[Tuple[str, Sequence[float], Sequence[float]]],
+ title: str, xlabel: str = "", ylabel: str = "",
+ width: int = 720, height: int = 680) -> str:
+ pts_all = [(x, y) for _, xs, ys in groups for x, y in zip(xs, ys)
+ if _finite(x) and _finite(y)]
+ if not pts_all:
+ return _write(path, _canvas([], width, height, title))
+ lo = min(min(x, y) for x, y in pts_all)
+ hi = max(max(x, y) for x, y in pts_all)
+ if hi - lo < 1e-9:
+ hi += 1.0
+ pad = 0.04 * (hi - lo)
+ lo, hi = lo - pad, hi + pad
+ x0, y0, x1, y1 = 78, 52, width - 24, height - 58
+ body: List[str] = []
+ _axes(body, x0, y0, x1, y1, lo, hi, lo, hi, xlabel or "model (N·m)",
+ ylabel or "measured (N·m)")
+ body.append(f'')
+ for k, (lab, xs, ys) in enumerate(groups):
+ c = PALETTE[k % len(PALETTE)]
+ # 散点限流, 避免 5k+ 点时 SVG 体积膨胀 (视觉上看不出差别)
+ step = max(1, len(xs) // 800)
+ for x, y in list(zip(xs, ys))[::step]:
+ if not (_finite(x) and _finite(y)):
+ continue
+ px = x0 + (x - lo) / (hi - lo) * (x1 - x0)
+ py = y1 - (y - lo) / (hi - lo) * (y1 - y0)
+ body.append(f'')
+ _legend(body, [g[0] for g in groups], x0, y0 - 12)
+ return _write(path, _canvas(body, width, height, title))
+
+
+def svg_hist(path: str, values: Sequence[float], title: str,
+ xlabel: str = "", bins: int = 30,
+ width: int = 900, height: int = 460) -> str:
+ vals = sorted(v for v in values if _finite(v))
+ if len(vals) < 2:
+ return _write(path, _canvas([], width, height, title))
+ lo, hi = vals[0], vals[-1]
+ if hi - lo < 1e-12:
+ lo, hi = lo - 0.5, hi + 0.5
+ step = (hi - lo) / bins
+ counts = [0] * bins
+ for v in vals:
+ k = min(bins - 1, int((v - lo) / step))
+ counts[k] += 1
+ yhi = max(counts) * 1.08 if counts else 1
+ x0, y0, x1, y1 = 78, 52, width - 24, height - 58
+ body: List[str] = []
+ _axes(body, x0, y0, x1, y1, lo, hi, 0.0, yhi, xlabel or "residual (N·m)", "count")
+ bw = (x1 - x0) / bins
+ for k, c in enumerate(counts):
+ if c == 0:
+ continue
+ hgt = c / yhi * (y1 - y0)
+ body.append(f'')
+ if lo <= 0.0 <= hi:
+ px = x0 + (0.0 - lo) / (hi - lo) * (x1 - x0)
+ body.append(f'')
+ return _write(path, _canvas(body, width, height, title))
+
+
+# ---------------------------------------------------------------------------
+# 分析结果 -> 一整套图
+# ---------------------------------------------------------------------------
+
+def generate_all(result: AnalysisResult, outdir: str,
+ prefix: str = "") -> Dict[str, str]:
+ os.makedirs(outdir, exist_ok=True)
+ n = result.n
+ t = result.t
+ paths: Dict[str, str] = {}
+
+ def name(s: str) -> str:
+ return os.path.join(outdir, f"{prefix}{s}.svg")
+
+ for j in range(n):
+ paths[f"j{j + 1}_tau"] = svg_line(
+ name(f"joint{j + 1}_tau"),
+ [("实测 τ", t, result.tau_meas[j]),
+ ("模型 τ (raw)", t, result.tau_model[j]),
+ ("模型 τ (scaled)", t, result.tau_scaled[j])],
+ f"关节 J{j + 1} · 实测 vs 逆动力学", "t (s)", "τ (N·m)",
+ colors=["#111", "#1f77b4", "#d62728"],
+ dashed=[False, False, True])
+ paths[f"j{j + 1}_res"] = svg_line(
+ name(f"joint{j + 1}_residual"),
+ [("residual (raw)", t,
+ [result.tau_meas[j][i] - result.tau_model[j][i]
+ for i in range(result.n_samples)]),
+ ("residual (scaled)", t,
+ [result.tau_meas[j][i] - result.tau_scaled[j][i]
+ for i in range(result.n_samples)])],
+ f"关节 J{j + 1} · 力矩残差", "t (s)", "τ_meas − τ_model (N·m)",
+ y_zero=True, colors=["#1f77b4", "#d62728"], dashed=[False, True])
+ paths[f"j{j + 1}_hist"] = svg_hist(
+ name(f"joint{j + 1}_residual_hist"),
+ [result.tau_meas[j][i] - result.tau_model[j][i]
+ for i in range(result.n_samples)],
+ f"关节 J{j + 1} · 残差分布",
+ xlabel="τ_meas − τ_model (N·m)")
+
+ # 速度分层柱状
+ key = f"J{j + 1}"
+ strata = result.summary["strata"]["speed"].get(key, {})
+ if strata:
+ cats = list(strata.keys())
+ vals = [strata[c]["rmse"] for c in cats]
+ paths[f"j{j + 1}_speed"] = svg_bars(
+ name(f"joint{j + 1}_rmse_by_speed"), cats,
+ [("RMSE", vals)], f"关节 J{j + 1} · 按速度分层 RMSE",
+ ylabel="RMSE (N·m)")
+
+ labels = [f"J{j + 1}" for j in range(n)]
+ raw = result.summary["metrics"]["raw"]["per_joint"]
+ scaled = result.summary["metrics"]["scaled"]["per_joint"]
+ paths["rmse"] = svg_bars(
+ name("rmse_by_joint"), labels,
+ [("raw", [raw[l]["rmse"] for l in labels]),
+ ("scaled", [scaled[l]["rmse"] for l in labels])],
+ "逐轴 RMSE (raw = 物理量, scaled = 固件前馈口径)", ylabel="RMSE (N·m)")
+ paths["bias"] = svg_bars(
+ name("bias_by_joint"), labels,
+ [("bias", [raw[l]["bias"] for l in labels])],
+ "逐轴平均残差 (bias = mean(τ_meas − τ_model))",
+ ylabel="bias (N·m)", colors=["#ff7f0e"])
+
+ paths["parity"] = svg_scatter(
+ name("parity_measured_vs_model"),
+ [(f"J{j + 1}", result.tau_model[j], result.tau_meas[j]) for j in range(n)],
+ "实测 vs 模型 (所有关节)")
+
+ delay = result.summary["delay"]
+ series = []
+ for j in range(n):
+ key = f"J{j + 1}"
+ if key not in delay:
+ continue
+ curve = delay[key].get("curve") or []
+ series.append((key, [c[0] / result.fs * 1000.0 for c in curve],
+ [c[1] for c in curve]))
+ if series:
+ paths["delay"] = svg_line(
+ name("delay_scan"),
+ series, "延迟扫描 · RMSE vs 实测时移", "lag (ms)", "RMSE (N·m)")
+
+ # 归一化残差汇总 (各轴除以自身 RMSE, 便于同图比较)
+ pooled = []
+ for j in range(n):
+ r = raw[f"J{j + 1}"]["rmse"]
+ if not r or r <= 1e-12:
+ continue
+ pooled += [(result.tau_meas[j][i] - result.tau_model[j][i]) / r
+ for i in range(result.n_samples)]
+ if pooled:
+ paths["hist"] = svg_hist(name("residual_normalized_hist"), pooled,
+ "所有关节 · 归一化残差 (residual / RMSE_j)",
+ xlabel="residual / RMSE_j")
+ return paths
diff --git a/tools/tau_eval/report.py b/tools/tau_eval/report.py
new file mode 100644
index 0000000..71dfcbb
--- /dev/null
+++ b/tools/tau_eval/report.py
@@ -0,0 +1,219 @@
+# -*- coding: utf-8 -*-
+"""把分析结果写成 Markdown 报告 —— 指标表、分层、延迟、归因与图索引。"""
+from __future__ import annotations
+
+import os
+from typing import Dict, List, Optional
+
+from .analyze import AnalysisResult
+
+
+def _f(v, nd: int = 4) -> str:
+ if v is None:
+ return "—"
+ if isinstance(v, float):
+ if v != v:
+ return "—"
+ return f"{v:.{nd}f}"
+ return str(v)
+
+
+def _table(headers: List[str], rows: List[List[str]]) -> str:
+ out = ["| " + " | ".join(headers) + " |",
+ "|" + "|".join(["---"] * len(headers)) + "|"]
+ for r in rows:
+ out.append("| " + " | ".join(r) + " |")
+ return "\n".join(out)
+
+
+def _figs(paths: Dict[str, str], outdir: str, keys: List[str]) -> List[str]:
+ lines = []
+ for k in keys:
+ p = paths.get(k)
+ if not p:
+ continue
+ rel = os.path.relpath(p, outdir)
+ title = os.path.splitext(os.path.basename(p))[0]
+ lines.append(f"")
+ lines.append("")
+ return lines
+
+
+def write_report(result: AnalysisResult, figure_paths: Optional[Dict[str, str]] = None,
+ outdir: str = ".", filename: str = "report.md",
+ command: Optional[str] = None) -> str:
+ s = result.summary
+ figs = figure_paths or {}
+ n = result.n
+ raw = s["metrics"]["raw"]
+ scaled = s["metrics"]["scaled"]
+ agg = raw["aggregate"]
+ meta = result.snapshot
+ opts = s["options"]
+
+ lines: List[str] = []
+ lines.append("# 电流环力矩估计精度评估报告")
+ lines.append("")
+ lines.append(f"- 场景: **{s.get('scenario', '—')}**")
+ lines.append(f"- 采样: {s['n_samples']} 帧 @ {s['fs_hz']:.2f} Hz, "
+ f"分析窗 {s['t_span_s']:.2f} s")
+ lines.append(f"- 固件: `{s['model'].get('firmware') or '—'}`; "
+ f"几何指纹: `{s['model'].get('geometry_sha16')}`")
+ lines.append(f"- 重力向量: {s['model'].get('gravity')} "
+ f"(台架若为 +Y 请用 `--gravity 0,9.81,0`)")
+ lines.append(f"- payload: {s['model'].get('payload')}")
+ lines.append("")
+
+ lines.append("## 结论摘要")
+ lines.append("")
+ lines.append(f"- 逐轴平均 RMSE (raw): **{_f(agg.get('mean_rmse'))} N·m**; "
+ f"最大 {_f(agg.get('max_rmse'))} N·m "
+ f"({agg.get('worst_joint')})")
+ lines.append(f"- 逐轴平均 |bias|: {_f(agg.get('mean_abs_bias'))} N·m; "
+ f"平均相对 RMSE: {_f(agg.get('mean_rel_rmse'), 3)}")
+ lines.append(f"- 固件前馈口径 (scaled) 平均 RMSE: "
+ f"**{_f(scaled['aggregate'].get('mean_rmse'))} N·m**")
+ lines.append("")
+ diags = s.get("diagnosis") or []
+ if diags:
+ lines.append("主要误差来源 (按证据排序, 不是最终判决):")
+ lines.append("")
+ for d in diags:
+ lines.append(f"- **{d['source']}** — {d['evidence']}")
+ lines.append(f" - 建议: {d['suggestion']}")
+ lines.append("")
+
+ lines.append("## 方法与口径")
+ lines.append("")
+ lines.append(f"- 模型: 固件在线读回的刚体参数 + jm + payload 复合 "
+ f"(`model.json`, 几何来自 `dyn_model_data.h`)。")
+ lines.append("- 逆动力学: 世界系牛顿-欧拉 RNEA (固件 `rnea_core_m` 的逐行移植), "
+ "`τ = M·ddq + C·dq + G + jm·ddq + friction`。")
+ lines.append(f"- 滤波: 零相位 Butterworth (filtfilt), "
+ f"q/dq {opts['cutoff_dq_hz']} Hz, ddq {opts['cutoff_ddq_hz']} Hz, "
+ f"τ {opts['cutoff_tau_hz']} Hz; ddq 由 dq 中心差分后再低通。")
+ lines.append(f"- 边沿裁剪: {_f(opts.get('trim_s'), 3)} s "
+ f"(默认 `max(2/cutoff_ddq, 0.1)`, 上限 span/5), "
+ f"避开零相位滤波两端瞬态。")
+ lines.append("- 残差定义: `τ_meas − τ_model`, 关节侧, N·m; "
+ "`raw` 为物理量, `scaled` 按固件 gravity_scale/inertia_scale 口径。")
+ if s.get("resampled"):
+ lines.append("- ⚠ 原始到达间隔不均匀, 分析前已重采样到均匀网格。")
+ lines.append("")
+
+ lines.append("## 逐轴指标 (raw)")
+ lines.append("")
+ rows = []
+ for j in range(n):
+ k = f"J{j + 1}"
+ m = raw["per_joint"][k]
+ ms = scaled["per_joint"][k]
+ rows.append([k, _f(m["rmse"]), _f(m["mae"]), _f(m["bias"]),
+ _f(m["peak_abs"]), _f(m["rel_rmse"], 3),
+ _f(m["r2"], 3), _f(m["corr"], 3), _f(ms["rmse"])])
+ lines.append(_table(
+ ["关节", "RMSE", "MAE", "bias", "peak", "rel_RMSE", "R²", "corr",
+ "RMSE (scaled)"], rows))
+ lines.append("")
+
+ lines.append("## 按速度分层 (raw RMSE, N·m)")
+ lines.append("")
+ sp = s["strata"]["speed"]
+ cats = []
+ for v in sp.values():
+ for c in v:
+ if c not in cats:
+ cats.append(c)
+ cats.sort()
+ rows = []
+ for j in range(n):
+ k = f"J{j + 1}"
+ row = [k]
+ for c in cats:
+ row.append(_f(sp.get(k, {}).get(c, {}).get("rmse")))
+ rows.append(row)
+ if cats:
+ lines.append(_table(["关节"] + cats, rows))
+ lines.append("")
+
+ lines.append("## 延迟扫描")
+ lines.append("")
+ lines.append("lag > 0 = 实测相对模型滞后; improvement = 时移后可再降的 RMSE 比例。")
+ lines.append("")
+ rows = []
+ for j in range(n):
+ d = s["delay"].get(f"J{j + 1}")
+ if not d:
+ continue
+ rows.append([f"J{j + 1}", _f(d["lag_ms"], 2), _f(d["rmse_at_zero"]),
+ _f(d["rmse_at_best"]), f"{d['improvement'] * 100:.1f}%"])
+ if rows:
+ lines.append(_table(["关节", "最优 lag (ms)", "零时移 RMSE",
+ "最优 RMSE", "可改善"], rows))
+ lines.append("")
+
+ lines.append("## 残差与其他项的相关")
+ lines.append("")
+ rows = []
+ for j in range(n):
+ r = s["residual"].get(f"J{j + 1}", {})
+ c = r.get("correlation", {})
+ rows.append([f"J{j + 1}", _f(r.get("static_bias")),
+ _f(c.get("gravity"), 2), _f(c.get("inertia"), 2),
+ _f(c.get("friction"), 2), _f(c.get("sign_dq"), 2),
+ _f(c.get("ddq"), 2)])
+ lines.append(_table(["关节", "静止 bias", "r(G)", "r(I)", "r(F)",
+ "r(sign dq)", "r(ddq)"], rows))
+ lines.append("")
+
+ lines.append("## 各理论项 RMS (N·m)")
+ lines.append("")
+ cr = s.get("components_rms", {})
+ rows = []
+ for j in range(n):
+ rows.append([f"J{j + 1}"] + [_f(cr.get(name, [None] * n)[j])
+ for name in ("gravity", "coriolis", "inertia",
+ "jm", "friction")])
+ lines.append(_table(["关节", "G", "C", "I", "jm·ddq", "friction"], rows))
+ lines.append("")
+
+ lines.append("## 图")
+ lines.append("")
+ keys = []
+ for j in range(n):
+ keys += [f"j{j + 1}_tau", f"j{j + 1}_res"]
+ keys += ["rmse", "bias", "parity", "delay", "hist"]
+ fig_lines = _figs(figs, outdir, keys)
+ if fig_lines:
+ lines += fig_lines
+ else:
+ lines.append("(未生成图)")
+ lines.append("")
+
+ lines.append("## 复现")
+ lines.append("")
+ if command:
+ lines.append("```")
+ lines.append(command)
+ lines.append("```")
+ else:
+ lines.append("```")
+ lines.append("python3 tools/tau_eval/run.py analyze <会话目录>")
+ lines.append("```")
+ lines.append("")
+ lines.append("## 限制")
+ lines.append("")
+ lines.append("- τ 是**电流环估计值**, 不是关节力矩传感器; 电流环标定误差会直接进入结论。")
+ lines.append("- 固件模型是辨识回归量: link mass 多为 1、惯量含负值、M 不正定; "
+ "只用于前向计算, **不可求 M⁻¹**。")
+ lines.append("- `get_gravity` 的静态核查只在末段静止时有效; 运动段不冒充静态。")
+ lines.append("- 环境温度读不到, 报告里的温度是电机绕组/MOS 温度; "
+ "ambient 只能用 `--ambient-c` 手工记录。")
+ lines.append("- 结论的适用范围是本组构型与速度; 外推到限位附近/高速段需另采数据。")
+ lines.append("")
+
+ path = os.path.join(outdir, filename)
+ os.makedirs(outdir, exist_ok=True)
+ with open(path, "w", encoding="utf-8") as f:
+ f.write("\n".join(lines))
+ return path
diff --git a/tools/tau_eval/rnea.py b/tools/tau_eval/rnea.py
new file mode 100644
index 0000000..6910e70
--- /dev/null
+++ b/tools/tau_eval/rnea.py
@@ -0,0 +1,237 @@
+# -*- coding: utf-8 -*-
+"""逆动力学 —— 固件 `dyn.c::rnea_core_m` 的逐行 Python 移植。
+
+世界系牛顿-欧拉,9 刚体串行链,重力通过 base 线加速度 −g 注入
+(与 pinocchio `rnea` 同符号)。输出是**关节侧**力矩:
+
+ RNEA(q, dq, ddq) = M(q)·ddq + C(q,dq)·dq + G(q)
+
+固件的 `dyn_rnea` 额外加 `jm·ddq`;这里保持内核纯净,由 `theory_torques()`
+按固件口径把 jm 单列,方便报告拆分误差来源。
+
+⚠ M 不保证正定 (模型是辨识回归量),本模块不提供求逆。
+"""
+from __future__ import annotations
+
+import math
+from typing import Dict, List, Sequence
+
+from .model import (DYN_AXIS, DYN_FIX_RPY, DYN_FIX_XYZ, DYN_JTYPE, DYN_PARENT,
+ N_JOINT, Body, ModelSnapshot, rpy_to_matrix,
+ sym6_to_matrix)
+
+
+def _matmul(a: Sequence[float], b: Sequence[float]) -> List[float]:
+ return [
+ a[0] * b[0] + a[1] * b[3] + a[2] * b[6],
+ a[0] * b[1] + a[1] * b[4] + a[2] * b[7],
+ a[0] * b[2] + a[1] * b[5] + a[2] * b[8],
+ a[3] * b[0] + a[4] * b[3] + a[5] * b[6],
+ a[3] * b[1] + a[4] * b[4] + a[5] * b[7],
+ a[3] * b[2] + a[4] * b[5] + a[5] * b[8],
+ a[6] * b[0] + a[7] * b[3] + a[8] * b[6],
+ a[6] * b[1] + a[7] * b[4] + a[8] * b[7],
+ a[6] * b[2] + a[7] * b[5] + a[8] * b[8],
+ ]
+
+
+def _matmul_t(a: Sequence[float], b: Sequence[float]) -> List[float]:
+ """a · bᵀ (行主序 3x3)。"""
+ return [
+ a[0] * b[0] + a[1] * b[1] + a[2] * b[2],
+ a[0] * b[3] + a[1] * b[4] + a[2] * b[5],
+ a[0] * b[6] + a[1] * b[7] + a[2] * b[8],
+ a[3] * b[0] + a[4] * b[1] + a[5] * b[2],
+ a[3] * b[3] + a[4] * b[4] + a[5] * b[5],
+ a[3] * b[6] + a[4] * b[7] + a[5] * b[8],
+ a[6] * b[0] + a[7] * b[1] + a[8] * b[2],
+ a[6] * b[3] + a[7] * b[4] + a[8] * b[5],
+ a[6] * b[6] + a[7] * b[7] + a[8] * b[8],
+ ]
+
+
+def _matvec(m: Sequence[float], v: Sequence[float]) -> List[float]:
+ return [m[0] * v[0] + m[1] * v[1] + m[2] * v[2],
+ m[3] * v[0] + m[4] * v[1] + m[5] * v[2],
+ m[6] * v[0] + m[7] * v[1] + m[8] * v[2]]
+
+
+def _cross(a: Sequence[float], b: Sequence[float]) -> List[float]:
+ return [a[1] * b[2] - a[2] * b[1],
+ a[2] * b[0] - a[0] * b[2],
+ a[0] * b[1] - a[1] * b[0]]
+
+
+def _zrot(th: float) -> List[float]:
+ c, s = math.cos(th), math.sin(th)
+ return [c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0]
+
+
+def _flatten(m: Sequence[Sequence[float]]) -> List[float]:
+ return [m[0][0], m[0][1], m[0][2],
+ m[1][0], m[1][1], m[1][2],
+ m[2][0], m[2][1], m[2][2]]
+
+
+#: 固定旋转预算成行主序 9 元组 —— 热路径上每次 RNEA 都要用。
+_FIX_IO = [tuple(_flatten(rpy_to_matrix(rpy))) for rpy in DYN_FIX_RPY]
+
+
+def _sym6_flat(i6: Sequence[float]) -> List[float]:
+ return _flatten(sym6_to_matrix(i6))
+
+
+def rnea_core(q: Sequence[float], dq: Sequence[float], ddq: Sequence[float],
+ bodies: Sequence[Body], gravity: Sequence[float] = (0.0, 0.0, -9.81),
+ gravity_on: bool = True) -> List[float]:
+ """完整 RNEA(不含 jm)。输入长度必须为 7。"""
+ if len(q) != N_JOINT or len(dq) != N_JOINT or len(ddq) != N_JOINT:
+ raise ValueError(f"q/dq/ddq 都需要 {N_JOINT} 个")
+ if len(bodies) != 9:
+ raise ValueError("bodies 需要 9 个刚体")
+
+ Rw: List[List[float]] = [[0.0] * 9 for _ in range(9)]
+ p: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ u: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ w: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ al: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ alin: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ f: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+ npi: List[List[float]] = [[0.0, 0.0, 0.0] for _ in range(9)]
+
+ Rw[0] = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+ if gravity_on:
+ alin[0] = [-float(gravity[0]), -float(gravity[1]), -float(gravity[2])]
+
+ for i in range(1, 9):
+ pa = DYN_PARENT[i]
+ rev = DYN_JTYPE[i] == 0
+ sgn = 1.0 if DYN_AXIS[i][2] >= 0.0 else -1.0
+ rwpf = _matmul(Rw[pa], _FIX_IO[i])
+ if rev:
+ Rw[i] = _matmul(rwpf, _zrot(sgn * float(q[i - 1])))
+ else:
+ Rw[i] = rwpf
+ tmp = _matvec(Rw[pa], DYN_FIX_XYZ[i])
+ p[i] = [p[pa][0] + tmp[0], p[pa][1] + tmp[1], p[pa][2] + tmp[2]]
+
+ if rev:
+ u[i] = _matvec(Rw[i], [0.0, 0.0, sgn])
+ dqi = float(dq[i - 1])
+ uq = [u[i][0] * dqi, u[i][1] * dqi, u[i][2] * dqi]
+ w[i] = [w[pa][0] + uq[0], w[pa][1] + uq[1], w[pa][2] + uq[2]]
+ cuq = _cross(w[pa], uq)
+ ai = float(ddq[i - 1])
+ al[i] = [al[pa][0] + u[i][0] * ai + cuq[0],
+ al[pa][1] + u[i][1] * ai + cuq[1],
+ al[pa][2] + u[i][2] * ai + cuq[2]]
+ else:
+ w[i] = list(w[pa])
+ al[i] = list(al[pa])
+
+ r = [p[i][0] - p[pa][0], p[i][1] - p[pa][1], p[i][2] - p[pa][2]]
+ ar = _cross(al[pa], r)
+ wr = _cross(w[pa], r)
+ wwx = _cross(w[pa], wr)
+ alin[i] = [alin[pa][0] + ar[0] + wwx[0],
+ alin[pa][1] + ar[1] + wwx[1],
+ alin[pa][2] + ar[2] + wwx[2]]
+
+ body = bodies[i]
+ comw_tmp = _matvec(Rw[i], body.com)
+ comw = [p[i][0] + comw_tmp[0], p[i][1] + comw_tmp[1], p[i][2] + comw_tmp[2]]
+ ro = [comw[0] - p[i][0], comw[1] - p[i][1], comw[2] - p[i][2]]
+ a_cross = _cross(al[i], ro)
+ wx = _cross(w[i], ro)
+ wwx2 = _cross(w[i], wx)
+ ac = [alin[i][0] + a_cross[0] + wwx2[0],
+ alin[i][1] + a_cross[1] + wwx2[1],
+ alin[i][2] + a_cross[2] + wwx2[2]]
+
+ f[i] = [body.mass * ac[0], body.mass * ac[1], body.mass * ac[2]]
+
+ It = _sym6_flat(body.inertia)
+ Iw = _matmul_t(_matmul(Rw[i], It), Rw[i])
+ Iw_al = _matvec(Iw, al[i])
+ Iw_w = _matvec(Iw, w[i])
+ c3 = _cross(w[i], Iw_w)
+ n = [Iw_al[0] + c3[0], Iw_al[1] + c3[1], Iw_al[2] + c3[2]]
+ rof = _cross(ro, f[i])
+ npi[i] = [n[0] + rof[0], n[1] + rof[1], n[2] + rof[2]]
+
+ tau = [0.0] * N_JOINT
+ Ftot = [0.0, 0.0, 0.0]
+ Macc = [0.0, 0.0, 0.0]
+ for i in range(8, 0, -1):
+ if i < 8:
+ off = [p[i + 1][0] - p[i][0], p[i + 1][1] - p[i][1], p[i + 1][2] - p[i][2]]
+ d1 = _cross(off, Ftot)
+ Macc = [Macc[0] + d1[0], Macc[1] + d1[1], Macc[2] + d1[2]]
+ mi = [npi[i][0] + Macc[0], npi[i][1] + Macc[1], npi[i][2] + Macc[2]]
+ Ftot = [f[i][0] + Ftot[0], f[i][1] + Ftot[1], f[i][2] + Ftot[2]]
+ if DYN_JTYPE[i] == 0:
+ tau[i - 1] = u[i][0] * mi[0] + u[i][1] * mi[1] + u[i][2] * mi[2]
+ Macc = mi
+ return tau
+
+
+def gravity_torque(q: Sequence[float], bodies: Sequence[Body],
+ gravity: Sequence[float] = (0.0, 0.0, -9.81)) -> List[float]:
+ z = [0.0] * N_JOINT
+ return rnea_core(q, z, z, bodies, gravity, gravity_on=True)
+
+
+def coriolis_torque(q: Sequence[float], dq: Sequence[float],
+ bodies: Sequence[Body],
+ gravity: Sequence[float] = (0.0, 0.0, -9.81)) -> List[float]:
+ z = [0.0] * N_JOINT
+ full = rnea_core(q, dq, z, bodies, gravity, gravity_on=True)
+ g = rnea_core(q, z, z, bodies, gravity, gravity_on=True)
+ return [full[i] - g[i] for i in range(N_JOINT)]
+
+
+def inertia_torque(q: Sequence[float], ddq: Sequence[float],
+ bodies: Sequence[Body]) -> List[float]:
+ z = [0.0] * N_JOINT
+ return rnea_core(q, z, ddq, bodies, (0.0, 0.0, 0.0), gravity_on=False)
+
+
+def mass_matrix(q: Sequence[float], bodies: Sequence[Body]) -> List[List[float]]:
+ """M(q),含 jm?不 —— 这里只返回刚体项;jm 由调用方加到对角。"""
+ z = [0.0] * N_JOINT
+ M = [[0.0] * N_JOINT for _ in range(N_JOINT)]
+ for j in range(N_JOINT):
+ e = [0.0] * N_JOINT
+ e[j] = 1.0
+ col = rnea_core(q, z, e, bodies, (0.0, 0.0, 0.0), gravity_on=False)
+ for i in range(N_JOINT):
+ M[i][j] = col[i]
+ for i in range(N_JOINT):
+ for j in range(i + 1, N_JOINT):
+ v = 0.5 * (M[i][j] + M[j][i])
+ M[i][j] = v
+ M[j][i] = v
+ return M
+
+
+def theory_torques(snapshot: ModelSnapshot, q: Sequence[float],
+ dq: Sequence[float], ddq: Sequence[float],
+ apply_scales: bool = False) -> Dict[str, List[float]]:
+ """逐项理论力矩。
+
+ 返回 `gravity/coriolis/inertia/jm/friction/total/total_scaled`。
+ `total` 是物理量 (M·ddq + C·dq + G + jm·ddq + friction),不乘标定系数;
+ `total_scaled` 按固件前馈口径乘 gravity_scale / inertia_scale。
+ """
+ bodies = snapshot.effective_bodies()
+ G = gravity_torque(q, bodies, snapshot.gravity)
+ C = coriolis_torque(q, dq, bodies, snapshot.gravity)
+ I = inertia_torque(q, ddq, bodies)
+ J = [snapshot.jm[i] * float(ddq[i]) for i in range(N_JOINT)]
+ F = snapshot.friction_torque(dq)
+ total = [G[i] + C[i] + I[i] + J[i] + F[i] for i in range(N_JOINT)]
+ scaled = [snapshot.gravity_scale[i] * G[i]
+ + snapshot.inertia_scale[i] * (C[i] + I[i] + J[i])
+ + F[i] for i in range(N_JOINT)]
+ return {"gravity": G, "coriolis": C, "inertia": I, "jm": J,
+ "friction": F, "total": total, "total_scaled": scaled}
diff --git a/tools/tau_eval/run.py b/tools/tau_eval/run.py
new file mode 100644
index 0000000..ebcfee3
--- /dev/null
+++ b/tools/tau_eval/run.py
@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""tau_eval 启动脚本 —— 从仓库任意目录可直接运行。
+
+ python3 tools/tau_eval/run.py # 默认子命令 run: 只读打印计划
+ python3 tools/tau_eval/run.py --go # 真跑三场景 + 分析 + 出图 + 报告
+ python3 tools/tau_eval/run.py analyze runs/20261029_153000_tau_eval
+
+把 `tools/` 放到 sys.path 后以 `tau_eval` 包导入, 避免依赖当前工作目录。
+"""
+import os
+import sys
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_TOOLS = os.path.dirname(_HERE)
+if _TOOLS not in sys.path:
+ sys.path.insert(0, _TOOLS)
+
+from tau_eval.cli import main # noqa: E402
+
+if __name__ == "__main__":
+ raise SystemExit(main())