feature_extractor

  1#!/usr/bin/env python3
  2"""
  3feature_extractor.py — swirl-phase feature extraction for the feels cup-content study.
  4
  5Consumes one telemetry CSV from repeatable_grasp.py (single continuous
  6pick-up → swirl → place macro) and outputs the explicit 10-D feature vector z
  7plus its L2-normalized embedding e in S^9, ready for MongoDB Atlas
  8$vectorSearch (index: cup_dynamics_index, 10 dims, cosine).
  9
 1010-D feature vector z:
 11  1. mass_j2        Δτ2 at pre-swirl peak-Z vs INIT tare        (N·m)
 12  2. mass_j3        Δτ3 at pre-swirl peak-Z vs INIT tare        (N·m)
 13  3. swirl_var_j2   variance of detrended τ2 across SWIRL       (N·m²)
 14  4. swirl_var_j3   variance of detrended τ3 across SWIRL       (N·m²)
 15  5. swirl_ptp_j3   peak-to-peak τ3 across SWIRL                (N·m)
 16  6. phase_lag_s    xcorr lag: TCP speed vs τ3 residual (s;
 17                    + = torque lags the commanded motion)
 18  7. rattle_e       spectral energy of τ3 residual in the observable
 19                    high band [1.5 Hz, Nyquist]. NOTE: at 4-5 Hz cloud
 20                    sampling Nyquist ≈ 2.5 Hz, so the true 5-25 Hz rattle
 21                    band is NOT observable — see diagnostics
 22                    ["rattle_band_limited"]. Needs the 100 Hz on-robot
 23                    module (control_service.py) for the real band.
 24  8. grip_comp      std of gripper_pos across SWIRL             (encoder units)
 25  9. lift_dyn_j3    std of τ3 in LIFT_TO_TRAVEL                 (N·m)
 26 10. swirl_speed    mean TCP speed across SWIRL                 (mm/s)
 27
 28Usage:
 29  python feature_extractor.py recordings/recording_20261029_180332_empty_cup.csv
 30"""
 31import argparse
 32import json
 33import os
 34import sys
 35
 36import numpy as np
 37import pandas as pd
 38from scipy.signal import butter, filtfilt
 39
 40JOINTS = list(range(1, 7))
 41SWIRL_PHASE = "SWIRL"
 42FALLBACK_DYN_PHASES = ["LIFT_TO_TRAVEL", "DESCEND_PLACE", "TRAVEL_SETTLING"]
 43FEATURE_NAMES = [
 44    "mass_j2", "mass_j3",
 45    "swirl_var_j2", "swirl_var_j3", "swirl_ptp_j3",
 46    "phase_lag_s", "rattle_e",
 47    "grip_comp", "lift_dyn_j3", "swirl_speed",
 48]
 49
 50# Plain-language meaning of each feature, shown in the dashboard.
 51FEATURE_DESCRIPTIONS = {
 52    "mass_j2": "Static gravity torque offset at shoulder (J2) — payload mass estimate [excluded from matching: tare noise exceeds payload signal]",
 53    "mass_j3": "Static gravity torque offset at elbow (J3) — payload mass estimate [excluded from matching: tare noise exceeds payload signal]",
 54    "swirl_var_j2": "Torque variance at shoulder during swirl — slosh / loose-content energy",
 55    "swirl_var_j3": "Torque variance at elbow during swirl",
 56    "swirl_ptp_j3": "Peak-to-peak torque swing at elbow during swirl — shifting mass",
 57    "phase_lag_s": "Lag between TCP motion and torque response — viscous / fluid damping",
 58    "rattle_e": "High-frequency torque energy — rattle (band-limited by 4-5 Hz sampling)",
 59    "grip_comp": "Gripper compliance change under load — squishy vs rigid contents",
 60    "lift_dyn_j3": "Dynamic torque at elbow during lift — inertial mass cue",
 61    "swirl_speed": "Mean TCP speed during swirl — excitation normalization [excluded from matching: measures the fixed macro's drive, not contents]",
 62}
 63
 64# Per-feature scales so the L2-normalized embedding isn't dominated by the
 65# largest-magnitude raw feature (swirl_speed ~160 mm/s vs variances ~1-5).
 66# Calibrated on swirl-protocol data ONLY (--calibrate --require-swirl);
 67# mixing protocols confounds motion with contents.
 68#
 69# Per-feature weights: 0.0 excludes a feature from the embedding (it stays in
 70# z for display/diagnostics). mass_j2/mass_j3 are proven dead — the audit
 71# found static hold torques identical across contents to 3 decimals, i.e.
 72# payload signal (~1 N-m) is below tare noise (~10 N-m). swirl_speed is a
 73# nuisance by construction: it measures how hard the fixed macro drives,
 74# which is ~constant across contents, so it only compresses cosine
 75# similarities toward 1. The embedding is effectively 7-D (index stays 10-D
 76# with zeros in the excluded dims — no migration needed).
 77FEATURE_WEIGHTS = {
 78    "mass_j2": 0.0,
 79    "mass_j3": 0.0,
 80    "swirl_var_j2": 1.0,
 81    "swirl_var_j3": 1.0,
 82    "swirl_ptp_j3": 1.0,
 83    "phase_lag_s": 1.0,
 84    "rattle_e": 1.0,
 85    "grip_comp": 1.0,
 86    "lift_dyn_j3": 1.0,
 87    "swirl_speed": 0.0,
 88}
 89# Calibrated as the 90th percentile of |z| over the recordings available on
 90# 2026-10-29 (n=8, only one true SWIRL recording). RECALIBRATE on the P0
 91# swirl dataset, then freeze — rescaling invalidates old Atlas signatures.
 92# Regenerate with:  python feature_extractor.py --calibrate
 93FEATURE_SCALES = {
 94    # Calibrated 2026-10-29 on swirl-only train recordings (n=33, 90th pct of |z|).
 95    # The previous values were fallback-protocol calibrated and collapsed every
 96    # swirl embedding to ~0.97 cosine (98%-confidence artifact). Reintroduced
 97    # to the feat branch after the main-branch rollback (was 84080f0).
 98    "mass_j2": 8.978,
 99    "mass_j3": 3.244,
100    "swirl_var_j2": 19.866,
101    "swirl_var_j3": 4.883,
102    "swirl_ptp_j3": 16.479,
103    "phase_lag_s": 1.455,
104    "rattle_e": 179.594,
105    "grip_comp": 0.632,
106    "lift_dyn_j3": 4.141,
107    "swirl_speed": 182.422,
108}
109
110
111# ── signal helpers ──────────────────────────────────────────────────────
112def _fs(df):
113    diffs = df["elapsed_sec"].diff().median()
114    return 1.0 / diffs if diffs and diffs > 0 else 5.0
115
116
117def _lowpass(x, fs, cutoff_hz=0.8, order=2):
118    nyq = fs / 2.0
119    wc = min(cutoff_hz, 0.9 * nyq) / nyq
120    b, a = butter(order, wc, btype="low")
121    padlen = 3 * max(len(a), len(b))
122    if len(x) <= padlen:
123        return x.astype(float)
124    return filtfilt(b, a, x)
125
126
127def _tare_with_qc(df):
128    """Per-joint INIT tare + quality flags (see BASELINE_FINDINGS.md)."""
129    tares, flags = {}, []
130    for j in JOINTS:
131        col = f"tau{j}_nm"
132        init = df.loc[df["phase"] == "INIT", col]
133        tares[j] = init.median() if len(init) else df[col].iloc[:5].median()
134    if len(init) >= 2:
135        d2 = df.loc[df["phase"] == "INIT", "tau2_nm"]
136        d3 = df.loc[df["phase"] == "INIT", "tau3_nm"]
137        if d2.max() - d2.min() > 3.0 or d3.max() - d3.min() > 1.0:
138            flags.append("INIT_DRIFT")
139        q2 = df.loc[df["phase"] == "INIT", "q2_deg"]
140        if q2.max() - q2.min() > 5.0:
141            flags.append("INIT_MOTION")
142    return tares, flags
143
144
145def _tcp_speed(df):
146    t = df["elapsed_sec"].values
147    x = df["tcp_x_mm"].values
148    y = df["tcp_y_mm"].values
149    dt = np.diff(t)
150    dt[dt <= 0] = np.nan
151    return np.concatenate([[0.0], np.hypot(np.diff(x), np.diff(y)) / dt])
152
153
154def _phase_lag(x, y, fs, max_lag_s=2.0):
155    """Lag of max |cross-correlation| between x and y, in seconds."""
156    a = np.asarray(x, dtype=float)
157    b = np.asarray(y, dtype=float)
158    a = a - a.mean()
159    b = b - b.mean()
160    if np.std(a) < 1e-9 or np.std(b) < 1e-9:
161        return 0.0
162    corr = np.correlate(a / (np.std(a) + 1e-12), b / (np.std(b) + 1e-12), mode="full")
163    lags = (np.arange(len(corr)) - (len(a) - 1)) / fs
164    m = np.abs(lags) <= max_lag_s
165    return float(lags[m][np.argmax(np.abs(corr[m]))]) if m.any() else 0.0
166
167
168# ── main extraction ─────────────────────────────────────────────────────
169def extract_features(df):
170    """Compute the 10-D swirl signature and normalized embedding from a recording.
171
172    Reads the SWIRL phase (or falls back to dynamic phases), detrends joint
173    torques against the INIT baseline, and computes the ten physical features
174    (static lift, swirl dynamics, grip/excitation groups).
175
176    Args:
177        df: recording DataFrame with phase, elapsed_sec, tcp_*_mm, tau*_nm columns.
178    Returns:
179        Dict with z (10-D raw feature list), e (L2-normalized embedding list),
180        diagnostics (tare flags, phase source, sample count, sample rate).
181    Raises:
182        ValueError: when required columns are missing.
183    """
184    if not {"phase", "elapsed_sec", "tcp_x_mm", "tcp_y_mm", "tau2_nm", "tau3_nm"}.issubset(df.columns):
185        raise ValueError("CSV missing required columns (phase, elapsed_sec, tcp_*, tau2/3_nm)")
186    fs = _fs(df)
187    tares, tare_flags = _tare_with_qc(df)
188    dtau2 = df["tau2_nm"].values - tares[2]
189    dtau3 = df["tau3_nm"].values - tares[3]
190    speed = _tcp_speed(df)
191
192    sw = df[df["phase"] == SWIRL_PHASE]
193    phase_source = SWIRL_PHASE
194    if len(sw) < 4:
195        sw = df[df["phase"].isin(FALLBACK_DYN_PHASES)]
196        phase_source = "fallback:" + ",".join(FALLBACK_DYN_PHASES)
197    if len(sw) < 4:
198        raise ValueError("No SWIRL (or fallback dynamic) phase samples in recording")
199
200    idx = sw.index.values
201    r2 = dtau2[idx] - _lowpass(dtau2[idx], fs)   # detrended residuals
202    r3 = dtau3[idx] - _lowpass(dtau3[idx], fs)
203    v = speed[idx]
204
205    # 1-2. mass offset: pre-swirl peak-Z mean vs INIT tare
206    pre = df.iloc[: idx[0]]
207    peak = pre[pre["tcp_z_mm"] == pre["tcp_z_mm"].max()]
208    mass_j2 = float(peak["tau2_nm"].mean() - tares[2]) if len(peak) else 0.0
209    mass_j3 = float(peak["tau3_nm"].mean() - tares[3]) if len(peak) else 0.0
210
211    # 3-5. swirl variance / peak-to-peak
212    swirl_var_j2 = float(np.var(r2))
213    swirl_var_j3 = float(np.var(r3))
214    swirl_ptp_j3 = float(np.ptp(dtau3[idx]))
215
216    # 6. phase lag: xcorr(TCP speed, τ3 residual), lag of max |corr| within ±2 s
217    phase_lag_s = _phase_lag(v, r3, fs)
218
219    # 7. rattle energy: observable high band only (honest about Nyquist)
220    nyq = fs / 2.0
221    lo, hi = 1.5, nyq
222    rattle_band_limited = nyq < 5.0
223    if len(r3) >= 8 and hi > lo:
224        freqs = np.fft.rfftfreq(len(r3), d=1.0 / fs)
225        psd = np.abs(np.fft.rfft(r3)) ** 2
226        band = (freqs >= lo) & (freqs <= hi)
227        rattle_e = float(np.sum(psd[band]))
228    else:
229        rattle_e = 0.0
230
231    # 8. grip compliance
232    grip_comp = float(sw["gripper_pos"].std()) if "gripper_pos" in sw else 0.0
233
234    # 9. lift transient
235    lift = df[df["phase"] == "LIFT_TO_TRAVEL"]
236    lift_dyn_j3 = float(lift["tau3_nm"].std()) if len(lift) >= 2 else 0.0
237
238    # 10. mean swirl speed
239    swirl_speed = float(np.nanmean(v)) if len(v) else 0.0
240
241    z = np.array([
242        mass_j2, mass_j3,
243        swirl_var_j2, swirl_var_j3, swirl_ptp_j3,
244        phase_lag_s, rattle_e,
245        grip_comp, lift_dyn_j3, swirl_speed,
246    ], dtype=float)
247    z = np.nan_to_num(z, nan=0.0, posinf=0.0, neginf=0.0)
248    scales = np.array([FEATURE_SCALES[n] for n in FEATURE_NAMES], dtype=float)
249    weights = np.array([FEATURE_WEIGHTS[n] for n in FEATURE_NAMES], dtype=float)
250    zs = z / np.where(scales > 0, scales, 1.0)   # balanced, unit-ish features
251    zw = zs * weights                            # drop dead/nuisance dims
252    norm = np.linalg.norm(zw)
253    e = (zw / norm).tolist() if norm > 1e-12 else [0.0] * len(zw)
254
255    diagnostics = {
256        "fs_hz": round(float(fs), 2),
257        "nyquist_hz": round(float(nyq), 2),
258        "n_samples": int(len(df)),
259        "phase_source": phase_source,
260        "n_swirl_samples": int(len(sw)),
261        "tare_flags": tare_flags,
262        "rattle_band_hz": [float(lo), round(float(hi), 2)],
263        "rattle_band_limited": bool(rattle_band_limited),
264        "label": str(df["object_label"].iloc[0]) if "object_label" in df else None,
265    }
266    return {"z": z.tolist(), "e": e, "diagnostics": diagnostics}
267
268
269def swirl_window_series(df):
270    """Detrended torque residuals over the dynamic (swirl) window, for the
271    dashboard 'why' charts. Returns a JSON-safe dict."""
272    if not {"phase", "elapsed_sec", "tau2_nm", "tau3_nm"}.issubset(df.columns):
273        raise ValueError("CSV missing required columns (phase, elapsed_sec, tau2/3_nm)")
274    fs = _fs(df)
275    tares, _ = _tare_with_qc(df)
276    dtau2 = df["tau2_nm"].values - tares[2]
277    dtau3 = df["tau3_nm"].values - tares[3]
278    speed = _tcp_speed(df) if {"tcp_x_mm", "tcp_y_mm"}.issubset(df.columns) else np.zeros(len(df))
279    sw = df[df["phase"] == SWIRL_PHASE]
280    if len(sw) < 4:
281        sw = df[df["phase"].isin(FALLBACK_DYN_PHASES)]
282    if len(sw) < 4:
283        raise ValueError("No SWIRL (or fallback dynamic) phase samples in recording")
284    idx = sw.index.values
285    t0 = float(df["elapsed_sec"].values[idx[0]])
286    r2 = dtau2[idx] - _lowpass(dtau2[idx], fs)
287    r3 = dtau3[idx] - _lowpass(dtau3[idx], fs)
288    return {
289        "t": [round(float(x) - t0, 3) for x in df["elapsed_sec"].values[idx]],
290        "r2": [round(float(x), 4) for x in r2],
291        "r3": [round(float(x), 4) for x in r3],
292        "phase_lag_s": round(_phase_lag(speed[idx], r3, fs), 3),
293        "fs_hz": round(float(fs), 2),
294    }
295
296
297def _calibrate():
298    """Recompute FEATURE_SCALES from all real recordings; print the dict."""
299    import glob
300    rows = []
301    for f in sorted(glob.glob("recordings/**/*.csv", recursive=True)):
302        d = pd.read_csv(f)
303        if not {"phase", "tau3_nm"}.issubset(d.columns):
304            continue
305        try:
306            rows.append(np.abs(extract_features(d)["z"]))
307        except Exception as ex:
308            print(f"skip {f}: {ex}", file=sys.stderr)
309    if not rows:
310        sys.exit("no usable recordings under recordings/")
311    A = np.array(rows)
312    print(f"# calibrated on {len(rows)} recordings")
313    print("FEATURE_SCALES = {")
314    for n, s in zip(FEATURE_NAMES, np.percentile(A, 90, axis=0)):
315        print(f'    "{n}": {s:.4g},')
316    print("}")
317
318
319def main():
320    """CLI entry: extract features from a CSV or recalibrate FEATURE_SCALES."""
321    ap = argparse.ArgumentParser(
322        description="Extract 10-D swirl features + normalized embedding.",
323        epilog="""examples:
324  python feature_extractor.py recordings/foo.csv          # print z, e, diagnostics
325  python feature_extractor.py --calibrate --require-swirl  # recompute FEATURE_SCALES
326                                                           # from swirl recordings, then
327                                                           # paste the output into FEATURE_SCALES
328
329The 5-25 Hz rattle band is NOT observable at 4-5 Hz cloud sampling;
330diagnostics flags rattle_band_limited honestly.""",
331        formatter_class=argparse.RawDescriptionHelpFormatter)
332    ap.add_argument("csv", nargs="?", help="Telemetry CSV from repeatable_grasp.py --record")
333    ap.add_argument("--calibrate", action="store_true",
334                    help="recompute FEATURE_SCALES from recordings/ and print them")
335    args = ap.parse_args()
336    if args.calibrate:
337        _calibrate()
338        return
339    if not args.csv:
340        ap.error("pass a CSV path (or --calibrate)")
341    if not os.path.isfile(args.csv):
342        sys.exit(f"file not found: {args.csv}")
343    out = extract_features(pd.read_csv(args.csv))
344    out["feature_names"] = FEATURE_NAMES
345    print(json.dumps(out, indent=2))
346
347
348if __name__ == "__main__":
349    main()