scripts/perception_states.py

from __future__ import annotations

import argparse
import hashlib
import io
import json
import tarfile
from pathlib import Path
from typing import Any, Iterable

import pyarrow as pa
import pyarrow.parquet as pq
from huggingface_hub import hf_hub_download


LOCK_PATH = Path(__file__).with_name("perception_sources.lock.json")
PRIMARY_STATE_COLUMNS = ("observation.state", "state")


def load_source_lock(path: Path = LOCK_PATH) -> dict[str, Any]:
    lock = json.loads(path.read_text(encoding="utf-8"))
    if lock.get("schema_version") != 1:
        raise ValueError(f"Unsupported perception-source lock schema: {lock.get('schema_version')!r}")
    return lock


def discover_robot_episodes(lock: dict[str, Any], cache_dir: Path | None = None) -> list[dict[str, Any]]:
    episodes: list[dict[str, Any]] = []
    for source_name, source in lock["annotation_sources"].items():
        parquet_path = _download(
            source["repo_id"], source["revision"], source["parquet_path"], cache_dir
        )
        for row in pq.read_table(parquet_path).to_pylist():
            if source_name == "droid":
                repo_id = row["source_dataset"]
                episode_index = int(row["episode_index"])
                episodes.append(
                    {
                        "bench_id": row["bench_id"],
                        "robot_family": "droid",
                        "repo_id": repo_id,
                        "revision": lock["upstream_revisions"][repo_id],
                        "episode_index": episode_index,
                        "fps": float(row["fps"]),
                        "expected_num_frames": int(row["num_frames"]),
                        "source_subset": row["source_subset"],
                        "data_path": f"{row['source_subset']}/data/chunk-000.tar",
                        "tar_member": f"chunk-000/episode_{episode_index:06d}.parquet",
                        "info_path": f"{row['source_subset']}/meta/info.json",
                    }
                )
            elif source_name == "galaxea":
                repo_id = row["source_dataset"]
                episode_index = int(row["source_episode_index"])
                episodes.append(
                    {
                        "bench_id": row["bench_id"],
                        "robot_family": "galaxea",
                        "repo_id": repo_id,
                        "revision": lock["upstream_revisions"][repo_id],
                        "episode_index": episode_index,
                        "fps": float(row["fps"]),
                        "expected_num_frames": int(row["num_frames"]),
                        "data_path": f"data/chunk-000/episode_{episode_index:06d}.parquet",
                        "info_path": "meta/info.json",
                    }
                )
            else:
                raise ValueError(f"Unknown annotation source {source_name!r}")

    episodes.sort(key=lambda item: item["bench_id"])
    bench_ids = [item["bench_id"] for item in episodes]
    if len(bench_ids) != len(set(bench_ids)):
        raise ValueError("Duplicate bench_id in robotic source mappings")
    expected = int(lock["coverage"]["robot_episodes"])
    if len(episodes) != expected:
        raise ValueError(f"Expected {expected} robotic episodes, found {len(episodes)}")
    return episodes


def normalize_episode(
    table: pa.Table,
    episode: dict[str, Any],
    feature_info: dict[str, Any],
) -> dict[str, Any]:
    columns = set(table.column_names)
    primary = next((name for name in PRIMARY_STATE_COLUMNS if name in columns), None)
    if primary is None:
        raise ValueError(f"{episode['bench_id']} has no supported proprioception column")
    if "frame_index" not in columns or "timestamp" not in columns:
        raise ValueError(f"{episode['bench_id']} is missing frame_index or timestamp")

    frame_index = [int(_scalar(value)) for value in table["frame_index"].to_pylist()]
    timestamps = [float(_scalar(value)) for value in table["timestamp"].to_pylist()]
    if frame_index != sorted(frame_index) or timestamps != sorted(timestamps):
        raise ValueError(f"{episode['bench_id']} state rows are not ordered")
    if len(frame_index) != episode["expected_num_frames"]:
        raise ValueError(
            f"{episode['bench_id']} expected {episode['expected_num_frames']} frames, "
            f"found {len(frame_index)} state rows"
        )

    channel_names = [primary]
    channel_names.extend(
        sorted(name for name in columns if name.endswith("_state") and name != primary)
    )
    channels = []
    for name in channel_names:
        values = [_vector(value) for value in table[name].to_pylist()]
        names = list((feature_info.get(name) or {}).get("names") or [])
        width = len(values[0]) if values else 0
        if any(len(value) != width for value in values):
            raise ValueError(f"{episode['bench_id']} channel {name!r} changes width")
        if names and len(names) != width:
            raise ValueError(f"{episode['bench_id']} channel {name!r} names do not match width")
        if not names:
            names = [f"value_{index}" for index in range(width)]
        channels.append({"name": name, "value_names": names, "values": values})

    return {
        "bench_id": episode["bench_id"],
        "robot_family": episode["robot_family"],
        "source_dataset": episode["repo_id"],
        "source_revision": episode["revision"],
        "source_episode_index": episode["episode_index"],
        "fps": episode["fps"],
        "num_frames": len(frame_index),
        "frame_index": frame_index,
        "timestamp_sec": timestamps,
        "primary_channel": primary,
        "channels": channels,
    }


