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'', + '', + f'{_esc(title)}', + *body, + "", + ]) + "\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"![{title}]({rel})") + 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())