File size: 14,462 Bytes
ab621ab
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
"""Training data of the baselines: a task's LeRobot v2.1 expert dataset read through a robot contract.

A contract names the cameras and which dataset columns form the policy state and the action rows, the same layouts
the registered GR00T / Psi0 checkpoints were trained on:

- `g1_sonic_dfx6` (G1 + INSPIRE, the evaluation/README.md interface; ego_view, chest): state44 =
  `observation.state[0:29]` body, measured `observation.inspire_{left,right}_hand_state`,
  `observation.projected_gravity`; action76 (`sonic_dfx6`) = `action.motion_token` 64 and the measured DFX6 of the
  same row (row 0 = the current opening);
- `h2_sharpa` (H2 + Sharpa, debug/newton/h2_sharpa_policy_eval.py; head, chest): state78 = `observation.state` 75,
  `observation.projected_gravity` 3; action75 = `action.body_target` 31, `action.hands` 44 (absolute targets);
- `h2_sharpa_planner` (H2 + Sharpa a14 v2, h2_policy_planner_eval.py; head, chest): state78; action82 = action75 +
  `action.planner` 7.

Frames: every camera's video is decoded once (PyAV), each frame resized with `resize_frame` (OpenCV INTER_AREA, the
same function the policy runtime applies to live frames) and stored as a uint8 memory map in a cache directory keyed by
the dataset, episodes, camera and size. A training sample is the frame t of an episode: the cameras and the state at t,
and the action rows t..t+horizon-1 (rows past the episode's end repeat its last row, as DextraVerse's windows).
"""
from __future__ import annotations

from dataclasses import dataclass
import hashlib
import json
import os
from pathlib import Path
import sys

import numpy as np

CACHE_VERSION = 1


@dataclass(frozen=True)
class Contract:
    name: str
    robot: str
    cameras: tuple
    state: tuple   # ((column, start, end), ...) concatenated in order
    action: tuple
    action_space: str | None = None  # the G1 evaluation interface's action space, if any
    body_dims: int = 0  # leading raw-radian body joint angles of the state (augmentation spec v2 perturbs these)

    @property
    def state_dim(self) -> int:
        return sum(end - start for _, start, end in self.state)

    @property
    def action_dim(self) -> int:
        return sum(end - start for _, start, end in self.action)

    def to_dict(self, cameras=None) -> dict:
        return dict(name=self.name, robot=self.robot, cameras=list(cameras or self.cameras),
                    state=[list(s) for s in self.state], action=[list(a) for a in self.action],
                    state_dim=self.state_dim, action_dim=self.action_dim, action_space=self.action_space,
                    body_dims=self.body_dims)


_G1_HANDS = (('observation.inspire_left_hand_state', 0, 6), ('observation.inspire_right_hand_state', 0, 6))
_H2_STATE = (('observation.state', 0, 75), ('observation.projected_gravity', 0, 3))
_H2_ACTION = (('action.body_target', 0, 31), ('action.hands', 0, 44))
# body_dims: G1 observation.state[0:29] are the 29 body joints; H2 observation.state[0:31] are legs 6+6, waist 3,
# head 2, arms 7+7 (docs/unitree_h2_sharpa.md), then the Sharpa hands.
CONTRACTS = {c.name: c for c in (
    Contract('g1_sonic_dfx6', 'g1', ('ego_view', 'chest'),
             (('observation.state', 0, 29), *_G1_HANDS, ('observation.projected_gravity', 0, 3)),
             (('action.motion_token', 0, 64), *_G1_HANDS), action_space='sonic_dfx6', body_dims=29),
    Contract('h2_sharpa', 'h2_sharpa', ('head', 'chest'), _H2_STATE, _H2_ACTION, body_dims=31),
    Contract('h2_sharpa_planner', 'h2_sharpa', ('head', 'chest'), _H2_STATE,
             (*_H2_ACTION, ('action.planner', 0, 7)), body_dims=31),
)}


def augmentation_module():
    """evaluation/augmentation: the augmentation spec v2 shared by every model's training (imported on demand)."""
    evaluation = str(Path(__file__).resolve().parents[1])
    if evaluation not in sys.path:
        sys.path.insert(0, evaluation)
    import augmentation
    return augmentation


def resize_frame(frame: np.ndarray, hw) -> np.ndarray:
    """(H, W, 3) uint8 RGB -> (h, w, 3) uint8, OpenCV INTER_AREA (training cache and live frames alike)."""
    import cv2
    frame = np.ascontiguousarray(frame, dtype=np.uint8)
    if frame.ndim != 3 or frame.shape[2] != 3:
        raise ValueError(f'expected an (H, W, 3) RGB frame, got {frame.shape}')
    h, w = int(hw[0]), int(hw[1])
    if frame.shape[:2] == (h, w):
        return frame
    return cv2.resize(frame, (w, h), interpolation=cv2.INTER_AREA)


