preprocessing/vm_diffusion/data/build_force_actions.py
| 1 | #!/usr/bin/env python |
| 2 | """Force-informed 9D actions from the mode1_v2 parquets (motherboard episodes only). |
| 3 | |
| 4 | The parquets carry per-native-frame calibrated normal force (react_calib, ~0.5 s |
| 5 | latency-compensated upstream) as columns force_{left,right}_normal_n, row-aligned 1:1 |
| 6 | with sensor_{left,right}_pose — no timestamp alignment is needed here. This script runs |
| 7 | vm_diffusion/model/pose7d_action.py over each episode and writes, per episode folder |
| 8 | under --videos-root (default: the mode1_v2_actions tree, one subfolder per episode): |
| 9 | |
| 10 | <root>/<episode>/sensor_actions_with_<F>.pt (T-1, 9) @ 30 fps |
| 11 | <root>/<episode>/sensor_actions_with_<F>_fps<N>.pt (F-1, 9) @ N fps |
| 12 | |
| 13 | where N = --target-fps (default 15 = stride 2, the default Wan2.2 tree since 2026-09-07; |
| 14 | --target-fps 6 --latent-dir .../mode1_v2_video_latents_wan2.2/tac gives the stride-5 |
| 15 | files) and <F> names the force encoding (--force-term): |
| 16 | |
| 17 | absolute -> sensor_actions_with_force*.pt dp = R_t^T (p_{t+1} - p_t) + k_f * f[t] * n |
| 18 | delta -> sensor_actions_with_dforce*.pt dp = R_t^T (p_{t+1} - p_t) + k_f * (f[t+1]-f[t]) * n |
| 19 | |
| 20 | with n = [0, -1, 0] and k_f = 1/(2 N/mm) = 0.0005 m/N. Same keys/meta as |
| 21 | sensor_actions{,_fps6}.pt (action/valid/rot_deg per side, frame_idx, meta with fps_stride |
| 22 | for the subsampled file), so `data.action_file:` can point straight at them. |
| 23 | |
| 24 | Force never touches rotation, so rot_deg/valid are identical to the pure-pose files. |
| 25 | Downsampling converts directly between the stride-N pose endpoints (bit-exact with the |
| 26 | SE(3) composition of native steps for the pose part). Under 'absolute' the force of each |
| 27 | coarse step is the one at its FIRST native frame (force_agg='start'), which makes the |
| 28 | applied force per action k exactly the force at native frame frame_idx[k] — same source, |
| 29 | same index; at stride 5 that is force_fps6.pt['force_<side>'][k] verbatim. The cost of |
| 30 | 'start' vs the span mean at stride 5: displacement rms 0.255 mm, max 2.69 mm (29% of the |
| 31 | force term's own std). Under 'delta' the force is the endpoint difference and force_agg |
| 32 | does not apply. The absolute term is a per-frame state and does not depend on fps; the |
| 33 | delta term grows with the stride like dp does. |
| 34 | |
| 35 | NOTE: valid_*/frame_idx are consumed by the Stage-1 builders only — the MoT world-model |
| 36 | dataset (`dataset.load_episode_actions`) reads action_*/meta and never applies the 30-deg |
| 37 | rotation dropout. |
| 38 | |
| 39 | pusht episodes have no force columns and are skipped. Existing outputs are skipped |
| 40 | unless --overwrite. |
| 41 | |
| 42 | Usage (needs pyarrow + torch; effecterase env): |
| 43 | /group2/ct/weihanx/miniconda3/envs/effecterase/bin/python \ |
| 44 | vm_diffusion/data/build_force_actions.py [--force-term delta] [--episodes ...] \ |
| 45 | [--overwrite] [--dry-run] |
| 46 | |
| 47 | fps6 tree (stride 5, explicit): |
| 48 | ... build_force_actions.py --target-fps 6 \ |
| 49 | --latent-dir /group2/ct/weihanx/world_model/mode1_v2_video_latents_wan2.2/tac |
| 50 | """ |
| 51 | from __future__ import annotations |
| 52 | |
| 53 | import argparse |
| 54 | import os |
| 55 | import math |
| 56 | import sys |
| 57 | from pathlib import Path |
| 58 | |
| 59 | import numpy as np |
| 60 | import torch |
| 61 | |
| 62 | _HERE = Path(__file__).resolve().parent |
| 63 | sys.path.insert(0, str(_HERE)) # build_episode_actions (same dir) |
| 64 | sys.path.insert(0, str(_HERE.parent / "model")) # pose7d_action |
| 65 | |
| 66 | from build_episode_actions import read_poses, rotation_deg # noqa: E402 |
| 67 | from pose7d_action import ( # noqa: E402 |
| 68 | DEFAULT_NORMAL_AXIS, |
| 69 | DEFAULT_STIFFNESS_N_PER_MM, |
| 70 | FORCE_TERMS, |
| 71 | Pose7DActionConfig, |
| 72 | convert_pose_sequence, |
| 73 | stiffness_to_k_f, |
| 74 | ) |
| 75 | |
| 76 | DEFAULT_PARQUET_ROOT = Path(os.environ.get("ACTS_PARQUET_ROOT", "mode1_v2")) |
| 77 | DEFAULT_VIDEOS_ROOT = Path(os.environ.get("ACTS_ACTIONS_OUT", "mode1_v2_videos")) # legacy tree (absolute files) |
| 78 | # New per-episode action tree (2026-09-07): default OUTPUT root for every builder here. |
| 79 | DEFAULT_ACTIONS_ROOT = Path(os.environ.get("ACTS_ACTIONS_OUT", "mode1_v2_actions")) |
| 80 | DEFAULT_LATENT_DIR = Path(os.environ.get("ACTS_TAC_LATENTS", "latents/tac")) |
| 81 | DEFAULT_TARGET_FPS = 15.0 |
| 82 | # Output names, keyed by force_term. Rebound in main() from --force-term / --target-fps / |
| 83 | # --out-fps-name; build_episode reads the module globals at call time, so both stay in sync. |
| 84 | FORCE_TERM_TAG = {"absolute": "force", "delta": "dforce"} |
| 85 | OUT_NATIVE = "sensor_actions_with_force.pt" |
| 86 | OUT_SUB = f"sensor_actions_with_force_fps{int(DEFAULT_TARGET_FPS)}.pt" |
| 87 | FORCE_COLUMNS = ("force_left_normal_n", "force_right_normal_n") |
| 88 | FORCE_AGG = "start" # ('absolute') force of coarse step k = force at native frame stride*k |
| 89 | MAX_ROT_DEG = 30.0 # same dropout rule as build_episode_actions.py |
| 90 | |
| 91 | |
| 92 | def read_forces(path: Path) -> dict[str, np.ndarray] | None: |
| 93 | """Parquet -> {'left': (T,), 'right': (T,)} newtons, or None if absent (pusht).""" |
| 94 | import pyarrow.parquet as pq |
| 95 | |
| 96 | have = set(pq.ParquetFile(str(path)).schema_arrow.names) |
| 97 | if not set(FORCE_COLUMNS) <= have: |
| 98 | return None |
| 99 | table = pq.read_table(str(path), columns=list(FORCE_COLUMNS)) |
| 100 | out = {} |
| 101 | for side, col in zip(("left", "right"), FORCE_COLUMNS): |
| 102 | f = np.asarray(table.column(col).to_pylist(), dtype=np.float64) |
| 103 | if not np.isfinite(f).all() or (f < 0).any(): |
| 104 | raise RuntimeError(f"{path.name}: bad values in {col}") |
| 105 | out[side] = f |
| 106 | return out |
| 107 | |
| 108 | |
| 109 | def base_meta(parquet: Path, cfg: Pose7DActionConfig) -> dict: |
| 110 | return { |
| 111 | "layout": "[dp_x, dp_y, dp_z, r1..r6]", |
| 112 | "frame": ("sensor frame at time t; dp = R_t^T (p_{t+1} - p_t) " |
| 113 | "+ k_f * f_n * normal_axis, dR = R_t^T R_{t+1}"), |
| 114 | "rot6d": ("raw Rot6D(dR), first two columns as [c1x,c1y,c1z, c2x,c2y,c2z]; " |
| 115 | "NOT identity-subtracted"), |
| 116 | "units": "metres", |
| 117 | "quat_order": "xyzw", |
| 118 | "source": str(parquet), |
| 119 | "max_rot_deg": MAX_ROT_DEG, |
| 120 | "gel_frame": False, |
| 121 | # force-specific |
| 122 | "force_informed": True, |
| 123 | "force_term": cfg.force_term, |
| 124 | "stiffness_n_per_mm": cfg.stiffness_n_per_mm, |
| 125 | "k_f": cfg.resolved_k_f(), |
| 126 | "normal_axis": tuple(cfg.normal_axis), |
| 127 | "force_source": "force_<side>_normal_n (twm.force_recovery / react_calib), " |
| 128 | "row-aligned with the poses in the same parquet", |
| 129 | "force_note": ( |
| 130 | "force enters translation only, along normal_axis; rotation and rot_deg/valid are " |
| 131 | "identical to sensor_actions*.pt. force_term='absolute': k_f * f[start] (per-frame " |
| 132 | "indentation state, fps-independent); force_term='delta': k_f * (f[end] - f[start]) " |
| 133 | "(change of indentation over the step, grows with the stride like dp)" |
| 134 | ), |
| 135 | } |
| 136 | |
| 137 | |
| 138 | def build_side(pose: torch.Tensor, force: np.ndarray, cfg: Pose7DActionConfig): |
| 139 | """-> (action (n,9) f32, valid (n,) bool, rot_deg (n,) f32, frame_idx (F,) i64).""" |
| 140 | actions, fidx = convert_pose_sequence(pose.numpy(), force, cfg) |
| 141 | a = torch.from_numpy(actions).float() |
| 142 | if not torch.isfinite(a).all(): |
| 143 | raise RuntimeError("non-finite action") |
| 144 | rd = rotation_deg(a) # force does not touch the rotation block |
| 145 | return a, rd <= MAX_ROT_DEG, rd.float(), fidx |
| 146 | |
| 147 | |
| 148 | def valid_and(valid_native: torch.Tensor, fidx: np.ndarray) -> torch.Tensor: |
| 149 | """A coarse step is valid only if every native step it spans is valid.""" |
| 150 | return torch.tensor([bool(valid_native[fidx[k]:fidx[k + 1]].all()) |
| 151 | for k in range(len(fidx) - 1)]) |
| 152 | |
| 153 | |
| 154 | def build_episode(parquet: Path, out_dir: Path, latent_dir: Path, |
| 155 | cfg_native: Pose7DActionConfig, cfg_sub: Pose7DActionConfig, |
| 156 | overwrite: bool, dry_run: bool) -> str: |
| 157 | todo = [n for n in (OUT_NATIVE, OUT_SUB) if overwrite or not (out_dir / n).exists()] |
| 158 | if not todo: |
| 159 | return f"[skip] {parquet.stem}: outputs exist" |
| 160 | |
| 161 | forces = read_forces(parquet) |
| 162 | if forces is None: |
| 163 | return f"[skip] {parquet.stem}: no force columns (pusht?)" |
| 164 | pose_l, pose_r, _ = read_poses(parquet) |
| 165 | T = int(pose_l.shape[0]) |
| 166 | if forces["left"].shape[0] != T: |
| 167 | raise RuntimeError(f"{parquet.stem}: force rows {forces['left'].shape[0]} != poses {T}") |
| 168 | |
| 169 | lat = torch.load(latent_dir / f"{parquet.stem}.pt", map_location="cpu", weights_only=False) |
| 170 | fs = int(lat["fps_stride"]) |
| 171 | if fs != cfg_sub.stride(): |
| 172 | raise RuntimeError(f"{parquet.stem}: latent fps_stride={fs} != config stride {cfg_sub.stride()}") |
| 173 | n_frames = math.ceil(T / fs) |
| 174 | if n_frames != int(lat["num_frames"]): |
| 175 | raise RuntimeError(f"{parquet.stem}: derived {n_frames} frames but latents say {lat['num_frames']}") |
| 176 | |
| 177 | payloads: dict[str, dict] = {} |
| 178 | valid_native: dict[str, torch.Tensor] = {} # filled by the OUT_NATIVE pass first |
| 179 | for out_name, cfg in ((OUT_NATIVE, cfg_native), (OUT_SUB, cfg_sub)): |
| 180 | payload: dict[str, object] = {} |
| 181 | for side, pose in (("left", pose_l), ("right", pose_r)): |
| 182 | a, valid, rd, fidx = build_side(pose, forces[side], cfg) |
| 183 | if out_name == OUT_SUB: |
| 184 | # native validity is authoritative: AND over the spanned native steps |
| 185 | valid = valid_and(valid_native[side], fidx) |
| 186 | else: |
| 187 | valid_native[side] = valid |
| 188 | payload[f"action_{side}"] = a |
| 189 | payload[f"valid_{side}"] = valid |
| 190 | payload[f"rot_deg_{side}"] = rd |
| 191 | payload["frame_idx"] = torch.from_numpy(fidx).to(torch.int32) |
| 192 | payload["meta"] = base_meta(parquet, cfg) |
| 193 | if out_name == OUT_SUB: |
| 194 | payload["meta"].update({ |
| 195 | "fps": float(lat["fps"]), |
| 196 | "src_fps": float(lat["src_fps"]), |
| 197 | "fps_stride": fs, |
| 198 | "num_frames": n_frames, |
| 199 | "native_num_frames": T, |
| 200 | "force_agg": cfg.force_agg if cfg.force_term == "absolute" else None, |
| 201 | "downsampled_from": str(parquet), |
| 202 | "downsample": ( |
| 203 | "action[k] converts directly between poses frame_idx[k] -> frame_idx[k+1] " |
| 204 | "(== ordered SE(3) composition of the spanned native steps); " |
| 205 | + ("force taken at native frame frame_idx[k] ('start'); at stride 5 that is " |
| 206 | "force_fps6.pt[k]" if cfg.force_term == "absolute" else |
| 207 | "force = f[frame_idx[k+1]] - f[frame_idx[k]] (endpoint difference)") |
| 208 | ), |
| 209 | }) |
| 210 | payloads[out_name] = payload |
| 211 | |
| 212 | if not dry_run: |
| 213 | out_dir.mkdir(parents=True, exist_ok=True) |
| 214 | for out_name in todo: |
| 215 | out_path = out_dir / out_name |
| 216 | tmp = out_path.with_name(out_path.name + ".tmp") |
| 217 | torch.save(payloads[out_name], tmp) |
| 218 | tmp.replace(out_path) |
| 219 | |
| 220 | n6 = payloads[OUT_SUB]["action_left"].shape[0] |
| 221 | flagged = int((~payloads[OUT_NATIVE]["valid_left"]).sum() |
| 222 | + (~payloads[OUT_NATIVE]["valid_right"]).sum()) |
| 223 | return (f"[write] {parquet.stem}: T={T} native={T - 1} {OUT_SUB}={n6} actions x9 per side " |
| 224 | f"({flagged} native steps flagged)" + (" [dry-run]" if dry_run else "")) |
| 225 | |
| 226 | |
| 227 | def main() -> None: |
| 228 | ap = argparse.ArgumentParser( |
| 229 | description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter |
| 230 | ) |
| 231 | ap.add_argument("--parquet-root", type=Path, default=DEFAULT_PARQUET_ROOT) |
| 232 | ap.add_argument("--videos-root", type=Path, default=DEFAULT_ACTIONS_ROOT, |
| 233 | help="OUTPUT root; one subfolder per episode (default: the mode1_v2_actions " |
| 234 | "tree; the legacy absolute files live under mode1_v2_videos)") |
| 235 | ap.add_argument("--latent-dir", type=Path, default=DEFAULT_LATENT_DIR, |
| 236 | help="tactile latent dir; fps/fps_stride/num_frames are validated against it " |
| 237 | "(default: the fps15 tree)") |
| 238 | ap.add_argument("--force-term", choices=FORCE_TERMS, default="absolute", |
| 239 | help="force encoding: 'absolute' -> *_with_force*, 'delta' -> *_with_dforce*") |
| 240 | ap.add_argument("--episodes", nargs="*", default=None, |
| 241 | help="explicit episode stems (default: every motherboard parquet)") |
| 242 | ap.add_argument("--stiffness-n-per-mm", type=float, default=DEFAULT_STIFFNESS_N_PER_MM, |
| 243 | help="gel normal stiffness k; k_f = 1/(k*1000) m/N") |
| 244 | ap.add_argument("--overwrite", action="store_true") |
| 245 | ap.add_argument("--target-fps", type=float, default=DEFAULT_TARGET_FPS, |
| 246 | help="rate of the subsampled output; round(src_fps/target_fps) must equal " |
| 247 | "the fps_stride of the latents under --latent-dir (default 15)") |
| 248 | ap.add_argument("--out-fps-name", default=None, |
| 249 | help="subsampled output filename " |
| 250 | "(default sensor_actions_with_<force|dforce>_fps<int(target-fps)>.pt)") |
| 251 | ap.add_argument("--dry-run", action="store_true", help="build + report without writing") |
| 252 | args = ap.parse_args() |
| 253 | |
| 254 | global OUT_NATIVE, OUT_SUB |
| 255 | tag = FORCE_TERM_TAG[args.force_term] |
| 256 | OUT_NATIVE = f"sensor_actions_with_{tag}.pt" |
| 257 | OUT_SUB = args.out_fps_name or f"sensor_actions_with_{tag}_fps{int(args.target_fps)}.pt" |
| 258 | |
| 259 | if args.episodes: |
| 260 | parquets = [args.parquet_root / f"{e}.parquet" for e in args.episodes] |
| 261 | for p in parquets: |
| 262 | if not p.exists(): |
| 263 | raise FileNotFoundError(p) |
| 264 | else: |
| 265 | parquets = sorted(args.parquet_root.glob("motherboard_*.parquet")) |
| 266 | if not parquets: |
| 267 | raise SystemExit(f"no motherboard parquet under {args.parquet_root}") |
| 268 | |
| 269 | common = dict(use_force=True, stiffness_n_per_mm=args.stiffness_n_per_mm, |
| 270 | normal_axis=DEFAULT_NORMAL_AXIS, src_fps=30.0, force_term=args.force_term) |
| 271 | cfg_native = Pose7DActionConfig(**common, target_fps=None) |
| 272 | cfg_sub = Pose7DActionConfig(**common, target_fps=args.target_fps, force_agg=FORCE_AGG) |
| 273 | |
| 274 | print(f"{len(parquets)} episode(s) -> {args.videos_root}/*/{{{OUT_NATIVE},{OUT_SUB}}} " |
| 275 | f"(target_fps={args.target_fps:g} stride={cfg_sub.stride()}, latents={args.latent_dir}, " |
| 276 | f"k={args.stiffness_n_per_mm} N/mm, k_f={stiffness_to_k_f(args.stiffness_n_per_mm):g} m/N, " |
| 277 | f"normal_axis={DEFAULT_NORMAL_AXIS}, force_term={args.force_term}, " |
| 278 | f"force_agg={FORCE_AGG if args.force_term == 'absolute' else None})") |
| 279 | n_written = 0 |
| 280 | for parquet in parquets: |
| 281 | out_dir = args.videos_root / parquet.stem |
| 282 | if not out_dir.is_dir() and args.dry_run: |
| 283 | print(f" [note] {parquet.stem}: destination folder {out_dir} does not exist yet") |
| 284 | msg = build_episode(parquet, out_dir, args.latent_dir, cfg_native, cfg_sub, |
| 285 | args.overwrite, args.dry_run) |
| 286 | print(f" {msg}") |
| 287 | n_written += msg.startswith("[write]") and not args.dry_run |
| 288 | print(f"done ({n_written} episodes written)") |
| 289 | |
| 290 | |
| 291 | if __name__ == "__main__": |
| 292 | main() |
| 293 | |