File size: 14,457 Bytes
3671737
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
"""Data transforms for BEHAVIOR-1K dataset.

Standard transforms imported from OpenPI.
B1K-specific: TaskIndexToTaskId, ComputeSubtaskStateFromMeta, TokenizeFASTActions

Reference: https://github.com/wensi-ai/openpi/tree/behavior
"""

import dataclasses
import logging
import numpy as np

# Import all standard transforms from OpenPI
from openpi.transforms import (
    DataTransformFn,
    DataDict,
    Group,
    CompositeTransform,
    compose,
    RepackTransform,
    Normalize,
    Unnormalize,
    ResizeImages,
    SubsampleActions,
    DeltaActions,
    AbsoluteActions,
    PadStatesAndActions,
    PromptFromLeRobotTask,  # Not used for PI_BEHAVIOR but kept for compatibility
    flatten_dict,
    unflatten_dict,
    transform_dict,
    apply_tree,
    pad_to_dim,
    make_bool_mask,
)

from b1k.models.pi_behavior_config import TASK_NUM_STAGES
from b1k.shared.normalize import NormStats


@dataclasses.dataclass(frozen=True)
class TaskIndexToTaskId(DataTransformFn):
    """Directly converts task_index to task_id for PI_BEHAVIOR model.
    
    PI_BEHAVIOR uses task embeddings instead of text prompts. This transform
    converts the dataset's task_index to task_id and prepares tokenized_prompt
    as [task_id, subtask_state] for the model.
    
    Assumes:
    - data["task_index"] exists (from dataset)
    - data["subtask_state"] exists (computed by ComputeSubtaskStateFromMeta)
    
    Creates:
    - data["tokenized_prompt"]: np.array([task_id, subtask_state], dtype=int32)
    - data["tokenized_prompt_mask"]: np.array([True, True], dtype=bool)
    """
    
    # Optional task remapping (dataset task_index → model task_id)
    # If None, assumes direct mapping (task_index == task_id)
    task_mapping: dict[int, int] | None = None
    
    def __call__(self, data: DataDict) -> DataDict:
        # During inference, task_id might be provided directly instead of task_index
        if "task_id" in data:
            # Direct task_id provided (inference mode)
            task_id = int(data["task_id"])
        elif "task_index" in data:
            # task_index provided (training mode)
            task_index = int(data["task_index"])
            
            # Apply mapping if provided, otherwise use task_index directly as task_id
            if self.task_mapping is not None:
                if task_index not in self.task_mapping:
                    raise ValueError(f"task_index {task_index} not found in mapping")
                task_id = self.task_mapping[task_index]
            else:
                task_id = task_index
        else:
            # During inference, if neither is provided, this transform should be skipped
            # The tokenized_prompt should already be set up correctly
            if "tokenized_prompt" in data:
                return data  # Already has tokenized_prompt, skip this transform
            raise ValueError("Either task_index, task_id, or tokenized_prompt is required for PI_BEHAVIOR model")

        # Pack task_id and subtask_state (if available) into tokenized_prompt
        if "subtask_state" in data:
            subtask_state = int(data["subtask_state"])
            prompt_tokens = np.array([task_id, subtask_state], dtype=np.int32)  # [task_id, subtask_state]
            prompt_mask = np.array([True, True], dtype=bool)
        else:
            prompt_tokens = np.array([task_id], dtype=np.int32)  # Just [task_id]
            prompt_mask = np.array([True], dtype=bool)

        return {
            **data, 
            "tokenized_prompt": prompt_tokens,
            "tokenized_prompt_mask": prompt_mask
        }


