preprocessing/vm_diffusion/data/build_pose7d_force.py
12.2 KB · 252 lines · python Raw
1 #!/usr/bin/env python
2 """Per-frame 7D pose + normal-force streams for the `action_chunking` representation.
3
4 `action_chunking` (vm_diffusion/model/pose7d_action.py) expresses every frame of a
5 training window relative to the window's FIRST frame (the anchor):
6
7 a_k = [ R_0^T (p_{k+1} - p_0) + k_f * F_k * [0,-1,0] , Rot6D(R_0^T R_{k+1}) ]
8
9 F_k = f_{k+1} (data.force_term: absolute -- the arrival frame's indentation)
10 = f_{k+1} - f_0 (data.force_term: delta -- change of indentation since the anchor)
11
12 Because the anchor depends on the window, and latent-cache windows overlap, the rows
13 cannot be precomputed per frame. This script therefore stores the INPUTS at the latent
14 rate -- one 7D pose and one force per fps-subsampled frame -- and the dataset builds
15 the rows per window with `raw_pose_chunk_to_force_informed_9d`. The stream is
16 encoding-agnostic: which force_term is applied is decided by the training config, not
17 by this file.
18
19 <videos-root>/<episode>/sensor_pose7d_force_fps<N>.pt
20
21 pose_left / pose_right (F, 7) float64 [x,y,z,qx,qy,qz,qw], world frame
22 force_left / force_right (F,) float32 normal force [N] at that frame
23 valid_left / valid_right (F-1,) bool DELTA-step validity (rot <= 30 deg,
24 AND over the spanned native steps);
25 chunk row k is trustworthy iff
26 valid[:k+1].all()
27 frame_idx (F,) int32 native frame index of each row
28 meta dict fps_stride / num_frames / k_f / ...
29
30 Frame grid and force sampling match the delta_action files exactly: frame_idx =
31 arange(0, T, stride), force point-sampled at native frame frame_idx[k] (== the 'start'
32 force of sensor_actions_with_force_fps<N>.pt row k, == force_fps6.pt[k] at stride 5).
33 When a delta twin (sensor_actions_with_{force,dforce}_fps<N>.pt) exists in the output
34 folder its valid_*/frame_idx are asserted equal, as a consistency check.
35
36 pusht episodes have no force columns and are skipped. Existing outputs are skipped
37 unless --overwrite.
38
39 Usage (needs pyarrow + torch; effecterase env). Defaults: fps15 (stride 2) and the
40 mode1_v2_actions output tree:
41 /group2/ct/weihanx/miniconda3/envs/effecterase/bin/python \
42 vm_diffusion/data/build_pose7d_force.py [--episodes ...] [--overwrite] [--dry-run]
43
44 fps6 tree (stride 5, explicit):
45 ... build_pose7d_force.py --target-fps 6 \
46 --latent-dir /group2/ct/weihanx/world_model/mode1_v2_video_latents_wan2.2/tac
47 """
48 from __future__ import annotations
49
50 import argparse
51 import math
52 import sys
53 from pathlib import Path
54
55 import numpy as np
56 import torch
57
58 _HERE = Path(__file__).resolve().parent
59 sys.path.insert(0, str(_HERE)) # build_episode_actions / build_force_actions
60 sys.path.insert(0, str(_HERE.parent / "model")) # pose7d_action
61
62 from build_episode_actions import read_poses, rotation_deg # noqa: E402
63 from build_force_actions import ( # noqa: E402
64 DEFAULT_ACTIONS_ROOT,
65 DEFAULT_LATENT_DIR,
66 DEFAULT_PARQUET_ROOT,
67 DEFAULT_TARGET_FPS,
68 MAX_ROT_DEG,
69 read_forces,
70 valid_and,
71 )
72 from pose7d_action import ( # noqa: E402
73 DEFAULT_NORMAL_AXIS,
74 DEFAULT_STIFFNESS_N_PER_MM,
75 Pose7DActionConfig,
76 raw_pose_sequence_to_force_informed_9d,
77 stiffness_to_k_f,
78 subsample_pose_sequence,
79 )
80
81 CHUNK_FORMULA = ("row k of a window with anchor frame 0: dp = R_0^T (p_{k+1} - p_0) + k_f * F_k "
82 "* normal_axis, dR = R_0^T R_{k+1}; F_k = f_{k+1} (force_term absolute) or "
83 "f_{k+1} - f_0 (force_term delta), chosen by the training config; normal not "
84 "rotated; built per window by pose7d_action.raw_pose_chunk_to_force_informed_9d")
85
86
87 def delta_files_for(out_name: str) -> list[str]:
88 """sensor_pose7d_force_fps6.pt -> the delta twins sharing its grid (either force encoding)."""
89 return [out_name.replace("sensor_pose7d_force", f"sensor_actions_with_{tag}")
90 for tag in ("force", "dforce")]
91
92
93 def build_side(pose: torch.Tensor, force: np.ndarray, cfg: Pose7DActionConfig):
94 """-> (pose_sub (F,7) f64, force_sub (F,) f32, valid (F-1,) bool, frame_idx (F,) i64)."""
95 poses_np = pose.numpy()
96 pose_sub, force_sub, fidx = subsample_pose_sequence(poses_np, force, cfg)
97 # delta-step validity from the pure-pose native stream (force never touches rotation)
98 native = torch.from_numpy(
99 raw_pose_sequence_to_force_informed_9d(poses_np, np.zeros(poses_np.shape[0]), k_f=0.0)).float()
100 valid_native = rotation_deg(native) <= MAX_ROT_DEG
101 valid = valid_and(valid_native, fidx)
102 if not (np.isfinite(pose_sub).all() and np.isfinite(force_sub).all()):
103 raise RuntimeError("non-finite pose/force")
104 return (torch.from_numpy(pose_sub), torch.from_numpy(force_sub).float(), valid,
105 torch.from_numpy(fidx))
106
107
108 def build_episode(parquet: Path, out_dir: Path, latent_dir: Path, cfg: Pose7DActionConfig,
109 out_name: str, overwrite: bool, dry_run: bool) -> str:
110 out_path = out_dir / out_name
111 if out_path.exists() and not overwrite:
112 return f"[skip] {parquet.stem}: {out_name} exists"
113
114 forces = read_forces(parquet)
115 if forces is None:
116 return f"[skip] {parquet.stem}: no force columns (pusht?)"
117 pose_l, pose_r, _ = read_poses(parquet)
118 T = int(pose_l.shape[0])
119 if forces["left"].shape[0] != T:
120 raise RuntimeError(f"{parquet.stem}: force rows {forces['left'].shape[0]} != poses {T}")
121
122 lat = torch.load(latent_dir / f"{parquet.stem}.pt", map_location="cpu", weights_only=False)
123 fs = int(lat["fps_stride"])
124 if fs != cfg.stride():
125 raise RuntimeError(f"{parquet.stem}: latent fps_stride={fs} != config stride {cfg.stride()}")
126 n_frames = math.ceil(T / fs)
127 if n_frames != int(lat["num_frames"]):
128 raise RuntimeError(f"{parquet.stem}: derived {n_frames} frames but latents say {lat['num_frames']}")
129
130 payload: dict[str, object] = {}
131 fidx_ref = None
132 for side, pose in (("left", pose_l), ("right", pose_r)):
133 p, f, valid, fidx = build_side(pose, forces[side], cfg)
134 if p.shape[0] != n_frames:
135 raise RuntimeError(f"{parquet.stem}/{side}: {p.shape[0]} subsampled frames != {n_frames}")
136 payload[f"pose_{side}"] = p
137 payload[f"force_{side}"] = f
138 payload[f"valid_{side}"] = valid
139 fidx_ref = fidx
140 payload["frame_idx"] = fidx_ref.to(torch.int32)
141 payload["meta"] = {
142 "action_repr": "action_chunking",
143 "layout": "pose7d [x,y,z,qx,qy,qz,qw] world frame per frame; normal force [N] per frame",
144 "quat_order": "xyzw",
145 "units": "metres / newtons",
146 "chunk_formula": CHUNK_FORMULA,
147 "force_sample": "frame",
148 "force_note": ("force_<side>[k] = native force at frame_idx[k] (== delta file's 'start' "
149 "force); per-frame ABSOLUTE newtons -- the encoding (force_term) is "
150 "applied by the dataset, not stored here"),
151 "valid_note": ("valid[k] is the DELTA step k->k+1 (AND over spanned native steps); "
152 "a chunk row k is trustworthy iff valid[:k+1].all()"),
153 "max_rot_deg": MAX_ROT_DEG,
154 "gel_frame": False,
155 "stiffness_n_per_mm": cfg.stiffness_n_per_mm,
156 "k_f": cfg.resolved_k_f(),
157 "normal_axis": tuple(cfg.normal_axis),
158 "force_source": "force_<side>_normal_n (twm.force_recovery / react_calib), "
159 "row-aligned with the poses in the same parquet",
160 "source": str(parquet),
161 "fps": float(lat["fps"]),
162 "src_fps": float(lat["src_fps"]),
163 "fps_stride": fs,
164 "num_frames": n_frames,
165 "native_num_frames": T,
166 "downsampled_from": str(parquet),
167 }
168
169 # consistency check against whichever delta twins exist (same grid, same valid rule)
170 checked = ""
171 for delta_path in (out_dir / n for n in delta_files_for(out_name)):
172 if not delta_path.is_file():
173 continue
174 dd = torch.load(delta_path, map_location="cpu", weights_only=False)
175 if not torch.equal(dd["frame_idx"].to(torch.int32), payload["frame_idx"]):
176 raise RuntimeError(f"{parquet.stem}: frame_idx differs from {delta_path.name}")
177 for side in ("left", "right"):
178 if not torch.equal(dd[f"valid_{side}"].bool(), payload[f"valid_{side}"].bool()):
179 raise RuntimeError(f"{parquet.stem}: valid_{side} differs from {delta_path.name}")
180 checked += f", valid/frame_idx == {delta_path.name}"
181 if not checked:
182 checked = ", no delta twin in the output folder (check skipped)"
183
184 if not dry_run:
185 out_dir.mkdir(parents=True, exist_ok=True)
186 tmp = out_path.with_name(out_path.name + ".tmp")
187 torch.save(payload, tmp)
188 tmp.replace(out_path)
189
190 flagged = int((~payload["valid_left"]).sum() + (~payload["valid_right"]).sum())
191 return (f"[write] {parquet.stem}: T={T} -> {out_name} F={n_frames} frames x (7 pose + 1 force) "
192 f"per side ({flagged} delta steps flagged{checked})" + (" [dry-run]" if dry_run else ""))
193
194
195 def main() -> None:
196 ap = argparse.ArgumentParser(
197 description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter
198 )
199 ap.add_argument("--parquet-root", type=Path, default=DEFAULT_PARQUET_ROOT)
200 ap.add_argument("--videos-root", type=Path, default=DEFAULT_ACTIONS_ROOT,
201 help="OUTPUT root; one subfolder per episode (default: the mode1_v2_actions tree)")
202 ap.add_argument("--latent-dir", type=Path, default=DEFAULT_LATENT_DIR,
203 help="tactile latent dir; fps/fps_stride/num_frames are validated against it "
204 "(default: the fps15 tree)")
205 ap.add_argument("--episodes", nargs="*", default=None,
206 help="explicit episode stems (default: every motherboard parquet)")
207 ap.add_argument("--stiffness-n-per-mm", type=float, default=DEFAULT_STIFFNESS_N_PER_MM,
208 help="gel normal stiffness k; k_f = 1/(k*1000) m/N (stored in meta)")
209 ap.add_argument("--overwrite", action="store_true")
210 ap.add_argument("--target-fps", type=float, default=DEFAULT_TARGET_FPS,
211 help="rate of the output; round(src_fps/target_fps) must equal the "
212 "fps_stride of the latents under --latent-dir (default 15)")
213 ap.add_argument("--out-fps-name", default=None,
214 help="output filename (default sensor_pose7d_force_fps<int(target-fps)>.pt)")
215 ap.add_argument("--dry-run", action="store_true", help="build + report without writing")
216 args = ap.parse_args()
217
218 out_name = args.out_fps_name or f"sensor_pose7d_force_fps{int(args.target_fps)}.pt"
219
220 if args.episodes:
221 parquets = [args.parquet_root / f"{e}.parquet" for e in args.episodes]
222 for p in parquets:
223 if not p.exists():
224 raise FileNotFoundError(p)
225 else:
226 parquets = sorted(args.parquet_root.glob("motherboard_*.parquet"))
227 if not parquets:
228 raise SystemExit(f"no motherboard parquet under {args.parquet_root}")
229
230 cfg = Pose7DActionConfig(use_force=True, stiffness_n_per_mm=args.stiffness_n_per_mm,
231 normal_axis=DEFAULT_NORMAL_AXIS, src_fps=30.0,
232 target_fps=args.target_fps, action_repr="action_chunking")
233
234 print(f"{len(parquets)} episode(s) -> {args.videos_root}/*/{out_name} "
235 f"(target_fps={args.target_fps:g} stride={cfg.stride()}, latents={args.latent_dir}, "
236 f"k={args.stiffness_n_per_mm} N/mm, k_f={stiffness_to_k_f(args.stiffness_n_per_mm):g} m/N, "
237 f"normal_axis={DEFAULT_NORMAL_AXIS}, action_repr=action_chunking)")
238 n_written = 0
239 for parquet in parquets:
240 out_dir = args.videos_root / parquet.stem
241 if not out_dir.is_dir() and args.dry_run:
242 print(f" [note] {parquet.stem}: destination folder {out_dir} does not exist yet")
243 msg = build_episode(parquet, out_dir, args.latent_dir, cfg, out_name,
244 args.overwrite, args.dry_run)
245 print(f" {msg}")
246 n_written += msg.startswith("[write]") and not args.dry_run
247 print(f"done ({n_written} episodes written)")
248
249
250 if __name__ == "__main__":
251 main()
252