class LeRobotDataset:
    """A LeRobot v2.1 dataset directory (meta/info.json, meta/episodes.jsonl, data/, videos/)."""

    def __init__(self, root):
        self.root = Path(root).expanduser().resolve()
        self.info = json.loads((self.root / 'meta/info.json').read_text())
        if self.info.get('codebase_version') != 'v2.1':
            raise ValueError(f"{self.root}: LeRobot {self.info.get('codebase_version')}, expected v2.1")
        rows = [json.loads(line) for line in (self.root / 'meta/episodes.jsonl').read_text().splitlines() if line]
        self.lengths = {int(r['episode_index']): int(r['length']) for r in rows}
        tasks = [json.loads(line) for line in (self.root / 'meta/tasks.jsonl').read_text().splitlines() if line]
        self.tasks = {int(t['task_index']): t['task'] for t in tasks}
        self.fps = float(self.info['fps'])

    @property
    def name(self) -> str:
        """The dataset's folder name (its parent's for an export folder named `lerobot`)."""
        return self.root.parent.name if self.root.name == 'lerobot' else self.root.name

    def chunk(self, episode: int) -> int:
        return episode // int(self.info['chunks_size'])

    def parquet(self, episode: int) -> Path:
        return self.root / self.info['data_path'].format(episode_chunk=self.chunk(episode), episode_index=episode)

    def video(self, episode: int, camera: str) -> Path:
        return self.root / self.info['video_path'].format(episode_chunk=self.chunk(episode), episode_index=episode,
                                                          video_key=f'observation.images.{camera}')

    def check_contract(self, contract: Contract, cameras):
        features = self.info['features']
        for column, start, end in (*contract.state, *contract.action):
            if column not in features:
                raise KeyError(f'{self.root.name}: no column {column!r} (contract {contract.name})')
            width = int(np.prod(features[column]['shape']))
            if end > width:
                raise ValueError(f'{column} has {width} values; the contract reads {start}:{end}')
        for camera in cameras:
            key = f'observation.images.{camera}'
            if features.get(key, {}).get('dtype') != 'video':
                raise KeyError(f'{self.root.name}: no video {key!r}')


def parse_episodes(spec: str | None, available) -> list:
    """'0:100', '0,3,7' or None (all), restricted to the dataset's episodes, sorted."""
    available = sorted(available)
    if not spec:
        return available
    chosen = set()
    for part in spec.split(','):
        if ':' in part:
            lo, hi = part.split(':')
            chosen.update(range(int(lo or 0), int(hi) if hi else available[-1] + 1))
        else:
            chosen.add(int(part))
    missing = sorted(chosen - set(available))
    if missing:
        raise ValueError(f'episodes not in the dataset: {missing[:10]}')
    return sorted(chosen)


def load_lowdim(dataset: LeRobotDataset, contract: Contract, episodes) -> dict:
    """State (N, state_dim) and action (N, action_dim) float32 of the episodes in order, plus each episode's start
    row, length and task text."""
    import pyarrow.parquet as pq
    columns = sorted({c for c, _, _ in (*contract.state, *contract.action)} | {'frame_index', 'task_index'})
    states, actions, starts, lengths, tasks, offset = [], [], [], [], set(), 0
    for episode in episodes:
        table = pq.read_table(dataset.parquet(episode), columns=columns)
        data = {c: np.asarray(table.column(c).to_pylist(), dtype=np.float64) for c in columns}
        n = len(data['frame_index'])
        if n != dataset.lengths[episode] or not np.array_equal(data['frame_index'], np.arange(n)):
            raise ValueError(f'episode {episode}: {n} rows / frame_index differ from episodes.jsonl')

        def pick(spec):
            return np.concatenate([data[c].reshape(n, -1)[:, s:e] for c, s, e in spec], axis=1).astype(np.float32)

        states.append(pick(contract.state))
        actions.append(pick(contract.action))
        tasks.update(dataset.tasks[int(i)] for i in np.unique(data['task_index']))
        starts.append(offset)
        lengths.append(n)
        offset += n
    state, action = np.concatenate(states), np.concatenate(actions)
    if not (np.isfinite(state).all() and np.isfinite(action).all()):
        raise ValueError('non-finite state or action values')
    return dict(state=state, action=action, starts=np.asarray(starts), lengths=np.asarray(lengths),
                episodes=list(episodes), tasks=sorted(tasks))


def statistics(lowdim: dict, rows: np.ndarray) -> dict:
    out = {}
    for key in ('state', 'action'):
        x = lowdim[key][rows].astype(np.float64)
        out[key] = dict(min=x.min(0).tolist(), max=x.max(0).tolist(), mean=x.mean(0).tolist(), std=x.std(0).tolist())
    return out


