preprocessing/data_preprocessing/build_episode_actions.py
| 1 | #!/usr/bin/env python |
| 2 | """Precompute per-episode sensor-frame 9D actions from the mode1_v2 parquets. |
| 3 | |
| 4 | Reads the world-frame sensor poses |
| 5 | |
| 6 | <parquet-root>/<episode>.parquet columns sensor_left_pose / sensor_right_pose, |
| 7 | each (T, 7) as [x, y, z, qx, qy, qz, qw] |
| 8 | |
| 9 | and writes, next to that episode's videos, |
| 10 | |
| 11 | <videos-root>/<episode>/sensor_actions.pt |
| 12 | action_left : (T-1, 9) float32 [dp(3, metres), Rot6D(dR)(6)] |
| 13 | action_right : (T-1, 9) float32 |
| 14 | valid_left : (T-1,) bool False where the step looks like tracking dropout |
| 15 | valid_right : (T-1,) bool |
| 16 | rot_deg_left : (T-1,) float32 per-step rotation magnitude, degrees |
| 17 | rot_deg_right: (T-1,) float32 |
| 18 | frame_idx : (T,) int32 action[t] maps frame t -> t+1 |
| 19 | meta : dict conventions, so consumers need not guess |
| 20 | |
| 21 | Both actions are expressed in the sensor frame at time t (see common/actions.py); |
| 22 | the rotation is stored as raw Rot6D(dR), NOT identity-subtracted — that transform |
| 23 | belongs at the model input, alongside standardization, so this file stays a |
| 24 | faithful encoding of the pose stream. |
| 25 | |
| 26 | Left and right stay as two separate (T-1, 9) tensors rather than one (T-1, 18): |
| 27 | consumers that want the concatenated form build it at the model boundary, and |
| 28 | single-sided uses do not have to slice. |
| 29 | |
| 30 | With --gel-frame the poses are first composed with T_rigid_gel (see common/gel_frame.py) |
| 31 | and written to sensor_actions_gel.pt instead. The action is then expressed in the GEL |
| 32 | frame: +z is the real gel normal and the origin sits on the gel surface, so dp_z is press |
| 33 | depth and dp_x, dp_y are in-plane shear. sensor_action_9d itself is unchanged — only its |
| 34 | input poses are. rot_deg and valid are identical either way, because dR_gel = R_g^T dR R_g |
| 35 | is a conjugation and so preserves the geodesic angle. |
| 36 | """ |
| 37 | from __future__ import annotations |
| 38 | |
| 39 | import argparse |
| 40 | import os |
| 41 | import sys |
| 42 | from pathlib import Path |
| 43 | |
| 44 | import numpy as np |
| 45 | import torch |
| 46 | |
| 47 | sys.path.insert(0, str(Path(__file__).resolve().parents[1])) |
| 48 | |
| 49 | from common.actions import rot6d_to_matrix, sensor_action_9d # noqa: E402 |
| 50 | from common.gel_frame import calib_dir_for, gel_frame, poses_to_gel # noqa: E402 |
| 51 | |
| 52 | DEFAULT_PARQUET_ROOT = Path(os.environ.get("ACTS_PARQUET_ROOT", "mode1_v2")) |
| 53 | DEFAULT_VIDEOS_ROOT = Path(os.environ.get("ACTS_ACTIONS_OUT", "mode1_v2_videos")) |
| 54 | OUT_NAME = "sensor_actions.pt" |
| 55 | GEL_OUT_NAME = "sensor_actions_gel.pt" |
| 56 | |
| 57 | POSE_COLUMNS = ("sensor_left_pose", "sensor_right_pose") |
| 58 | |
| 59 | |
| 60 | def read_poses(path: Path) -> tuple[torch.Tensor, torch.Tensor, np.ndarray]: |
| 61 | """Parquet -> (pose_left, pose_right) as (T, 7) float64 tensors, plus frame_idx.""" |
| 62 | import pyarrow.parquet as pq |
| 63 | |
| 64 | cols = list(POSE_COLUMNS) |
| 65 | have = set(pq.ParquetFile(str(path)).schema_arrow.names) |
| 66 | if "frame_idx" in have: |
| 67 | cols.append("frame_idx") |
| 68 | missing = set(POSE_COLUMNS) - have |
| 69 | if missing: |
| 70 | raise RuntimeError(f"{path.name}: missing pose column(s) {sorted(missing)}") |
| 71 | |
| 72 | table = pq.read_table(str(path), columns=cols) |
| 73 | |
| 74 | def _pose(name: str) -> torch.Tensor: |
| 75 | arr = np.asarray(table.column(name).to_pylist(), dtype=np.float64) |
| 76 | if arr.ndim != 2 or arr.shape[1] != 7: |
| 77 | raise RuntimeError(f"{path.name}: expected (T,7) for {name}, got {arr.shape}") |
| 78 | return torch.from_numpy(arr) |
| 79 | |
| 80 | pl, pr = _pose(POSE_COLUMNS[0]), _pose(POSE_COLUMNS[1]) |
| 81 | if pl.shape != pr.shape: |
| 82 | raise RuntimeError(f"{path.name}: left/right pose length mismatch {pl.shape} vs {pr.shape}") |
| 83 | |
| 84 | if "frame_idx" in cols: |
| 85 | frame_idx = np.asarray(table.column("frame_idx").to_pylist(), dtype=np.int32) |
| 86 | else: |
| 87 | frame_idx = np.arange(pl.shape[0], dtype=np.int32) |
| 88 | |
| 89 | return pl, pr, frame_idx |
| 90 | |
| 91 | |
| 92 | def rotation_deg(action9: torch.Tensor) -> torch.Tensor: |
| 93 | """Per-step geodesic rotation magnitude, degrees, recovered from the 6D block.""" |
| 94 | R = rot6d_to_matrix(action9[:, 3:]) |
| 95 | cos = (R.diagonal(dim1=-2, dim2=-1).sum(-1) - 1.0) / 2.0 |
| 96 | return torch.rad2deg(torch.arccos(cos.clamp(-1.0, 1.0))) |
| 97 | |
| 98 | |
| 99 | def build_episode(parquet: Path, out_dir: Path, max_rot_deg: float, |
| 100 | out_name: str = OUT_NAME, gel: bool = False) -> dict: |
| 101 | pose_l, pose_r, frame_idx = read_poses(parquet) |
| 102 | |
| 103 | payload: dict[str, object] = {} |
| 104 | gel_meta: dict[str, object] = {} |
| 105 | n_flagged = 0 |
| 106 | for side, pose in (("left", pose_l), ("right", pose_r)): |
| 107 | if gel: |
| 108 | # Re-express the poses in the gel frame BEFORE the (unchanged) converter, so |
| 109 | # the 9D action comes out with +z along the real gel normal and its origin on |
| 110 | # the gel surface. Every downstream raster inherits the fix for free. |
| 111 | R_g, c_gel, info = gel_frame(side, calib_dir_for(parquet.stem)) |
| 112 | pose = poses_to_gel(pose, R_g, c_gel) |
| 113 | gel_meta[side] = info |
| 114 | action = sensor_action_9d(pose) |
| 115 | rot_deg = rotation_deg(action) |
| 116 | # A >30 deg/frame step at 30 fps is tracking dropout or a quaternion sign |
| 117 | # flip, not real motion. Flag rather than drop, so indices stay aligned. |
| 118 | valid = rot_deg <= max_rot_deg |
| 119 | n_flagged += int((~valid).sum()) |
| 120 | |
| 121 | if not torch.isfinite(action).all(): |
| 122 | raise RuntimeError(f"{parquet.name}: non-finite action for {side}") |
| 123 | |
| 124 | payload[f"action_{side}"] = action.float() |
| 125 | payload[f"valid_{side}"] = valid |
| 126 | payload[f"rot_deg_{side}"] = rot_deg.float() |
| 127 | |
| 128 | payload["frame_idx"] = torch.from_numpy(frame_idx) |
| 129 | payload["meta"] = { |
| 130 | "layout": "[dp_x, dp_y, dp_z, r1..r6]", |
| 131 | "frame": "sensor frame at time t; dp = R_t^T (p_{t+1} - p_t), dR = R_t^T R_{t+1}", |
| 132 | "rot6d": "raw Rot6D(dR), first two columns as [c1x,c1y,c1z, c2x,c2y,c2z]; NOT identity-subtracted", |
| 133 | "units": "metres", |
| 134 | "quat_order": "xyzw", |
| 135 | "source": str(parquet), |
| 136 | "max_rot_deg": max_rot_deg, |
| 137 | "gel_frame": bool(gel), |
| 138 | } |
| 139 | if gel: |
| 140 | # Record the full basis: the in-plane axes are a free choice, so Stage 2 has to |
| 141 | # be able to reproduce this exact one. |
| 142 | payload["meta"]["gel_calibration"] = gel_meta |
| 143 | payload["meta"]["frame"] = ( |
| 144 | "GEL frame at time t (+z = gel_axis_in_rigid, origin = gel surface centre); " |
| 145 | "poses composed with T_rigid_gel, then the standard sensor_action_9d" |
| 146 | ) |
| 147 | |
| 148 | out_dir.mkdir(parents=True, exist_ok=True) |
| 149 | out_path = out_dir / out_name |
| 150 | tmp = out_path.with_name(out_path.name + ".tmp") # NOT with_suffix: it eats ".pt" |
| 151 | torch.save(payload, tmp) |
| 152 | tmp.replace(out_path) |
| 153 | |
| 154 | return { |
| 155 | "path": out_path, |
| 156 | "T": int(pose_l.shape[0]), |
| 157 | "n_actions": int(payload["action_left"].shape[0]), |
| 158 | "flagged": n_flagged, |
| 159 | } |
| 160 | |
| 161 | |
| 162 | def main(): |
| 163 | ap = argparse.ArgumentParser( |
| 164 | description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter |
| 165 | ) |
| 166 | ap.add_argument("--parquet-root", type=Path, default=DEFAULT_PARQUET_ROOT) |
| 167 | ap.add_argument("--videos-root", type=Path, default=DEFAULT_VIDEOS_ROOT) |
| 168 | ap.add_argument("--episodes", nargs="*", default=None, |
| 169 | help="explicit episode stems (default: every parquet in --parquet-root)") |
| 170 | ap.add_argument("--max-rot-deg", type=float, default=30.0, |
| 171 | help="per-step rotation above this is flagged invalid (not dropped)") |
| 172 | ap.add_argument("--gel-frame", action="store_true", |
| 173 | help="express the action in the gel frame (+z = gel normal, origin on " |
| 174 | "the gel surface) instead of the raw OptiTrack rigid frame") |
| 175 | ap.add_argument("--out-name", default=None, |
| 176 | help=f"output filename (default: {OUT_NAME}, or {GEL_OUT_NAME} with --gel-frame)") |
| 177 | ap.add_argument("--overwrite", action="store_true") |
| 178 | args = ap.parse_args() |
| 179 | |
| 180 | out_name = args.out_name or (GEL_OUT_NAME if args.gel_frame else OUT_NAME) |
| 181 | |
| 182 | if args.episodes: |
| 183 | parquets = [args.parquet_root / f"{e}.parquet" for e in args.episodes] |
| 184 | for p in parquets: |
| 185 | if not p.exists(): |
| 186 | raise FileNotFoundError(p) |
| 187 | else: |
| 188 | parquets = sorted(args.parquet_root.glob("*.parquet")) |
| 189 | if not parquets: |
| 190 | raise FileNotFoundError(f"no parquet found under {args.parquet_root}") |
| 191 | |
| 192 | print(f"building {'GEL-frame ' if args.gel_frame else ''}actions for {len(parquets)} " |
| 193 | f"episode(s) -> {args.videos_root}/*/{out_name}") |
| 194 | n_written = n_skipped = 0 |
| 195 | total_flagged = 0 |
| 196 | for parquet in parquets: |
| 197 | out_dir = args.videos_root / parquet.stem |
| 198 | if not out_dir.is_dir(): |
| 199 | # A parquet with no video folder means the two trees disagree; that is a |
| 200 | # data problem worth stopping for, not something to paper over. |
| 201 | raise FileNotFoundError( |
| 202 | f"{parquet.name}: no destination folder {out_dir} — parquet and video trees disagree" |
| 203 | ) |
| 204 | if (out_dir / out_name).exists() and not args.overwrite: |
| 205 | print(f" [skip] {parquet.stem}: {out_name} exists") |
| 206 | n_skipped += 1 |
| 207 | continue |
| 208 | |
| 209 | info = build_episode(parquet, out_dir, args.max_rot_deg, out_name, args.gel_frame) |
| 210 | total_flagged += info["flagged"] |
| 211 | print(f" [write] {parquet.stem}: T={info['T']} actions={info['n_actions']}x9 " |
| 212 | f"per side ({info['flagged']} flagged)") |
| 213 | n_written += 1 |
| 214 | |
| 215 | print(f"done ({n_written} written, {n_skipped} skipped, {total_flagged} flagged steps)") |
| 216 | |
| 217 | |
| 218 | if __name__ == "__main__": |
| 219 | main() |
| 220 | |