def export_perception_states(
    output_path: Path,
    *,
    lock_path: Path = LOCK_PATH,
    cache_dir: Path | None = None,
    bench_ids: Iterable[str] | None = None,
) -> dict[str, Any]:
    lock = load_source_lock(lock_path)
    episodes = discover_robot_episodes(lock, cache_dir)
    selected = set(bench_ids or [])
    if selected:
        known = {episode["bench_id"] for episode in episodes}
        unknown = selected - known
        if unknown:
            raise ValueError(f"Unknown robotic bench IDs: {sorted(unknown)}")
        episodes = [episode for episode in episodes if episode["bench_id"] in selected]

    rows = []
    for episode in episodes:
        table, feature_info = _load_episode(episode, cache_dir)
        rows.append(normalize_episode(table, episode, feature_info))

    output_path.parent.mkdir(parents=True, exist_ok=True)
    table = pa.Table.from_pylist(rows, schema=perception_schema())
    pq.write_table(table, output_path, compression="zstd")
    digest = hashlib.sha256(output_path.read_bytes()).hexdigest()
    provenance = {
        "schema_version": 1,
        "artifact": output_path.name,
        "sha256": digest,
        "episodes": len(rows),
        "bench_ids": [row["bench_id"] for row in rows],
        "source_lock": lock_path.name,
        "source_lock_sha256": hashlib.sha256(lock_path.read_bytes()).hexdigest(),
        "video_only_episodes": lock["coverage"]["video_only_episodes"],
    }
    provenance_path = output_path.with_suffix(output_path.suffix + ".provenance.json")
    provenance_path.write_text(json.dumps(provenance, indent=2) + "\n", encoding="utf-8")
    return provenance


def perception_schema() -> pa.Schema:
    return pa.schema(
        [
            ("bench_id", pa.string()),
            ("robot_family", pa.string()),
            ("source_dataset", pa.string()),
            ("source_revision", pa.string()),
            ("source_episode_index", pa.int64()),
            ("fps", pa.float64()),
            ("num_frames", pa.int64()),
            ("frame_index", pa.list_(pa.int64())),
            ("timestamp_sec", pa.list_(pa.float64())),
            ("primary_channel", pa.string()),
            (
                "channels",
                pa.list_(
                    pa.struct(
                        [
                            ("name", pa.string()),
                            ("value_names", pa.list_(pa.string())),
                            ("values", pa.list_(pa.list_(pa.float64()))),
                        ]
                    )
                ),
            ),
        ]
    )


def _load_episode(episode: dict[str, Any], cache_dir: Path | None) -> tuple[pa.Table, dict[str, Any]]:
    info_path = _download(
        episode["repo_id"], episode["revision"], episode["info_path"], cache_dir
    )
    feature_info = json.loads(info_path.read_text(encoding="utf-8")).get("features", {})
    data_path = _download(
        episode["repo_id"], episode["revision"], episode["data_path"], cache_dir
    )
    if episode.get("tar_member"):
        with tarfile.open(data_path) as archive:
            member = archive.extractfile(episode["tar_member"])
            if member is None:
                raise FileNotFoundError(episode["tar_member"])
            table = pq.read_table(io.BytesIO(member.read()))
    else:
        table = pq.read_table(data_path)
    return table, feature_info


def _download(repo_id: str, revision: str, filename: str, cache_dir: Path | None) -> Path:
    return Path(
        hf_hub_download(
            repo_id=repo_id,
            repo_type="dataset",
            revision=revision,
            filename=filename,
            cache_dir=str(cache_dir) if cache_dir else None,
        )
    )


def _scalar(value: Any) -> Any:
    if isinstance(value, (list, tuple)):
        if len(value) != 1:
            raise ValueError(f"Expected a scalar or one-element sequence, got {value!r}")
        return value[0]
    return value


def _vector(value: Any) -> list[float]:
    if value is None:
        return []
    if not isinstance(value, (list, tuple)):
        value = [value]
    return [float(item) for item in value]


def main() -> None:
    parser = argparse.ArgumentParser(
        description="Export pinned proprioception sidecars for WGO-Bench robotic episodes."
    )
    parser.add_argument("output", type=Path)
    parser.add_argument("--lock", type=Path, default=LOCK_PATH)
    parser.add_argument("--cache-dir", type=Path)
    parser.add_argument("--bench-id", action="append", default=[])
    args = parser.parse_args()
    result = export_perception_states(
        args.output,
        lock_path=args.lock,
        cache_dir=args.cache_dir,
        bench_ids=args.bench_id,
    )
    print(json.dumps(result, indent=2))


if __name__ == "__main__":
    main()