preprocessing/vm_diffusion/data/build_force_actions.py
14.5 KB · 293 lines · python Raw
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