@dataclasses.dataclass(frozen=True)
class ComputeSubtaskStateFromMeta(DataTransformFn):
    """Computes subtask state from timestamp using dataset.meta.episodes.
    
    Divides episode into task-specific number of stages based on episode length.
    Requires dataset reference to access episode metadata.
    
    Args:
        dataset: Dataset instance with meta.episodes containing episode_length
    
    Assumes:
    - data["episode_index"] exists
    - data["timestamp"] exists (frame index within episode)
    - data["task_index"] exists (for task-specific stage count)
    - dataset.meta.episodes contains episode_length for each episode
    
    Creates:
    - data["subtask_state"]: Stage index (0 to num_stages-1)
    """
    
    dataset: object | None = None  # Will be set by data loader
    
    def __call__(self, data: DataDict) -> DataDict:
        if self.dataset is None:
            # During inference or when dataset is not available, use default
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
        
        if "episode_index" not in data or "timestamp" not in data or "task_index" not in data:
            # Missing required fields, default to stage 0
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
        
        episode_index = int(data["episode_index"])
        timestamp = float(data["timestamp"])
        task_index = int(data["task_index"])
        
        # Validate task_index
        if not (0 <= task_index < 50):
            logging.warning(f"Invalid task_index {task_index}, using stage 0")
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
        
        # Get number of stages for this task
        num_stages = TASK_NUM_STAGES[task_index]
        
        # Get episode length from dataset metadata
        if not hasattr(self.dataset, 'meta') or not hasattr(self.dataset.meta, 'episodes'):
            # No metadata available
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
            
        meta_episodes = self.dataset.meta.episodes
        
        if episode_index not in meta_episodes:
            logging.warning(f"Episode {episode_index} not found in metadata, using stage 0")
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
            
        episode_info = meta_episodes[episode_index]
        
        # Try 'length' first (standard key), then 'episode_length' (alternative)
        episode_length = episode_info.get('length', episode_info.get('episode_length', None))
        
        if episode_length is None or episode_length <= 0:
            logging.warning(f"Invalid episode_length for episode {episode_index}, using stage 0")
            data["subtask_state"] = np.array(0, dtype=np.int32)
            return data
        
        episode_length = float(episode_length)
        
        # CRITICAL: Convert timestamp (in seconds) to frames (30 FPS)
        # Dataset provides timestamp in seconds, episode_length is in frames
        current_step = timestamp * 30.0
        
        # Divide episode into num_stages equal parts
        frames_per_stage = episode_length / num_stages
        
        # Compute current stage (0-indexed)
        subtask_state = int(current_step / frames_per_stage)
        
        # Clamp to valid range [0, num_stages-1]
        subtask_state = max(0, min(subtask_state, num_stages - 1))
        
        data["subtask_state"] = np.array(subtask_state, dtype=np.int32)
        return data