# -- frame cache --------------------------------------------------------------------------------------------------
def _decode_into(job):
    path, out_path, total, start, length, hw = job
    import av
    frames = np.memmap(out_path, dtype=np.uint8, mode='r+', shape=(total, hw[0], hw[1], 3))
    count = 0
    with av.open(str(path)) as container:
        stream = container.streams.video[0]
        stream.thread_type = 'AUTO'
        for frame in container.decode(stream):
            if count < length:
                frames[start + count] = resize_frame(frame.to_ndarray(format='rgb24'), hw)
            count += 1
    frames.flush()
    del frames
    if count != length:
        raise ValueError(f'{path}: {count} video frames, {length} data rows')
    return length


def frame_cache(dataset: LeRobotDataset, lowdim: dict, cameras, hw, cache_root, workers=8) -> dict:
    """{camera: path of the (N, h, w, 3) uint8 memory map} for the episodes of `lowdim`, decoded once and reused."""
    from multiprocessing import get_context
    total = int(lowdim['lengths'].sum())
    paths = {}
    for camera in cameras:
        key = json.dumps(dict(version=CACHE_VERSION, root=str(dataset.root), episodes=lowdim['episodes'],
                              lengths=lowdim['lengths'].tolist(), camera=camera, hw=list(hw)), sort_keys=True)
        digest = hashlib.sha256(key.encode()).hexdigest()[:16]
        folder = Path(cache_root).expanduser() / f'{dataset.name}_{camera}_{hw[0]}x{hw[1]}_{digest}'
        folder.mkdir(parents=True, exist_ok=True)
        out, meta = folder / 'frames.u8', folder / 'meta.json'
        if meta.exists() and json.loads(meta.read_text()).get('complete'):
            paths[camera] = out
            continue
        np.memmap(out, dtype=np.uint8, mode='w+', shape=(total, hw[0], hw[1], 3)).flush()
        jobs = [(dataset.video(e, camera), out, total, int(s), int(n), tuple(hw))
                for e, s, n in zip(lowdim['episodes'], lowdim['starts'], lowdim['lengths'])]
        with get_context('spawn').Pool(max(1, int(workers))) as pool:  # the caller may hold torch threads
            for done, _ in enumerate(pool.imap_unordered(_decode_into, jobs), 1):
                if done % 20 == 0 or done == len(jobs):
                    print(json.dumps(dict(event='frame_cache', camera=camera, episodes=done, of=len(jobs))),
                          flush=True)
        meta.write_text(json.dumps(dict(json.loads(key), frames=total, complete=True), indent=1))
        paths[camera] = out
    return paths


class ChunkDataset:
    """torch-style dataset of (images (n_cameras, h, w, 3) uint8, state (state_dim,), action (horizon, action_dim))
    over the frames of the selected episodes (positions into lowdim['episodes']). Memory maps open lazily per
    process (DataLoader workers).

    `body_dims` > 0 turns on the training augmentation (docs/augmentation_v2.md, evaluation/augmentation): per item,
    the cached frames of all cameras get one hb_aug_v2.augment call (one time step, no future targets) and the first
    `body_dims` state values the per-sample joint bias + noise; the action rows are never touched. 0 (evaluation,
    offline checks) returns the cached frames and the recorded state unchanged, deterministically."""

    def __init__(self, lowdim: dict, frames: dict, cameras, hw, horizon: int, positions, body_dims: int = 0):
        self.lowdim, self.paths, self.cameras, self.hw, self.horizon = lowdim, frames, list(cameras), tuple(hw), horizon
        self.positions, self.body_dims = list(positions), int(body_dims)
        if self.body_dims:
            augmentation_module()  # asserts the spec version before any worker starts
        self.total = int(lowdim['lengths'].sum())
        index, end = [], np.zeros(self.total, np.int64)
        for p in positions:
            s, n = int(lowdim['starts'][p]), int(lowdim['lengths'][p])
            index.append(np.arange(s, s + n))
            end[s:s + n] = s + n
        self.index, self.end = np.concatenate(index), end
        self._maps, self._pid = None, None

    def __len__(self):
        return len(self.index)

    def _frames(self):
        if self._maps is None or self._pid != os.getpid():
            self._maps = {c: np.memmap(self.paths[c], dtype=np.uint8, mode='r', shape=(self.total, *self.hw, 3))
                          for c in self.cameras}
            self._pid = os.getpid()
        return self._maps

    def rows(self, i: int) -> np.ndarray:
        return np.minimum(np.arange(i, i + self.horizon), self.end[i] - 1)

    def __getitem__(self, k):
        import torch
        i = int(self.index[k])
        maps = self._frames()
        images = torch.from_numpy(np.stack([maps[c][i] for c in self.cameras]))
        state = self.lowdim['state'][i]
        if self.body_dims:
            augmentation = augmentation_module()
            images, _ = augmentation.augment_frames_uint8(images)
            state = augmentation.perturb_body(state, self.body_dims)
        return images, torch.from_numpy(state), torch.from_numpy(self.lowdim['action'][self.rows(i)])