preprocessing/vm_diffusion/data/build_pose7d_force.py
| 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 | |