@dataclasses.dataclass(frozen=True)
class TokenizeFASTActions(DataTransformFn):
    """Tokenize actions to FAST discrete tokens for PI_BEHAVIOR auxiliary training.
    
    Converts continuous actions to discrete tokens using a trained FAST tokenizer.
    This is used for auxiliary training to improve action quality.
    
    Note: Uses GLOBAL normalization even if per-timestamp normalization is enabled
    elsewhere, to preserve temporal smoothness for better DCT compression.
    
    Args:
        tokenizer_path: Path to trained FAST tokenizer directory
        encoded_dim_ranges: List of (start, end) tuples for dimensions to encode
        max_fast_tokens: Maximum number of tokens (truncate/pad)
        norm_stats: Normalization stats for denorm/renorm if per-timestamp is used
        use_per_timestamp: Whether per-timestamp normalization is being used
    
    Assumes:
    - data["actions"] exists and is normalized
    - data["state"] exists (for delta computation)
    
    Creates:
    - data["fast_tokens"]: Discrete token indices [max_fast_tokens]
    - data["fast_token_mask"]: Boolean mask for valid tokens [max_fast_tokens]
    """
    
    # Path to FAST tokenizer
    tokenizer_path: str
    # Action dimensions to encode: [(start, end), ...]
    encoded_dim_ranges: list[tuple[int, int]]
    # Max tokens to predict
    max_fast_tokens: int = 180
    # Normalization stats (for denorm/renorm if per-timestamp is used)
    norm_stats: dict[str, NormStats] | None = None
    # Whether per-timestamp normalization is being used
    use_per_timestamp: bool = False
    
    def _get_tokenizer(self):
        """Lazy load tokenizer (called in each worker process).

        transformers>=5 can no longer build the saved PreTrainedTokenizerFast via
        UniversalActionProcessor.from_pretrained, so this re-implements its encode path directly:
        DCT over time -> round(coeff * scale) -> shift by min_token -> chars -> BPE ids.
        """
        if not hasattr(self, '_tokenizer'):
            import json
            from pathlib import Path

            from scipy.fft import dct
            from tokenizers import Tokenizer

            tokenizer_dir = Path(self.tokenizer_path)
            cfg = json.loads((tokenizer_dir / "processor_config.json").read_text())
            bpe = Tokenizer.from_file(str(tokenizer_dir / "tokenizer.json"))
            scale, min_token = float(cfg["scale"]), int(cfg["min_token"])

            def encode(action_chunk: np.ndarray) -> list[list[int]]:
                if action_chunk.ndim == 2:
                    action_chunk = action_chunk[None, ...]
                coeff = np.around(dct(action_chunk, axis=1, norm="ortho") * scale)
                return [
                    bpe.encode("".join(map(chr, np.maximum(e.flatten() - min_token, 0).astype(int)))).ids
                    for e in coeff
                ]

            object.__setattr__(self, '_tokenizer', encode)

        return self._tokenizer
    
    def __call__(self, data: DataDict) -> DataDict:
        # Only tokenize if actions are present (training time)
        if "actions" not in data:
            return data
        
        actions = data["actions"]  # [H, D] - may be per-timestamp normalized
        
        # Extract encoded dimensions
        encoded_chunks = []
        for start, end in self.encoded_dim_ranges:
            encoded_chunks.append(actions[:, start:end])
        encoded_actions = np.concatenate(encoded_chunks, axis=-1)  # [H, D_encoded]
        
        # Convert to global QUANTILE normalization for FAST (always, regardless of per_timestamp)
        # This clips outliers and provides consistent [-1, 1] range for DCT compression
        if self.norm_stats is not None:
            action_stats = self.norm_stats['actions']
            
            # Extract stats for encoded dimensions only
            # Build dimension indices from ranges
            encoded_dims = []
            for start, end in self.encoded_dim_ranges:
                encoded_dims.extend(range(start, end))
            encoded_dims = np.array(encoded_dims)
            
            # Denormalize to get back to raw delta actions
            if self.use_per_timestamp:
                # Denormalize from per-timestamp MEAN/STD normalization
                per_ts_mean = action_stats.per_timestamp_mean[:, encoded_dims]  # [H, D_encoded]
                per_ts_std = action_stats.per_timestamp_std[:, encoded_dims]    # [H, D_encoded]
                actions_delta = encoded_actions * per_ts_std + per_ts_mean
            else:
                # Denormalize from global MEAN/STD normalization
                global_mean = action_stats.mean[encoded_dims]  # [D_encoded]
                global_std = action_stats.std[encoded_dims]    # [D_encoded]
                actions_delta = encoded_actions * global_std + global_mean
            
            # Renormalize using GLOBAL QUANTILE normalization (for FAST)
            # This clips to [q01, q99] and maps to [-1, 1]
            q01 = action_stats.q01[encoded_dims]  # [D_encoded]
            q99 = action_stats.q99[encoded_dims]  # [D_encoded]
            actions_delta = np.clip(actions_delta, q01, q99)
            encoded_actions = 2.0 * (actions_delta - q01) / np.maximum(q99 - q01, 1e-6) - 1.0
        
        # Tokenize
        tokenizer = self._get_tokenizer()
        tokens = tokenizer(encoded_actions[None])[0]
        
        if isinstance(tokens, list):
            tokens = np.array(tokens)
        
        # Truncate or pad to max_fast_tokens
        if len(tokens) > self.max_fast_tokens:
            tokens = tokens[:self.max_fast_tokens]
            mask = np.ones(self.max_fast_tokens, dtype=bool)
        else:
            mask = np.concatenate([
                np.ones(len(tokens), dtype=bool),
                np.zeros(self.max_fast_tokens - len(tokens), dtype=bool)
            ])
            tokens = np.pad(tokens, (0, self.max_fast_tokens - len(tokens)), constant_values=0)
        
        data["fast_tokens"] = tokens.astype(np.int32)
        data["fast_token_mask"] = mask
        
        return data



@dataclasses.dataclass(frozen=True)
class OverrideTaskIndex(DataTransformFn):
    """Forces task_index to a fixed model task slot.

    Used to fine-tune a 50-task PI_BEHAVIOR checkpoint on a task it never saw (e.g. 2026 task 57,
    sweeping_garage) by training it in an existing slot, so all parameter shapes stay unchanged.
    """

    task_index: int

    def __call__(self, data: DataDict) -> DataDict:
        return {**data, "task_index": np.array(self.task_index, dtype=np.int64)}


@dataclasses.dataclass(frozen=True)
class ComputeSubtaskStateFromLengths(DataTransformFn):
    """Same stage split as ComputeSubtaskStateFromMeta, but from an {episode_index: length} dict."""

    episode_lengths: dict[int, int]

    def __call__(self, data: DataDict) -> DataDict:
        num_stages = TASK_NUM_STAGES[int(data["task_index"])]
        frames_per_stage = self.episode_lengths[int(data["episode_index"])] / num_stages
        stage = int(float(data["timestamp"]) * 30.0 / frames_per_stage)
        data["subtask_state"] = np.array(max(0, min(stage, num_stages - 1)), dtype=np.int32)
        return data