Luke-Xing commited on
Commit
3671737
·
verified ·
1 Parent(s): 8d5d556

Upload 27 files

Browse files
Files changed (27) hide show
  1. task 57/policy_code/commit.txt +1 -0
  2. task 57/policy_code/scripts/compute_norm_stats.py +694 -0
  3. task 57/policy_code/scripts/serve_b1k.py +190 -0
  4. task 57/policy_code/scripts/serve_b1k_2026.py +149 -0
  5. task 57/policy_code/scripts/train.py +450 -0
  6. task 57/policy_code/scripts/train_fast_tokenizer.py +377 -0
  7. task 57/policy_code/src/b1k/__init__.py +9 -0
  8. task 57/policy_code/src/b1k/models/__init__.py +6 -0
  9. task 57/policy_code/src/b1k/models/observation.py +143 -0
  10. task 57/policy_code/src/b1k/models/pi_behavior.py +1118 -0
  11. task 57/policy_code/src/b1k/models/pi_behavior_config.py +172 -0
  12. task 57/policy_code/src/b1k/policies/__init__.py +4 -0
  13. task 57/policy_code/src/b1k/policies/b1k_policy.py +166 -0
  14. task 57/policy_code/src/b1k/policies/checkpoint_switcher.py +181 -0
  15. task 57/policy_code/src/b1k/policies/pi_behavior_policy.py +125 -0
  16. task 57/policy_code/src/b1k/policies/policy_config.py +119 -0
  17. task 57/policy_code/src/b1k/shared/__init__.py +4 -0
  18. task 57/policy_code/src/b1k/shared/correction_rules.py +333 -0
  19. task 57/policy_code/src/b1k/shared/eval_b1k_wrapper.py +307 -0
  20. task 57/policy_code/src/b1k/shared/normalize.py +172 -0
  21. task 57/policy_code/src/b1k/training/__init__.py +6 -0
  22. task 57/policy_code/src/b1k/training/checkpoints.py +209 -0
  23. task 57/policy_code/src/b1k/training/config.py +559 -0
  24. task 57/policy_code/src/b1k/training/data_loader.py +285 -0
  25. task 57/policy_code/src/b1k/training/weight_loaders.py +87 -0
  26. task 57/policy_code/src/b1k/transforms.py +352 -0
  27. task 57/policy_code/src/b1k/transforms_normalize.py +124 -0
task 57/policy_code/commit.txt ADDED
@@ -0,0 +1 @@
 
 
1
+ 3a16af8b79fd95b3062357e2c288a0103b44c18c
task 57/policy_code/scripts/compute_norm_stats.py ADDED
@@ -0,0 +1,694 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Compute normalization statistics for BEHAVIOR-1K dataset.
2
+
3
+ Computes mean, std, min, max from dataset in parallel.
4
+ Supports per-timestamp normalization and action correlation matrices.
5
+ """
6
+
7
+ import numpy as np
8
+ import pandas as pd
9
+ import tyro
10
+ from pathlib import Path
11
+ from concurrent.futures import ProcessPoolExecutor, as_completed
12
+ import multiprocessing as mp
13
+
14
+ import openpi.models.model as _model
15
+ import openpi.transforms as transforms
16
+
17
+ # Import B1K-specific modules
18
+ from b1k.shared import normalize
19
+ from b1k.training import config as _config
20
+ from b1k.policies.b1k_policy import extract_state_from_proprio
21
+
22
+
23
+ def get_delta_transform_from_config(config_name: str):
24
+ """Get the delta action transform from the config."""
25
+ config = _config.get_config(config_name)
26
+ data_config = config.data.create(config.assets_dirs, config.model)
27
+
28
+ # Find the DeltaActions transform in the data transforms
29
+ delta_transform = None
30
+ for transform in data_config.data_transforms.inputs:
31
+ if isinstance(transform, transforms.DeltaActions):
32
+ delta_transform = transform
33
+ break
34
+
35
+ if delta_transform is None:
36
+ raise ValueError("No DeltaActions transform found in config")
37
+
38
+ return delta_transform
39
+
40
+
41
+ def apply_delta_transform_from_config(state: np.ndarray, actions: np.ndarray, mask) -> np.ndarray:
42
+ """Apply delta action transform using the mask from config.
43
+
44
+ Args:
45
+ state: Current state (23-dim)
46
+ actions: Absolute actions (23-dim)
47
+ mask: Boolean mask from DeltaActions transform
48
+
49
+ Returns:
50
+ delta_actions: Delta actions relative to current state (23-dim)
51
+ """
52
+ if mask is None:
53
+ return actions
54
+
55
+ delta_actions = actions.copy()
56
+ mask = np.asarray(mask)
57
+ dims = mask.shape[-1]
58
+
59
+ # Apply delta transform: delta = action - state for specified dimensions
60
+ delta_actions[:dims] = np.where(mask, actions[:dims] - state[:dims], actions[:dims])
61
+
62
+ return delta_actions
63
+
64
+
65
+ def process_episode_file(args):
66
+ """Process a single episode file and return statistics."""
67
+ episode_file, delta_mask, action_horizon, compute_per_timestamp, compute_correlation, sample_fraction = args
68
+
69
+ try:
70
+ # Read parquet file directly
71
+ df = pd.read_parquet(episode_file)
72
+
73
+ # Extract states and actions
74
+ states = []
75
+ raw_actions = [] # Keep raw actions for per-timestamp processing
76
+ actions = []
77
+
78
+ for _, row in df.iterrows():
79
+ # Get raw proprioception and actions
80
+ raw_state = np.array(row["observation.state"]) # 256-dim
81
+ raw_action = np.array(row["action"]) # 23-dim
82
+
83
+ # Apply state extraction (same as training/inference)
84
+ processed_state = extract_state_from_proprio(raw_state) # 23-dim
85
+
86
+ # Apply delta transform to actions (for regular statistics)
87
+ delta_action = apply_delta_transform_from_config(processed_state, raw_action, delta_mask)
88
+
89
+ states.append(processed_state)
90
+ raw_actions.append(raw_action)
91
+ actions.append(delta_action)
92
+
93
+ if len(states) == 0:
94
+ return None, None
95
+
96
+ states = np.array(states)
97
+ raw_actions = np.array(raw_actions)
98
+ actions = np.array(actions)
99
+
100
+ # Compute episode statistics (including min/max which we'll use instead of quantiles)
101
+ episode_stats = {
102
+ "state": {
103
+ "count": len(states),
104
+ "sum": np.sum(states, axis=0),
105
+ "sum_sq": np.sum(states**2, axis=0),
106
+ "min": np.min(states, axis=0),
107
+ "max": np.max(states, axis=0),
108
+ },
109
+ "actions": {
110
+ "count": len(actions),
111
+ "sum": np.sum(actions, axis=0),
112
+ "sum_sq": np.sum(actions**2, axis=0),
113
+ "min": np.min(actions, axis=0),
114
+ "max": np.max(actions, axis=0),
115
+ }
116
+ }
117
+
118
+ # Compute per-timestamp statistics and action chunks if requested
119
+ per_timestamp_data = None
120
+ correlation_chunks = None
121
+ if compute_per_timestamp or compute_correlation:
122
+ # Create action chunks for per-timestamp statistics
123
+ # For each timestep, take the current state and next action_horizon absolute actions
124
+ action_chunks = []
125
+ for i in range(len(states) - action_horizon + 1):
126
+ current_state = states[i] # State at time i
127
+ # Take next action_horizon absolute actions starting from time i
128
+ future_absolute_actions = raw_actions[i:i + action_horizon] # Shape: (action_horizon, action_dim)
129
+
130
+ # Apply delta transform to each action using the SAME current state
131
+ delta_chunk = np.zeros_like(future_absolute_actions)
132
+ for t in range(action_horizon):
133
+ delta_chunk[t] = apply_delta_transform_from_config(current_state, future_absolute_actions[t], delta_mask)
134
+
135
+ action_chunks.append(delta_chunk)
136
+
137
+ if action_chunks:
138
+ action_chunks = np.array(action_chunks) # Shape: (num_chunks, action_horizon, action_dim)
139
+
140
+ # Sample chunks (not individual actions!) to reduce memory usage
141
+ # Use fixed seed per episode for reproducibility
142
+ rng = np.random.RandomState(hash(str(episode_file)) % (2**31))
143
+ n_chunk_samples = max(1, int(len(action_chunks) * sample_fraction))
144
+
145
+ if sample_fraction < 1.0 and n_chunk_samples < len(action_chunks):
146
+ chunk_indices = rng.choice(len(action_chunks), size=n_chunk_samples, replace=False)
147
+ sampled_chunks = action_chunks[chunk_indices]
148
+ else:
149
+ sampled_chunks = action_chunks
150
+
151
+ if compute_per_timestamp:
152
+ per_timestamp_data = sampled_chunks
153
+ if compute_correlation:
154
+ correlation_chunks = sampled_chunks
155
+
156
+ # Don't return full states/actions to save memory - we have everything we need in episode_stats
157
+ return episode_stats, (per_timestamp_data, correlation_chunks)
158
+
159
+ except Exception as e:
160
+ print(f"Error processing {episode_file}: {e}")
161
+ return None, None
162
+
163
+
164
+ def aggregate_episode_stats(episode_stats_list, all_data_list, config, compute_correlation=False, max_correlation_samples=2000000, compute_quantiles_sample_size=1000000):
165
+ """Aggregate statistics from multiple episodes."""
166
+
167
+ # Collect per-timestamp data if available
168
+ per_timestamp_chunks = []
169
+ for data in all_data_list:
170
+ if data is not None and len(data) > 0 and data[0] is not None:
171
+ per_timestamp_chunks.append(data[0])
172
+
173
+ # Collect correlation chunks if available
174
+ correlation_chunks = []
175
+ for data in all_data_list:
176
+ if data is not None and len(data) > 1 and data[1] is not None:
177
+ correlation_chunks.append(data[1])
178
+
179
+ # Collect raw data for quantile computation (sample if too large)
180
+ state_data_for_quantiles = []
181
+ action_data_for_quantiles = []
182
+
183
+ for data in all_data_list:
184
+ if data is not None and len(data) > 0 and data[0] is not None:
185
+ # data[0] is per_timestamp chunks: [N, H, D]
186
+ chunks = data[0]
187
+ # For actions, we have action chunks
188
+ # For states, we would need separate collection (skip for now, use min/max)
189
+ action_data_for_quantiles.append(chunks)
190
+
191
+ # Aggregate counts, sums, etc.
192
+ final_stats = {"state": {}, "actions": {}}
193
+
194
+ for key in ["state", "actions"]:
195
+ total_count = sum(stats[key]["count"] for stats in episode_stats_list if stats is not None)
196
+ total_sum = np.sum([stats[key]["sum"] for stats in episode_stats_list if stats is not None], axis=0)
197
+ total_sum_sq = np.sum([stats[key]["sum_sq"] for stats in episode_stats_list if stats is not None], axis=0)
198
+ global_min = np.min([stats[key]["min"] for stats in episode_stats_list if stats is not None], axis=0)
199
+ global_max = np.max([stats[key]["max"] for stats in episode_stats_list if stats is not None], axis=0)
200
+
201
+ # Compute final statistics
202
+ mean = total_sum / total_count
203
+ variance = (total_sum_sq / total_count) - mean**2
204
+ std = np.sqrt(np.maximum(0, variance))
205
+
206
+ # Compute actual percentiles for actions (sample if needed for memory efficiency)
207
+ if key == "actions" and action_data_for_quantiles:
208
+ print(f"Computing actual percentiles for actions...")
209
+ # Concatenate all action chunks and flatten to [N*H, D]
210
+ all_action_data = np.vstack(action_data_for_quantiles) # [N, H, D]
211
+ all_action_data_flat = all_action_data.reshape(-1, all_action_data.shape[-1]) # [N*H, D]
212
+
213
+ # Sample if too large for memory
214
+ if len(all_action_data_flat) > compute_quantiles_sample_size:
215
+ print(f" Sampling {compute_quantiles_sample_size:,} from {len(all_action_data_flat):,} data points")
216
+ rng = np.random.RandomState(42)
217
+ indices = rng.choice(len(all_action_data_flat), size=compute_quantiles_sample_size, replace=False)
218
+ all_action_data_flat = all_action_data_flat[indices]
219
+
220
+ q01 = np.percentile(all_action_data_flat, 1, axis=0)
221
+ q99 = np.percentile(all_action_data_flat, 99, axis=0)
222
+ print(f" Computed q01/q99 from {len(all_action_data_flat):,} samples")
223
+ else:
224
+ # Fallback to min/max for state (no chunks collected)
225
+ q01 = global_min
226
+ q99 = global_max
227
+
228
+ final_stats[key] = {
229
+ "mean": mean,
230
+ "std": std,
231
+ "q01": q01,
232
+ "q99": q99,
233
+ }
234
+
235
+ # Compute per-timestamp statistics for actions if available
236
+ per_timestamp_stats = None
237
+ if per_timestamp_chunks and key == "actions":
238
+ # Combine all chunks: (num_episodes * num_chunks_per_episode, action_horizon, action_dim)
239
+ all_chunks = np.vstack(per_timestamp_chunks)
240
+
241
+ # Compute statistics for each timestamp
242
+ action_horizon, action_dim = all_chunks.shape[1], all_chunks.shape[2]
243
+ per_timestamp_mean = np.zeros((action_horizon, action_dim))
244
+ per_timestamp_std = np.zeros((action_horizon, action_dim))
245
+ per_timestamp_q01 = np.zeros((action_horizon, action_dim))
246
+ per_timestamp_q99 = np.zeros((action_horizon, action_dim))
247
+
248
+ for t in range(action_horizon):
249
+ timestep_data = all_chunks[:, t, :] # Shape: (num_chunks, action_dim)
250
+ per_timestamp_mean[t] = np.mean(timestep_data, axis=0)
251
+ per_timestamp_std[t] = np.std(timestep_data, axis=0)
252
+ per_timestamp_q01[t] = np.percentile(timestep_data, 1, axis=0)
253
+ per_timestamp_q99[t] = np.percentile(timestep_data, 99, axis=0)
254
+
255
+ per_timestamp_stats = {
256
+ "per_timestamp_mean": per_timestamp_mean,
257
+ "per_timestamp_std": per_timestamp_std,
258
+ "per_timestamp_q01": per_timestamp_q01,
259
+ "per_timestamp_q99": per_timestamp_q99,
260
+ }
261
+
262
+ # Compute full correlation matrix for actions if available
263
+ correlation_stats = None
264
+ if compute_correlation and not correlation_chunks:
265
+ raise RuntimeError(
266
+ "Correlation computation was requested but no correlation chunks were collected. "
267
+ "This indicates an issue with episode processing or insufficient data."
268
+ )
269
+
270
+ if correlation_chunks:
271
+ print("Computing full action correlation matrix...")
272
+ # Combine all chunks: (num_episodes * num_chunks_per_episode, action_horizon, action_dim)
273
+ all_corr_chunks = np.vstack(correlation_chunks)
274
+ action_horizon, action_dim = all_corr_chunks.shape[1], all_corr_chunks.shape[2]
275
+ target_action_dim = config.model.action_dim # 32 (with padding)
276
+
277
+ print(f"Total action chunks available: {all_corr_chunks.shape[0]} of shape ({action_horizon}, {action_dim})")
278
+
279
+ # PAD action dimensions BEFORE computing correlations
280
+ if action_dim < target_action_dim:
281
+ print(f"Padding actions from {action_dim}D to {target_action_dim}D...")
282
+ # Pad with zeros: (num_samples, action_horizon, action_dim) -> (num_samples, action_horizon, target_action_dim)
283
+ padding = np.zeros((all_corr_chunks.shape[0], action_horizon, target_action_dim - action_dim))
284
+ all_corr_chunks_padded = np.concatenate([all_corr_chunks, padding], axis=2)
285
+ print(f"Padded chunks shape: {all_corr_chunks_padded.shape}")
286
+ else:
287
+ all_corr_chunks_padded = all_corr_chunks[:, :, :target_action_dim]
288
+
289
+ # Flatten chunks to (num_samples, action_horizon * target_action_dim)
290
+ flattened_chunks = all_corr_chunks_padded.reshape(-1, action_horizon * target_action_dim)
291
+ total_samples = flattened_chunks.shape[0]
292
+
293
+ # Subsample if we have too many samples (for computational efficiency)
294
+ if total_samples > max_correlation_samples:
295
+ print(f"Subsampling {max_correlation_samples} random chunks for correlation computation (from {total_samples})")
296
+ print(f"This provides ~{100 * max_correlation_samples / total_samples:.1f}x speedup with negligible accuracy loss")
297
+ rng = np.random.RandomState(42) # Fixed seed for reproducibility
298
+ sample_indices = rng.choice(total_samples, size=max_correlation_samples, replace=False)
299
+ flattened_chunks = flattened_chunks[sample_indices]
300
+ print(f"Using {flattened_chunks.shape[0]} sampled chunks for correlation matrix computation")
301
+ else:
302
+ print(f"Using all {total_samples} chunks (no subsampling needed)")
303
+
304
+ # Normalize each dimension to zero mean and unit variance for correlation computation
305
+ # This ensures the correlation matrix captures pure correlation structure
306
+ chunk_mean = np.mean(flattened_chunks, axis=0)
307
+ chunk_std = np.std(flattened_chunks, axis=0)
308
+
309
+ # Identify constant dimensions (std ≈ 0, including padded dimensions)
310
+ constant_dims = chunk_std < 1e-6
311
+ print(f"Found {np.sum(constant_dims)} constant/padded dimensions (will be set to identity)")
312
+
313
+ # Normalize non-constant dimensions
314
+ normalized_chunks = flattened_chunks.copy()
315
+ normalized_chunks[:, ~constant_dims] = (flattened_chunks[:, ~constant_dims] - chunk_mean[~constant_dims]) / chunk_std[~constant_dims]
316
+ # Constant dimensions stay as 0 (already centered at 0 due to padding)
317
+
318
+ # Compute empirical covariance matrix
319
+ # For normalized data, covariance = correlation
320
+ cov_matrix = np.cov(normalized_chunks, rowvar=False) # Shape: [H*D, H*D]
321
+ print(f"Covariance matrix shape: {cov_matrix.shape}")
322
+
323
+ # Enforce diagonal = 1 for all dimensions (standard correlation matrix property)
324
+ # Handle constant dimensions carefully (avoid division by zero)
325
+ diag_vals = np.diag(cov_matrix).copy()
326
+ print(f"Diagonal before correction: min={np.min(diag_vals):.6f}, max={np.max(diag_vals):.6f}")
327
+
328
+ # Identify truly constant dimensions (diagonal ≈ 0)
329
+ constant_mask = diag_vals < 1e-10
330
+ num_constant = np.sum(constant_mask)
331
+ print(f"Found {num_constant} dimensions with zero variance on diagonal")
332
+
333
+ # Normalize correlation matrix (avoid division by zero)
334
+ diag_vals_safe = diag_vals.copy()
335
+ diag_vals_safe[constant_mask] = 1.0 # Prevent division by zero
336
+
337
+ normalizer = np.sqrt(diag_vals_safe[:, None] @ diag_vals_safe[None, :])
338
+ cov_matrix = cov_matrix / normalizer
339
+
340
+ # For constant dimensions: set their rows/columns to 0, diagonal to 1
341
+ cov_matrix[constant_mask, :] = 0.0
342
+ cov_matrix[:, constant_mask] = 0.0
343
+ np.fill_diagonal(cov_matrix, 1.0)
344
+
345
+ print(f"Diagonal after correction: min={np.min(np.diag(cov_matrix)):.6f}, max={np.max(np.diag(cov_matrix)):.6f}")
346
+ print(f"Matrix check: has NaN={np.any(np.isnan(cov_matrix))}, has Inf={np.any(np.isinf(cov_matrix))}")
347
+
348
+ # Add regularization for numerical stability
349
+ epsilon = 1e-6
350
+ cov_matrix_reg = cov_matrix + epsilon * np.eye(cov_matrix.shape[0])
351
+
352
+ # Check eigenvalues for positive definiteness
353
+ eigenvalues = np.linalg.eigvalsh(cov_matrix_reg)
354
+ min_eigenvalue = np.min(eigenvalues)
355
+ print(f"Min eigenvalue after regularization: {min_eigenvalue:.6e}")
356
+
357
+ if min_eigenvalue <= 0:
358
+ print(f"WARNING: Matrix not positive definite! Adding stronger regularization...")
359
+ # Add stronger regularization if needed
360
+ epsilon = max(1e-5, -min_eigenvalue + 1e-5)
361
+ cov_matrix_reg = cov_matrix + epsilon * np.eye(cov_matrix.shape[0])
362
+ eigenvalues = np.linalg.eigvalsh(cov_matrix_reg)
363
+ min_eigenvalue = np.min(eigenvalues)
364
+ print(f"New min eigenvalue: {min_eigenvalue:.6e}")
365
+
366
+ # Compute Cholesky decomposition
367
+ try:
368
+ chol_lower = np.linalg.cholesky(cov_matrix_reg)
369
+ print(f"Cholesky decomposition successful! Shape: {chol_lower.shape}")
370
+
371
+ # Verify reconstruction: L @ L.T ≈ Σ
372
+ reconstructed = chol_lower @ chol_lower.T
373
+ reconstruction_error = np.linalg.norm(reconstructed - cov_matrix_reg, 'fro') / np.linalg.norm(cov_matrix_reg, 'fro')
374
+ print(f"Cholesky reconstruction error: {reconstruction_error:.6e}")
375
+
376
+ # Compute averaged spatial correlation (dim × dim)
377
+ # Average correlation across all timesteps
378
+ print("\nComputing averaged spatial correlation (dim × dim)...")
379
+ spatial_corrs = []
380
+ for t in range(action_horizon):
381
+ start_idx = t * target_action_dim
382
+ end_idx = (t + 1) * target_action_dim
383
+ timestep_data = normalized_chunks[:, start_idx:end_idx]
384
+ corr_t = np.cov(timestep_data, rowvar=False)
385
+
386
+ # Normalize to ensure diagonal = 1
387
+ diag_t = np.diag(corr_t)
388
+ corr_t = corr_t / np.sqrt(diag_t[:, None] @ diag_t[None, :])
389
+
390
+ spatial_corrs.append(corr_t)
391
+
392
+ avg_spatial_corr = np.mean(spatial_corrs, axis=0) # (target_action_dim, target_action_dim)
393
+ # Final diagonal enforcement
394
+ np.fill_diagonal(avg_spatial_corr, 1.0)
395
+ print(f"Averaged spatial correlation shape: {avg_spatial_corr.shape}")
396
+
397
+ # Compute averaged temporal correlation (time × time)
398
+ # Average correlation across all NON-CONSTANT dimensions only
399
+ print("Computing averaged temporal correlation (time × time)...")
400
+ temporal_corrs = []
401
+
402
+ # Identify which dimensions are constant (per original dimension, before flattening)
403
+ dim_is_constant = []
404
+ for d in range(target_action_dim):
405
+ # Check if this dimension is constant across all timesteps
406
+ dim_indices_check = [t * target_action_dim + d for t in range(action_horizon)]
407
+ dim_std = chunk_std[dim_indices_check]
408
+ # Dimension is constant if std < 1e-6 for all its timesteps
409
+ dim_is_constant.append(np.all(dim_std < 1e-6))
410
+
411
+ num_constant = np.sum(dim_is_constant)
412
+ print(f" Excluding {num_constant} constant dimensions from temporal averaging")
413
+
414
+ for d in range(target_action_dim):
415
+ if dim_is_constant[d]:
416
+ continue # Skip constant dimensions
417
+
418
+ dim_indices = [t * target_action_dim + d for t in range(action_horizon)]
419
+ dim_data = normalized_chunks[:, dim_indices]
420
+ corr_d = np.cov(dim_data, rowvar=False)
421
+
422
+ # Normalize to ensure diagonal = 1
423
+ diag_d = np.diag(corr_d)
424
+ corr_d = corr_d / np.sqrt(diag_d[:, None] @ diag_d[None, :])
425
+
426
+ temporal_corrs.append(corr_d)
427
+
428
+ if len(temporal_corrs) > 0:
429
+ avg_temporal_corr = np.mean(temporal_corrs, axis=0) # (action_horizon, action_horizon)
430
+ # Final diagonal enforcement
431
+ np.fill_diagonal(avg_temporal_corr, 1.0)
432
+ print(f"Averaged temporal correlation shape: {avg_temporal_corr.shape}")
433
+ else:
434
+ # All dimensions are constant - use identity
435
+ avg_temporal_corr = np.eye(action_horizon)
436
+ print(f"All dimensions constant - using identity for temporal correlation")
437
+
438
+ # Store correlation matrices
439
+ # Beta shrinkage will be applied at model load time
440
+ correlation_stats = {
441
+ "action_correlation_cholesky": chol_lower,
442
+ "action_correlation_spatial": avg_spatial_corr, # (D, D) - averaged spatial
443
+ "action_correlation_temporal": avg_temporal_corr, # (H, H) - averaged temporal
444
+ }
445
+ except np.linalg.LinAlgError as e:
446
+ raise RuntimeError(
447
+ f"Cholesky decomposition failed: {e}. "
448
+ "This indicates the covariance matrix is not positive definite even after regularization. "
449
+ "This is a critical error that needs investigation."
450
+ )
451
+
452
+ return final_stats, per_timestamp_stats, correlation_stats
453
+
454
+
455
+ def main(
456
+ config_name: str,
457
+ max_episodes: int | None = None,
458
+ num_workers: int | None = None,
459
+ per_timestamp: bool = False,
460
+ correlation: bool = True,
461
+ max_correlation_samples: int = 2000000,
462
+ sample_fraction: float = 0.1
463
+ ):
464
+ """Compute normalization statistics quickly for B1K config using parallel processing.
465
+
466
+ Args:
467
+ config_name: Name of the training config
468
+ max_episodes: Maximum number of episodes to process (None = all)
469
+ num_workers: Number of parallel workers (None = auto)
470
+ per_timestamp: Whether to compute per-timestamp statistics
471
+ correlation: Whether to compute action correlation matrix
472
+ max_correlation_samples: Maximum number of samples to use for correlation computation
473
+ (subsampling for efficiency, 2M is typically sufficient)
474
+ sample_fraction: Fraction of action chunks to sample from each episode for
475
+ correlation/per-timestamp stats (default 0.1 = 10%, use 1.0 for no sampling).
476
+ Note: mean/std/min/max are ALWAYS computed over ALL data.
477
+ """
478
+
479
+ config = _config.get_config(config_name)
480
+ data_config = config.data.create(config.assets_dirs, config.model)
481
+
482
+ if not data_config.behavior_dataset_root:
483
+ raise ValueError("This script only works with B1K behavior datasets")
484
+
485
+ # Get the delta transform from config
486
+ delta_transform = get_delta_transform_from_config(config_name)
487
+ print(f"Using delta transform with mask: {delta_transform.mask}")
488
+
489
+ # Check if we should compute per-timestamp stats
490
+ compute_per_timestamp = per_timestamp or data_config.use_per_timestamp_norm
491
+ compute_correlation = correlation
492
+ action_horizon = config.model.action_horizon
493
+
494
+ if compute_per_timestamp:
495
+ print(f"Computing per-timestamp statistics with action_horizon={action_horizon}")
496
+
497
+ if compute_correlation:
498
+ print(f"Computing correlation matrix for action chunks with action_horizon={action_horizon}")
499
+
500
+ # Print sampling information
501
+ print(f"Memory-efficient mode: Using min/max instead of quantiles (q01/q99)")
502
+ if sample_fraction < 1.0:
503
+ print(f"Sampling {sample_fraction*100:.0f}% of action chunks from each episode for correlation/per-timestamp stats")
504
+ else:
505
+ print(f"Using all action chunks (no sampling) - this may cause OOM on large datasets")
506
+
507
+ # Find all episode parquet files
508
+ data_root = Path(data_config.behavior_dataset_root)
509
+ print(f"Looking for episode files in: {data_root}/data/task-*/episode_*.parquet")
510
+
511
+ episode_files = list(data_root.glob("data/task-*/episode_*.parquet"))
512
+ print(f"Found {len(episode_files)} total episode files")
513
+
514
+ if max_episodes is not None:
515
+ episode_files = episode_files[:max_episodes]
516
+ print(f"Limited to {len(episode_files)} files based on max_episodes")
517
+
518
+ if len(episode_files) == 0:
519
+ print("No episode files found! Checking directory structure...")
520
+ print(f"Contents of {data_root}:")
521
+ for item in data_root.iterdir():
522
+ print(f" {item}")
523
+ if (data_root / "data").exists():
524
+ print(f"Contents of {data_root}/data:")
525
+ for item in (data_root / "data").iterdir():
526
+ print(f" {item}")
527
+ raise ValueError("No episode files found")
528
+
529
+ print(f"Processing {len(episode_files)} episode files...")
530
+
531
+ # Set up parallel processing
532
+ if num_workers is None:
533
+ num_workers = min(mp.cpu_count(), len(episode_files))
534
+
535
+ # Ensure at least 1 worker
536
+ num_workers = max(1, num_workers)
537
+ print(f"Using {num_workers} workers")
538
+
539
+ # Process episodes in parallel
540
+ episode_stats_list = []
541
+ all_data_list = []
542
+
543
+ with ProcessPoolExecutor(max_workers=num_workers) as executor:
544
+ # Submit all jobs
545
+ future_to_file = {
546
+ executor.submit(process_episode_file, (episode_file, delta_transform.mask, action_horizon, compute_per_timestamp, compute_correlation, sample_fraction)): episode_file
547
+ for episode_file in episode_files
548
+ }
549
+
550
+ # Collect results
551
+ for future in as_completed(future_to_file):
552
+ episode_file = future_to_file[future]
553
+ try:
554
+ episode_stats, episode_data = future.result()
555
+ if episode_stats is not None:
556
+ episode_stats_list.append(episode_stats)
557
+ all_data_list.append(episode_data)
558
+
559
+ if len(episode_stats_list) % 10 == 0:
560
+ print(f"Processed {len(episode_stats_list)} episodes...")
561
+
562
+ except Exception as e:
563
+ print(f"Error processing {episode_file}: {e}")
564
+
565
+ print(f"Successfully processed {len(episode_stats_list)} episodes")
566
+
567
+ # Aggregate all statistics
568
+ print("Aggregating statistics...")
569
+ final_stats, per_timestamp_stats, correlation_stats = aggregate_episode_stats(
570
+ episode_stats_list, all_data_list,
571
+ config,
572
+ compute_correlation=compute_correlation,
573
+ max_correlation_samples=max_correlation_samples
574
+ )
575
+
576
+ # Prepare correlation matrix if computed
577
+ correlation_matrix = None
578
+ if compute_correlation:
579
+ if correlation_stats is None:
580
+ raise RuntimeError(
581
+ "Correlation computation was requested (--correlation) but correlation_stats is None. "
582
+ "This indicates an unexpected error in the correlation computation pipeline."
583
+ )
584
+ print("Adding action correlation matrix...")
585
+ chol_matrix = correlation_stats["action_correlation_cholesky"]
586
+ print(f"Correlation matrix shape: {chol_matrix.shape}")
587
+
588
+ # Verify size matches expected (should already be correct from padding before computation)
589
+ expected_dim = action_horizon * config.model.action_dim
590
+ if chol_matrix.shape[0] != expected_dim:
591
+ raise ValueError(
592
+ f"Correlation matrix has unexpected size {chol_matrix.shape[0]}, "
593
+ f"expected {expected_dim} (action_horizon={action_horizon} × action_dim={config.model.action_dim})"
594
+ )
595
+
596
+ correlation_matrix = chol_matrix
597
+ elif correlation_stats is not None:
598
+ # This shouldn't happen but handle it gracefully
599
+ print("WARNING: Correlation matrix was computed but --correlation flag was False. Ignoring correlation matrix.")
600
+
601
+ # Create NormStats objects with proper structure
602
+ norm_stats_dict = {}
603
+
604
+ for key in ["state", "actions"]:
605
+ # Prepare basic statistics
606
+ stats_kwargs = {}
607
+ for stat_type in ["mean", "std", "q01", "q99"]:
608
+ stat_value = final_stats[key][stat_type]
609
+ padded_value = transforms.pad_to_dim(stat_value, config.model.action_dim)
610
+ stats_kwargs[stat_type] = padded_value
611
+
612
+ # Add per-timestamp statistics if computed and this is actions
613
+ if per_timestamp_stats is not None and key == "actions":
614
+ print("Adding per-timestamp statistics...")
615
+ for stat_type in ["per_timestamp_mean", "per_timestamp_std", "per_timestamp_q01", "per_timestamp_q99"]:
616
+ stat_value = per_timestamp_stats[stat_type]
617
+ # Pad each timestamp to model action dimension
618
+ padded_value = []
619
+ for t in range(stat_value.shape[0]):
620
+ padded_timestep = transforms.pad_to_dim(stat_value[t], config.model.action_dim)
621
+ padded_value.append(padded_timestep)
622
+ stats_kwargs[stat_type] = np.array(padded_value)
623
+
624
+ # Add correlation matrices if computed and this is actions
625
+ if correlation_matrix is not None and key == "actions":
626
+ stats_kwargs["action_correlation_cholesky"] = correlation_matrix
627
+ # Also add averaged spatial and temporal correlations if available
628
+ if correlation_stats and "action_correlation_spatial" in correlation_stats:
629
+ stats_kwargs["action_correlation_spatial"] = correlation_stats["action_correlation_spatial"]
630
+ if correlation_stats and "action_correlation_temporal" in correlation_stats:
631
+ stats_kwargs["action_correlation_temporal"] = correlation_stats["action_correlation_temporal"]
632
+
633
+ # Create NormStats object
634
+ norm_stats_dict[key] = normalize.NormStats(**stats_kwargs)
635
+
636
+ # Save statistics
637
+ output_path = config.assets_dirs / data_config.repo_id
638
+ output_path.mkdir(parents=True, exist_ok=True)
639
+ normalize.save(output_path, norm_stats_dict)
640
+
641
+ # Print summary for verification
642
+ print("\nStatistics Summary (first 23 dims):")
643
+ state_means = np.array(norm_stats_dict["state"].mean[:23])
644
+ action_means = np.array(norm_stats_dict["actions"].mean[:23])
645
+ differences = np.abs(state_means - action_means)
646
+
647
+ print("State means: ", state_means)
648
+ print("Action means: ", action_means)
649
+ print("Differences: ", differences)
650
+ print("Max difference:", np.max(differences))
651
+ print("\nNote: For actions, q01/q99 are actual percentiles. For state, q01/q99 contain min/max values.")
652
+
653
+ if per_timestamp_stats is not None:
654
+ print(f"\nPer-timestamp statistics computed for {action_horizon} timesteps")
655
+ # Show first few timesteps for verification
656
+ per_ts_means = np.array(norm_stats_dict["actions"].per_timestamp_mean)
657
+ print(f"Per-timestamp means shape: {per_ts_means.shape}")
658
+ print("First 3 timesteps, first 5 dims:")
659
+ for t in range(min(3, per_ts_means.shape[0])):
660
+ print(f" t={t}: {per_ts_means[t, :5]}")
661
+
662
+ if compute_correlation:
663
+ print(f"\nCorrelation matrices computed!")
664
+
665
+ # Full correlation matrix
666
+ corr_matrix = np.array(norm_stats_dict["actions"].action_correlation_cholesky)
667
+ print(f" Full correlation (Cholesky): {corr_matrix.shape}")
668
+ print(f" Memory: {corr_matrix.nbytes / 1024 / 1024:.2f} MB")
669
+
670
+ # Averaged spatial correlation
671
+ if hasattr(norm_stats_dict["actions"], 'action_correlation_spatial') and \
672
+ norm_stats_dict["actions"].action_correlation_spatial is not None:
673
+ spatial = np.array(norm_stats_dict["actions"].action_correlation_spatial)
674
+ print(f" Spatial (dim×dim, averaged): {spatial.shape}")
675
+ print(f" Memory: {spatial.nbytes / 1024:.2f} KB")
676
+ print(f" Sample correlation (d0 vs d1): {spatial[0, 1]:.4f}")
677
+
678
+ # Averaged temporal correlation
679
+ if hasattr(norm_stats_dict["actions"], 'action_correlation_temporal') and \
680
+ norm_stats_dict["actions"].action_correlation_temporal is not None:
681
+ temporal = np.array(norm_stats_dict["actions"].action_correlation_temporal)
682
+ print(f" Temporal (time×time, averaged): {temporal.shape}")
683
+ print(f" Memory: {temporal.nbytes / 1024:.2f} KB")
684
+ print(f" Sample correlation (t0 vs t1): {temporal[0, 1]:.4f}")
685
+
686
+ print("\nNote: Beta shrinkage will be applied at model load time (not during stats computation)")
687
+ else:
688
+ print("\nCorrelation matrix computation was disabled (--no-correlation)")
689
+
690
+ print(f"\nStatistics saved to: {output_path}")
691
+
692
+
693
+ if __name__ == "__main__":
694
+ tyro.cli(main)
task 57/policy_code/scripts/serve_b1k.py ADDED
@@ -0,0 +1,190 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import dataclasses
2
+ import enum
3
+ import logging
4
+ import os
5
+ import pathlib
6
+ import socket
7
+
8
+ import numpy as np
9
+ import tyro
10
+
11
+ # Set JAX memory allocation before importing JAX (can be overridden by env vars)
12
+ os.environ.setdefault('XLA_PYTHON_CLIENT_MEM_FRACTION', '0.5') # Use 50% of GPU memory
13
+ os.environ.setdefault('XLA_PYTHON_CLIENT_ALLOCATOR', 'platform') # Platform allocator
14
+
15
+ from omnigibson.learning.utils.network_utils import WebsocketPolicyServer
16
+ from omnigibson.learning.datas import BehaviorLerobotDatasetMetadata
17
+
18
+ from openpi.policies import policy as _policy
19
+
20
+ # Import B1K-specific modules
21
+ from b1k.policies import policy_config as _policy_config # Use our custom policy_config
22
+ from b1k.policies.checkpoint_switcher import CheckpointSwitcher
23
+ from b1k.shared.eval_b1k_wrapper import B1KPolicyWrapper, B1KWrapperConfig
24
+ from b1k.training import config as _config
25
+
26
+
27
+ class EnvMode(enum.Enum):
28
+ # Not used, just kept for compatibility
29
+ ALOHA = "aloha"
30
+ ALOHA_SIM = "aloha_sim"
31
+ DROID = "droid"
32
+ LIBERO = "libero"
33
+
34
+
35
+ @dataclasses.dataclass
36
+ class Checkpoint:
37
+ """Load a policy from a trained checkpoint."""
38
+ config: str
39
+ dir: str
40
+
41
+
42
+ @dataclasses.dataclass
43
+ class Default:
44
+ """Use the default policy for the given environment."""
45
+
46
+
47
+ @dataclasses.dataclass
48
+ class Args:
49
+ """Arguments for the serve_policy script."""
50
+
51
+ # Environment to serve the policy for. This is only used when serving default policies.
52
+ env: EnvMode = EnvMode.ALOHA_SIM
53
+
54
+ # If provided, will be used in case the "prompt" key is not present in the data, or if the model doesn't have a default prompt.
55
+ default_prompt: str | None = None
56
+
57
+ # For PI_BEHAVIOR models: task ID (0-49) instead of text prompt
58
+ task_id: int | None = None
59
+
60
+ # Dataset root, used to retrieve the prompt of the task if taskname is not None.
61
+ dataset_root: str | None = "/scr/behavior/2025-challenge-demos"
62
+ # If provided, will be used to retrieve the prompt of the task, otherwise use turning_on_radio as default.
63
+ task_name: str | None = None
64
+
65
+ # Port to serve the policy on.
66
+ port: int = 8000
67
+ # Record the policy's behavior for debugging.
68
+ record: bool = False
69
+
70
+ # Specifies how to load the policy. If not provided, the default policy for the environment will be used.
71
+ policy: Checkpoint | Default = dataclasses.field(default_factory=Default)
72
+
73
+ # B1K Wrapper execution parameters
74
+ actions_to_execute: int = 26
75
+ actions_to_keep: int = 4
76
+ execute_in_n_steps: int = 20
77
+ history_len: int = 3
78
+ votes_to_promote: int = 2
79
+ time_threshold_inpaint: float = 0.3
80
+ num_steps: int = 20
81
+ apply_eval_tricks: bool = True # Enable correction rules and gripper variation checks
82
+
83
+ # Multi-checkpoint support for PI_BEHAVIOR models (optional)
84
+ task_checkpoint_mapping: str | None = None # Path to task-checkpoint mapping JSON file
85
+
86
+
87
+ def create_policy(args: Args) -> _policy.Policy:
88
+ """Create a policy from the given arguments."""
89
+ sample_kwargs = {"num_steps": args.num_steps}
90
+ return _policy_config.create_trained_policy(
91
+ _config.get_config(args.policy.config),
92
+ args.policy.dir,
93
+ default_prompt=args.default_prompt,
94
+ sample_kwargs=sample_kwargs
95
+ )
96
+
97
+
98
+ def main(args: Args) -> None:
99
+ # B1K only supports PI_BEHAVIOR models (task embeddings, no text prompts)
100
+ config = _config.get_config(args.policy.config)
101
+
102
+ # PI_BEHAVIOR model setup
103
+ if args.task_id is not None:
104
+ logging.info(f"Using PI_BEHAVIOR model with task_id: {args.task_id}")
105
+ task_id = args.task_id
106
+ else:
107
+ logging.info(f"Using PI_BEHAVIOR model - task_id will be extracted from observations")
108
+ task_id = None
109
+
110
+ # Placeholder prompt for PI_BEHAVIOR (not actually used by model)
111
+ prompt = "PI_BEHAVIOR model (task-conditioned)"
112
+ logging.info(f"Using prompt: {prompt}")
113
+
114
+ # Load initial/default policy
115
+ policy = create_policy(args)
116
+ policy_metadata = policy.metadata
117
+
118
+ # Create checkpoint switcher if mapping file provided
119
+ checkpoint_switcher = None
120
+ if args.task_checkpoint_mapping:
121
+ logging.info(f"Multi-checkpoint mode enabled: {args.task_checkpoint_mapping}")
122
+
123
+ sample_kwargs = {"num_steps": args.num_steps}
124
+
125
+ try:
126
+ checkpoint_switcher = CheckpointSwitcher(
127
+ config_path=args.task_checkpoint_mapping,
128
+ training_config=config,
129
+ sample_kwargs=sample_kwargs
130
+ )
131
+ logging.info("Checkpoint switcher initialized - will switch checkpoints based on task_id")
132
+ except Exception as e:
133
+ logging.error(f"Failed to initialize checkpoint switcher: {e}")
134
+ raise
135
+ else:
136
+ logging.info("Single checkpoint mode - using one checkpoint for all tasks")
137
+
138
+ # Record the policy's behavior.
139
+ if args.record:
140
+ policy = _policy.PolicyRecorder(policy, "policy_records")
141
+
142
+ # Create wrapper configuration
143
+ wrapper_config = B1KWrapperConfig(
144
+ actions_to_execute=args.actions_to_execute,
145
+ actions_to_keep=args.actions_to_keep,
146
+ execute_in_n_steps=args.execute_in_n_steps,
147
+ history_len=args.history_len,
148
+ votes_to_promote=args.votes_to_promote,
149
+ time_threshold_inpaint=args.time_threshold_inpaint,
150
+ num_steps=args.num_steps,
151
+ apply_eval_tricks=args.apply_eval_tricks,
152
+ )
153
+
154
+ logging.info(f"Wrapper config: execute={wrapper_config.actions_to_execute}, keep={wrapper_config.actions_to_keep}, steps={wrapper_config.execute_in_n_steps}, num_steps={wrapper_config.num_steps}")
155
+
156
+ if wrapper_config.apply_eval_tricks:
157
+ logging.info("Eval tricks ENABLED - correction rules and gripper variation checks active")
158
+ else:
159
+ logging.info("Eval tricks DISABLED (default behavior)")
160
+
161
+ # Create B1K wrapper with PI_BEHAVIOR-specific features
162
+ policy = B1KPolicyWrapper(
163
+ policy,
164
+ text_prompt=prompt, # Not used by PI_BEHAVIOR, kept for compatibility
165
+ task_id=task_id,
166
+ config=wrapper_config,
167
+ checkpoint_switcher=checkpoint_switcher
168
+ )
169
+
170
+ if checkpoint_switcher:
171
+ logging.info("Multi-checkpoint mode: checkpoints will switch based on task_id from observations")
172
+ else:
173
+ logging.info("Rolling inpainting enabled: will use initial_actions from input batch when provided")
174
+
175
+ hostname = socket.gethostname()
176
+ local_ip = socket.gethostbyname(hostname)
177
+ logging.info("Creating server (host: %s, ip: %s)", hostname, local_ip)
178
+
179
+ server = WebsocketPolicyServer(
180
+ policy=policy,
181
+ host="0.0.0.0",
182
+ port=args.port,
183
+ metadata=policy_metadata,
184
+ )
185
+ server.serve_forever()
186
+
187
+
188
+ if __name__ == "__main__":
189
+ logging.basicConfig(level=logging.INFO, force=True)
190
+ main(tyro.cli(Args))
task 57/policy_code/scripts/serve_b1k_2026.py ADDED
@@ -0,0 +1,149 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Serve a PI_BEHAVIOR checkpoint to the 2026 BEHAVIOR evaluator (omnigibson.eval.eval).
2
+
3
+ Uses the 2025 solution's B1KPolicyWrapper (rolling inpainting, stage voting, action compression)
4
+ behind openpi's 2026 websocket server, which speaks the same protocol as the 2026 evaluator client.
5
+
6
+ PYTHONPATH=src /workspace/openpi/.venv/bin/python scripts/serve_b1k_2026.py \
7
+ --policy-dir outputs/checkpoints/pi_behavior_sweep_lora/<exp>/<step>
8
+
9
+ Task mapping: the evaluator sends obs["task_id"] = 57 (sweeping_garage); the model was fine-tuned for
10
+ it in slot 36, so 57 is rewritten to 36. The 2025 "eval tricks" (hand-written per-task gripper/stage
11
+ correction rules) are OFF by default: slot 36's rules force both grippers open, which would make
12
+ holding the broom impossible.
13
+ """
14
+
15
+ import dataclasses
16
+ import logging
17
+ import os
18
+ import socket
19
+
20
+ os.environ.setdefault("XLA_PYTHON_CLIENT_MEM_FRACTION", "0.45")
21
+
22
+ import numpy as np
23
+ import torch
24
+ import tyro
25
+
26
+ from openpi.serving import websocket_b1k_server
27
+
28
+ from b1k.policies import policy_config as _policy_config
29
+ from b1k.policies.b1k_policy import PROPRIOCEPTION_INDICES
30
+ from b1k.shared.eval_b1k_wrapper import B1KPolicyWrapper, B1KWrapperConfig
31
+ from b1k.training import config as _config
32
+
33
+
34
+ @dataclasses.dataclass
35
+ class Args:
36
+ policy_dir: str
37
+ config: str = "pi_behavior_sweep_lora"
38
+ port: int = 8000
39
+ # Evaluator task id -> model task slot.
40
+ task_map: dict[int, int] = dataclasses.field(default_factory=lambda: {57: 36})
41
+ actions_to_execute: int = 26
42
+ actions_to_keep: int = 4
43
+ execute_in_n_steps: int = 20
44
+ history_len: int = 3
45
+ votes_to_promote: int = 2
46
+ time_threshold_inpaint: float = 0.3
47
+ num_steps: int = 20
48
+ apply_eval_tricks: bool = False
49
+ # Clamp trunk commands to the envelope of the 2026 sweeping demos (see cap_torso). Off = raw model output.
50
+ torso_cap: bool = True
51
+ # If set, append per-step trunk state / commanded / capped trunk action to this CSV (diagnostics).
52
+ torso_log: str | None = None
53
+
54
+
55
+ # JoyLo teleop moves the trunk along upright -> downward -> ground (og_teleop_cfg.py R1_*_TORSO_JOINT_POS).
56
+ TORSO_PATH = np.array([[0.45, -0.4, 0.0, 0.0], [1.6, -2.5, -0.94, 0.0], [1.735, -2.57, -2.1, 0.0]])
57
+ # Per-joint range of trunk actions over all 894,606 frames of the 2026 sweeping_garage demos.
58
+ TORSO_DEMO_MIN = np.array([0.45, -2.563, -1.833, 0.0])
59
+ TORSO_DEMO_MAX = np.array([1.721, -0.4, 0.078, 0.0])
60
+ # 99.9th percentile distance of demo trunk actions from TORSO_PATH (max 0.55).
61
+ TORSO_MAX_PATH_DIST = 0.3
62
+
63
+
64
+ def _closest_on_torso_path(q: np.ndarray) -> np.ndarray:
65
+ best, best_d = None, np.inf
66
+ for a, b in zip(TORSO_PATH[:-1], TORSO_PATH[1:]):
67
+ d = b - a
68
+ p = a + np.clip((q - a) @ d / (d @ d), 0.0, 1.0) * d
69
+ if np.linalg.norm(q - p) < best_d:
70
+ best, best_d = p, np.linalg.norm(q - p)
71
+ return best
72
+
73
+
74
+ def cap_torso(q: np.ndarray) -> np.ndarray:
75
+ """Keeps a trunk command inside the demo envelope: per-joint demo range, then at most
76
+ TORSO_MAX_PATH_DIST from the teleop trunk path. Stops the policy from leaning back/over."""
77
+ q = np.clip(q, TORSO_DEMO_MIN, TORSO_DEMO_MAX)
78
+ p = _closest_on_torso_path(q)
79
+ dist = np.linalg.norm(q - p)
80
+ if dist > TORSO_MAX_PATH_DIST:
81
+ q = p + (q - p) * (TORSO_MAX_PATH_DIST / dist)
82
+ return q
83
+
84
+
85
+ def _py(v):
86
+ return float(v) if isinstance(v, (np.floating, float)) else v
87
+
88
+
89
+ class TaskRemapWrapper:
90
+ """Rewrites obs["task_id"] from the evaluator's task index to the model's task slot."""
91
+
92
+ def __init__(self, policy: B1KPolicyWrapper, task_map: dict[int, int], torso_cap: bool = True,
93
+ torso_log: str | None = None):
94
+ self._policy = policy
95
+ self._task_map = task_map
96
+ self._torso_cap = torso_cap
97
+ self._torso_log = open(torso_log, "a") if torso_log else None
98
+ self._episode = 0
99
+ self._step = 0
100
+
101
+ def act(self, obs: dict):
102
+ if "task_id" in obs:
103
+ task_id = int(np.asarray(obs["task_id"]).reshape(-1)[0])
104
+ obs["task_id"] = np.array([self._task_map.get(task_id, task_id)], dtype=np.int64)
105
+ action = self._policy.act(obs)
106
+ raw = action[3:7].numpy().copy()
107
+ if self._torso_cap:
108
+ action[3:7] = torch.from_numpy(cap_torso(raw.astype(np.float64))).to(action.dtype)
109
+ if self._torso_log is not None:
110
+ state = np.asarray(obs["robot_r1::proprio"]).reshape(-1)[PROPRIOCEPTION_INDICES["R1Pro"]["trunk_qpos"]]
111
+ row = [self._episode, self._step, *state, *raw, *action[3:7].numpy()]
112
+ self._torso_log.write(",".join(f"{v:.4f}" if isinstance(v, float) else str(v) for v in map(_py, row)) + "\n")
113
+ if self._step % 100 == 0:
114
+ self._torso_log.flush()
115
+ self._step += 1
116
+ return action
117
+
118
+ def reset(self):
119
+ self._policy.reset()
120
+ self._episode += 1
121
+ self._step = 0
122
+
123
+
124
+ def main(args: Args) -> None:
125
+ config = _config.get_config(args.config)
126
+ policy = _policy_config.create_trained_policy(config, args.policy_dir, sample_kwargs={"num_steps": args.num_steps})
127
+ wrapper_config = B1KWrapperConfig(
128
+ actions_to_execute=args.actions_to_execute,
129
+ actions_to_keep=args.actions_to_keep,
130
+ execute_in_n_steps=args.execute_in_n_steps,
131
+ history_len=args.history_len,
132
+ votes_to_promote=args.votes_to_promote,
133
+ time_threshold_inpaint=args.time_threshold_inpaint,
134
+ num_steps=args.num_steps,
135
+ apply_eval_tricks=args.apply_eval_tricks,
136
+ )
137
+ default_task = next(iter(args.task_map.values()))
138
+ wrapped = B1KPolicyWrapper(policy, task_id=default_task, config=wrapper_config)
139
+ wrapped = TaskRemapWrapper(wrapped, args.task_map, torso_cap=args.torso_cap, torso_log=args.torso_log)
140
+ logging.info(f"Serving {args.policy_dir} on port {args.port}; task map {args.task_map}; torso_cap={args.torso_cap}; {wrapper_config}")
141
+ logging.info("Host: %s", socket.gethostname())
142
+ websocket_b1k_server.WebsocketPolicyServer(
143
+ policy=wrapped, host="0.0.0.0", port=args.port, metadata=policy.metadata
144
+ ).serve_forever()
145
+
146
+
147
+ if __name__ == "__main__":
148
+ logging.basicConfig(level=logging.INFO, force=True)
149
+ main(tyro.cli(Args))
task 57/policy_code/scripts/train.py ADDED
@@ -0,0 +1,450 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ Training script for BEHAVIOR-1K solution.
3
+
4
+ Based on https://github.com/PhysicalIntelligence/openpi/blob/behavior/openpi/scripts/train.py with custom modifications.
5
+ """
6
+
7
+ import dataclasses
8
+ import functools
9
+ import logging
10
+ import os
11
+ import platform
12
+ import time
13
+ from typing import Any
14
+
15
+ import etils.epath as epath
16
+ import flax.nnx as nnx
17
+ from flax.training import common_utils
18
+ import flax.traverse_util as traverse_util
19
+ import jax
20
+ import jax.experimental
21
+ import jax.numpy as jnp
22
+ import numpy as np
23
+ import optax
24
+ import tqdm_loggable.auto as tqdm
25
+ import wandb
26
+
27
+ # Configure JAX memory allocation to prevent OOM errors
28
+ os.environ.setdefault('XLA_PYTHON_CLIENT_MEM_FRACTION', '0.9')
29
+ os.environ.setdefault('XLA_PYTHON_CLIENT_ALLOCATOR', 'platform')
30
+
31
+ # Configure OpenBLAS to prevent thread creation errors
32
+ os.environ.setdefault('OPENBLAS_NUM_THREADS', '16')
33
+ os.environ.setdefault('MKL_NUM_THREADS', '16')
34
+
35
+ import openpi.models.model as _model
36
+ import openpi.shared.array_typing as at
37
+ import openpi.shared.nnx_utils as nnx_utils
38
+ import openpi.training.optimizer as _optimizer
39
+ import openpi.training.sharding as sharding
40
+ import openpi.training.utils as training_utils
41
+
42
+ # Import B1K-specific modules
43
+ from b1k.training import checkpoints as _checkpoints # Use our custom checkpoints (not openpi's!)
44
+ from b1k.training import config as _config
45
+ from b1k.training import data_loader as _data_loader
46
+ from b1k.training import weight_loaders as _weight_loaders
47
+ from b1k.models.pi_behavior import PiBehavior
48
+ from b1k.models.pi_behavior_config import PiBehaviorConfig
49
+ from b1k.models.observation import Observation
50
+
51
+
52
+ def init_logging():
53
+ """Custom logging format for better readability."""
54
+ level_mapping = {"DEBUG": "D", "INFO": "I", "WARNING": "W", "ERROR": "E", "CRITICAL": "C"}
55
+
56
+ class CustomFormatter(logging.Formatter):
57
+ def format(self, record):
58
+ record.levelname = level_mapping.get(record.levelname, record.levelname)
59
+ return super().format(record)
60
+
61
+ formatter = CustomFormatter(
62
+ fmt="%(asctime)s.%(msecs)03d [%(levelname)s] %(message)-80s (%(process)d:%(filename)s:%(lineno)s)",
63
+ datefmt="%H:%M:%S",
64
+ )
65
+
66
+ logger = logging.getLogger()
67
+ logger.setLevel(logging.INFO)
68
+ logger.handlers[0].setFormatter(formatter)
69
+
70
+
71
+ def init_wandb(config: _config.TrainConfig, *, resuming: bool, log_code: bool = False, enabled: bool = True):
72
+ if not enabled:
73
+ wandb.init(mode="disabled")
74
+ return
75
+
76
+ ckpt_dir = config.checkpoint_dir
77
+ if not ckpt_dir.exists():
78
+ raise FileNotFoundError(f"Checkpoint directory {ckpt_dir} does not exist.")
79
+ if resuming:
80
+ run_id = (ckpt_dir / "wandb_id.txt").read_text().strip()
81
+ wandb.init(id=run_id, resume="must", project=config.project_name)
82
+ else:
83
+ wandb.init(
84
+ name=config.exp_name,
85
+ config=dataclasses.asdict(config),
86
+ project=config.project_name,
87
+ )
88
+ (ckpt_dir / "wandb_id.txt").write_text(wandb.run.id)
89
+
90
+ if log_code:
91
+ wandb.run.log_code(epath.Path(__file__).parent.parent)
92
+
93
+
94
+ def _load_weights_and_validate(loader: _weight_loaders.WeightLoader, params_shape: at.Params) -> at.Params:
95
+ """Loads and validates the weights. Returns a loaded subset of the weights."""
96
+ loaded_params = loader.load(params_shape)
97
+
98
+ # Filter out nnx.Intermediate fields from both sides (they're not params, excluded from checkpoints)
99
+ # This allows loading old checkpoints that didn't have these fields
100
+ def filter_intermediate_fields(params_dict):
101
+ flat = traverse_util.flatten_dict(params_dict)
102
+ # List of field names that are nnx.Intermediate (excluded from checkpoints)
103
+ intermediate_field_names = [
104
+ 'action_correlation_cholesky', # Legacy full correlation matrix
105
+ 'L_spatial', # Separable spatial correlation
106
+ 'L_temporal', # Separable temporal correlation
107
+ 'cached_num_inpaint_actions', # Conditional sampling cache
108
+ 'cached_input_action_dim', # Conditional sampling cache
109
+ 'cached_Sigma_uo_Sigma_oo_inv', # Conditional sampling cache
110
+ 'cached_L_cond_free', # Conditional sampling cache
111
+ 'cached_Sigma_ou_Sigma_uu_inv', # Conditional sampling cache
112
+ 'cached_L_cond_inp', # Conditional sampling cache
113
+ ]
114
+ filtered = {k: v for k, v in flat.items()
115
+ if not any(field in str(k) for field in intermediate_field_names)}
116
+ return traverse_util.unflatten_dict(filtered)
117
+
118
+ # Validate loaded params structure
119
+ params_shape_filtered = filter_intermediate_fields(params_shape)
120
+ loaded_params_filtered = filter_intermediate_fields(loaded_params)
121
+ at.check_pytree_equality(expected=params_shape_filtered, got=loaded_params_filtered, check_shapes=True, check_dtypes=True)
122
+
123
+ # Remove jax.ShapeDtypeStruct and Intermediate fields from the loaded params
124
+ def should_exclude(k, v):
125
+ if isinstance(v, jax.ShapeDtypeStruct):
126
+ return True
127
+ # Exclude all intermediate fields
128
+ intermediate_field_names = [
129
+ 'action_correlation_cholesky', 'L_spatial', 'L_temporal',
130
+ 'cached_num_inpaint_actions', 'cached_input_action_dim',
131
+ 'cached_Sigma_uo_Sigma_oo_inv', 'cached_L_cond_free',
132
+ 'cached_Sigma_ou_Sigma_uu_inv', 'cached_L_cond_inp',
133
+ ]
134
+ return any(field in str(k) for field in intermediate_field_names)
135
+
136
+ return traverse_util.unflatten_dict(
137
+ {k: v for k, v in traverse_util.flatten_dict(loaded_params).items()
138
+ if not should_exclude(k, v)}
139
+ )
140
+
141
+
142
+ @at.typecheck
143
+ def init_train_state(
144
+ config: _config.TrainConfig,
145
+ init_rng: at.KeyArrayLike,
146
+ mesh: jax.sharding.Mesh,
147
+ *,
148
+ resume: bool,
149
+ norm_stats: dict | None = None
150
+ ) -> tuple[training_utils.TrainState, Any]:
151
+ tx = _optimizer.create_optimizer(config.optimizer, config.lr_schedule, weight_decay_mask=None)
152
+
153
+ def init(rng: at.KeyArrayLike, partial_params: at.Params | None = None) -> training_utils.TrainState:
154
+ rng, model_rng = jax.random.split(rng)
155
+ # initialize the model (and its parameters).
156
+ model = config.model.create(model_rng)
157
+
158
+ # Load correlation matrix into PiBehavior models BEFORE creating graphdef
159
+ if isinstance(model, PiBehavior) and norm_stats is not None:
160
+ model.load_correlation_matrix(norm_stats)
161
+ logging.info("Loaded correlation matrix during model initialization")
162
+
163
+ # Merge the partial params into the model.
164
+ if partial_params is not None:
165
+ graphdef, state = nnx.split(model)
166
+ # This will produce an error if the partial params are not a subset of the state.
167
+ state.replace_by_pure_dict(partial_params)
168
+ model = nnx.merge(graphdef, state)
169
+
170
+ params = nnx.state(model)
171
+ params = nnx_utils.state_map(params, config.freeze_filter, lambda p: p.replace(p.value.astype(jnp.bfloat16)))
172
+ return training_utils.TrainState(
173
+ step=0,
174
+ params=params,
175
+ model_def=nnx.graphdef(model),
176
+ tx=tx,
177
+ opt_state=tx.init(params.filter(config.trainable_filter)),
178
+ ema_decay=config.ema_decay,
179
+ ema_params=None if config.ema_decay is None else params,
180
+ )
181
+
182
+ train_state_shape = jax.eval_shape(init, init_rng)
183
+ state_sharding = sharding.fsdp_sharding(train_state_shape, mesh, log=True)
184
+
185
+ if resume:
186
+ return train_state_shape, state_sharding
187
+
188
+ partial_params = _load_weights_and_validate(config.weight_loader, train_state_shape.params.to_pure_dict())
189
+ replicated_sharding = jax.sharding.NamedSharding(mesh, jax.sharding.PartitionSpec())
190
+
191
+ # Initialize the train state and mix in the partial params.
192
+ train_state = jax.jit(
193
+ init,
194
+ donate_argnums=(1,), # donate the partial params buffer.
195
+ in_shardings=replicated_sharding,
196
+ out_shardings=state_sharding,
197
+ )(init_rng, partial_params)
198
+
199
+ # Log KV transform coefficients for PiBehavior models
200
+ model = nnx.merge(train_state.model_def, train_state.params)
201
+ if isinstance(model, PiBehavior) and hasattr(model, 'kv_transform') and model.kv_transform is not None:
202
+ logging.info("KV Transform Coefficients (after loading):")
203
+ logging.info("=" * 80)
204
+
205
+ k_coeffs = model.kv_transform.k_coeffs.value
206
+ v_coeffs = model.kv_transform.v_coeffs.value
207
+
208
+ logging.info("K Coefficients (each layer attends to all VLM layers):")
209
+ for i in range(k_coeffs.shape[0]):
210
+ coeffs_str = ", ".join([f"{float(c):.2f}" for c in k_coeffs[i]])
211
+ logging.info(f" Layer {i:2d}: [{coeffs_str}]")
212
+
213
+ logging.info("")
214
+ logging.info("V Coefficients (each layer attends to all VLM layers):")
215
+ for i in range(v_coeffs.shape[0]):
216
+ coeffs_str = ", ".join([f"{float(c):.2f}" for c in v_coeffs[i]])
217
+ logging.info(f" Layer {i:2d}: [{coeffs_str}]")
218
+
219
+ logging.info("=" * 80)
220
+
221
+ return train_state, state_sharding
222
+
223
+
224
+ @at.typecheck
225
+ def train_step(
226
+ config: _config.TrainConfig,
227
+ rng: at.KeyArrayLike,
228
+ state: training_utils.TrainState,
229
+ batch: tuple[Observation, _model.Actions],
230
+ ) -> tuple[training_utils.TrainState, dict[str, at.Array]]:
231
+ model = nnx.merge(state.model_def, state.params)
232
+ model.train()
233
+
234
+ @at.typecheck
235
+ def loss_fn(
236
+ model: PiBehavior, rng: at.KeyArrayLike, observation: Observation, actions: _model.Actions
237
+ ):
238
+ losses_dict = model.compute_detailed_loss(rng, observation, actions, train=True, num_flow_samples=config.num_flow_samples)
239
+ total_loss = jnp.mean(losses_dict["total_loss"])
240
+ return total_loss, losses_dict
241
+
242
+ train_rng = jax.random.fold_in(rng, state.step)
243
+ observation, actions = batch
244
+
245
+ # Filter out frozen params.
246
+ diff_state = nnx.DiffState(0, config.trainable_filter)
247
+ (loss, losses_dict), grads = nnx.value_and_grad(loss_fn, argnums=diff_state, has_aux=True)(model, train_rng, observation, actions)
248
+
249
+ # Knowledge insulation gradient monitoring
250
+ if config.model.use_knowledge_insulation:
251
+ # Helper functions to identify parameter groups
252
+ def is_action_expert_param(path_str):
253
+ # Action expert parameters:
254
+ # - Second LLM expert (300M params, marked with _1 suffix)
255
+ # - Action projections, time MLPs, kv_transform
256
+ return any(x in path_str for x in [
257
+ "_1", # All second expert parameters
258
+ "action_in_proj",
259
+ "action_out_proj",
260
+ "time_mlp_in",
261
+ "time_mlp_out",
262
+ "kv_transform"
263
+ ])
264
+
265
+ def is_vlm_param(path_str):
266
+ # VLM parameters: everything else (first expert, img, FAST, task modules)
267
+ return not is_action_expert_param(path_str)
268
+
269
+ # Compute gradient norms for monitoring only (no scaling applied)
270
+ def compute_group_norm(grads_state, predicate):
271
+ """Compute norm for gradients matching predicate."""
272
+ flat_grads = []
273
+ for path, value in jax.tree_util.tree_flatten_with_path(grads_state.to_pure_dict())[0]:
274
+ path_str = "/".join(str(k) for k in path)
275
+ if predicate(path_str):
276
+ if hasattr(value, 'value'):
277
+ flat_grads.append(value.value if hasattr(value, 'value') else value)
278
+ else:
279
+ flat_grads.append(value)
280
+
281
+ if flat_grads:
282
+ return jnp.sqrt(sum(jnp.sum(jnp.square(g)) for g in flat_grads))
283
+ return 0.0
284
+
285
+ grad_norm_vlm = compute_group_norm(grads, is_vlm_param)
286
+ grad_norm_action = compute_group_norm(grads, is_action_expert_param)
287
+ else:
288
+ grad_norm_vlm = None
289
+ grad_norm_action = None
290
+
291
+ params = state.params.filter(config.trainable_filter)
292
+ updates, new_opt_state = state.tx.update(grads, state.opt_state, params)
293
+ new_params = optax.apply_updates(params, updates)
294
+
295
+ # Update the model in place and return the new full state.
296
+ nnx.update(model, new_params)
297
+ new_params = nnx.state(model)
298
+
299
+ new_state = dataclasses.replace(state, step=state.step + 1, params=new_params, opt_state=new_opt_state)
300
+ if state.ema_decay is not None:
301
+ new_state = dataclasses.replace(
302
+ new_state,
303
+ ema_params=jax.tree.map(
304
+ lambda old, new: state.ema_decay * old + (1 - state.ema_decay) * new, state.ema_params, new_params
305
+ ),
306
+ )
307
+
308
+ # Filter out params that aren't kernels.
309
+ kernel_params = nnx.state(
310
+ model,
311
+ nnx.All(
312
+ nnx.Param,
313
+ nnx.Not(nnx_utils.PathRegex(".*/(bias|scale|pos_embedding|input_embedding)")),
314
+ lambda _, x: x.value.ndim > 1,
315
+ ),
316
+ )
317
+ info = {
318
+ "loss": loss,
319
+ "grad_norm": optax.global_norm(grads),
320
+ "param_norm": optax.global_norm(kernel_params),
321
+ }
322
+
323
+ # Add gradient norm breakdown for knowledge insulation monitoring
324
+ if grad_norm_vlm is not None:
325
+ info["grad_norm_vlm"] = grad_norm_vlm
326
+ info["grad_norm_action_expert"] = grad_norm_action
327
+
328
+ # Add detailed loss components to info
329
+ for key, value in losses_dict.items():
330
+ if isinstance(value, (float, int)) or (hasattr(value, 'ndim') and value.ndim == 0):
331
+ info[key] = value
332
+ else:
333
+ info[key] = jnp.mean(value)
334
+ return new_state, info
335
+
336
+
337
+ def main(config: _config.TrainConfig):
338
+ init_logging()
339
+ logging.info(f"Running on: {platform.node()}")
340
+
341
+ if config.batch_size % jax.device_count() != 0:
342
+ raise ValueError(
343
+ f"Batch size {config.batch_size} must be divisible by the number of devices {jax.device_count()}."
344
+ )
345
+
346
+ jax.config.update("jax_compilation_cache_dir", str(epath.Path("~/.cache/jax").expanduser()))
347
+
348
+ # Generate random seed if not provided
349
+ seed = config.seed
350
+ if seed is None:
351
+ seed = int(time.time() * 1000) % (2**32)
352
+ logging.info(f"Using random seed for JAX RNG: {seed}")
353
+
354
+ rng = jax.random.key(seed)
355
+ train_rng, init_rng = jax.random.split(rng)
356
+
357
+ mesh = sharding.make_mesh(config.fsdp_devices)
358
+ data_sharding = jax.sharding.NamedSharding(mesh, jax.sharding.PartitionSpec(sharding.DATA_AXIS))
359
+ replicated_sharding = jax.sharding.NamedSharding(mesh, jax.sharding.PartitionSpec())
360
+
361
+ checkpoint_manager, resuming = _checkpoints.initialize_checkpoint_dir(
362
+ config.checkpoint_dir,
363
+ keep_period=config.keep_period,
364
+ overwrite=config.overwrite,
365
+ resume=config.resume,
366
+ )
367
+ init_wandb(config, resuming=resuming, enabled=config.wandb_enabled)
368
+
369
+ data_loader = _data_loader.create_behavior_data_loader(
370
+ config,
371
+ sharding=data_sharding,
372
+ shuffle=True,
373
+ )
374
+
375
+ data_iter = iter(data_loader)
376
+ batch = next(data_iter)
377
+ logging.info(f"Initialized data loader:\n{training_utils.array_tree_to_info(batch)}")
378
+
379
+ # Log images from first batch to sanity check.
380
+ images_to_log = [
381
+ wandb.Image(np.concatenate([np.array(img[i]) for img in batch[0].images.values()], axis=1))
382
+ for i in range(min(5, len(next(iter(batch[0].images.values())))))
383
+ ]
384
+ wandb.log({"camera_views": images_to_log}, step=0)
385
+
386
+ # Get norm_stats for correlation matrix loading
387
+ data_config = data_loader.data_config()
388
+ if data_config.norm_stats is None:
389
+ raise ValueError(
390
+ "norm_stats not found. Run compute_norm_stats.py to generate normalization statistics."
391
+ )
392
+ norm_stats = data_config.norm_stats
393
+
394
+ train_state, train_state_sharding = init_train_state(
395
+ config, init_rng, mesh, resume=resuming, norm_stats=norm_stats
396
+ )
397
+ jax.block_until_ready(train_state)
398
+ logging.info(f"Initialized train state:\n{training_utils.array_tree_to_info(train_state.params)}")
399
+
400
+ if resuming:
401
+ train_state = _checkpoints.restore_state(checkpoint_manager, train_state, data_loader)
402
+
403
+ # Reload correlation matrix after restore
404
+ model = nnx.merge(train_state.model_def, train_state.params)
405
+ model.load_correlation_matrix(norm_stats)
406
+ logging.info("Reloaded correlation matrix after checkpoint restore")
407
+ train_state = dataclasses.replace(train_state, model_def=nnx.graphdef(model))
408
+
409
+ ptrain_step = jax.jit(
410
+ functools.partial(train_step, config),
411
+ in_shardings=(replicated_sharding, train_state_sharding, data_sharding),
412
+ out_shardings=(train_state_sharding, replicated_sharding),
413
+ donate_argnums=(1,),
414
+ )
415
+
416
+ start_step = int(train_state.step)
417
+ pbar = tqdm.tqdm(
418
+ range(start_step, config.num_train_steps),
419
+ initial=start_step,
420
+ total=config.num_train_steps,
421
+ dynamic_ncols=True,
422
+ )
423
+
424
+ infos = []
425
+ for step in pbar:
426
+ with sharding.set_mesh(mesh):
427
+ train_state, info = ptrain_step(train_rng, train_state, batch)
428
+ infos.append(info)
429
+ if step % config.log_interval == 0:
430
+ stacked_infos = common_utils.stack_forest(infos)
431
+ reduced_info = jax.device_get(jax.tree.map(jnp.mean, stacked_infos))
432
+
433
+ # Create a concise console log with main metrics
434
+ main_metrics = {k: v for k, v in reduced_info.items()
435
+ if "loss" in k or "accuracy" in k or k in ["grad_norm", "param_norm", "grad_norm_vlm", "grad_norm_action_expert"]}
436
+ info_str = ", ".join(f"{k}={v:.4f}" for k, v in main_metrics.items())
437
+ pbar.write(f"Step {step}: {info_str}")
438
+ wandb.log(reduced_info, step=step)
439
+ infos = []
440
+ batch = next(data_iter)
441
+
442
+ if (step % config.save_interval == 0 and step > start_step) or step == config.num_train_steps - 1:
443
+ _checkpoints.save_state(checkpoint_manager, train_state, data_loader, step)
444
+
445
+ logging.info("Waiting for checkpoint manager to finish")
446
+ checkpoint_manager.wait_until_finished()
447
+
448
+
449
+ if __name__ == "__main__":
450
+ main(_config.cli())
task 57/policy_code/scripts/train_fast_tokenizer.py ADDED
@@ -0,0 +1,377 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Train FAST tokenizer for action encoding.
2
+
3
+ This script:
4
+ 1. Loads action chunks from B1K dataset (with sampling)
5
+ 2. Applies delta transforms and per-timestamp normalization
6
+ 3. Trains FAST tokenizer on specified action dimensions
7
+ 4. Saves tokenizer to assets directory
8
+ 5. Reports compression statistics
9
+ """
10
+
11
+ import json
12
+ import numpy as np
13
+ import pandas as pd
14
+ import tyro
15
+ from pathlib import Path
16
+ from concurrent.futures import ProcessPoolExecutor
17
+ from transformers import AutoProcessor
18
+
19
+ import openpi.transforms as transforms
20
+
21
+ # Import B1K-specific modules
22
+ from b1k.training import config as _config
23
+ from b1k.policies.b1k_policy import extract_state_from_proprio
24
+
25
+
26
+ def get_delta_transform_from_config(config_name: str):
27
+ """Get the delta action transform from config."""
28
+ config = _config.get_config(config_name)
29
+ data_config = config.data.create(config.assets_dirs, config.model)
30
+
31
+ delta_transform = None
32
+ for transform in data_config.data_transforms.inputs:
33
+ if isinstance(transform, transforms.DeltaActions):
34
+ delta_transform = transform
35
+ break
36
+
37
+ if delta_transform is None:
38
+ raise ValueError("No DeltaActions transform found in config")
39
+
40
+ return delta_transform
41
+
42
+
43
+ def apply_delta_transform(state: np.ndarray, actions: np.ndarray, mask) -> np.ndarray:
44
+ """Apply delta transform using the mask from config."""
45
+ if mask is None:
46
+ return actions
47
+
48
+ delta_actions = actions.copy()
49
+ mask = np.asarray(mask)
50
+ dims = mask.shape[-1]
51
+ delta_actions[:dims] = np.where(mask, actions[:dims] - state[:dims], actions[:dims])
52
+
53
+ return delta_actions
54
+
55
+
56
+ def process_episode_file(args):
57
+ """Process episode file and return action chunks."""
58
+ episode_file, delta_mask, action_horizon, sample_fraction = args
59
+
60
+ try:
61
+ df = pd.read_parquet(episode_file)
62
+
63
+ states = []
64
+ raw_actions = []
65
+
66
+ for _, row in df.iterrows():
67
+ raw_state = np.array(row["observation.state"])
68
+ raw_action = np.array(row["action"])
69
+
70
+ processed_state = extract_state_from_proprio(raw_state)
71
+
72
+ states.append(processed_state)
73
+ raw_actions.append(raw_action)
74
+
75
+ if len(raw_actions) < action_horizon:
76
+ return None
77
+
78
+ states = np.array(states)
79
+ raw_actions = np.array(raw_actions)
80
+
81
+ # Create action chunks (sliding window)
82
+ # IMPORTANT: Apply delta transform the SAME way as compute_norm_stats
83
+ # All actions in a chunk are relative to the FIRST state in that chunk
84
+ action_chunks = []
85
+ for i in range(len(states) - action_horizon + 1):
86
+ current_state = states[i] # First state in chunk
87
+ future_absolute_actions = raw_actions[i:i + action_horizon]
88
+
89
+ # Apply delta transform to each action using the SAME current state
90
+ delta_chunk = np.zeros_like(future_absolute_actions)
91
+ for t in range(action_horizon):
92
+ delta_chunk[t] = apply_delta_transform(current_state, future_absolute_actions[t], delta_mask)
93
+
94
+ action_chunks.append(delta_chunk)
95
+
96
+ if len(action_chunks) == 0:
97
+ return None
98
+
99
+ action_chunks = np.array(action_chunks)
100
+
101
+ # Sample chunks
102
+ if sample_fraction < 1.0:
103
+ n_chunks = len(action_chunks)
104
+ n_samples = max(1, int(n_chunks * sample_fraction))
105
+ # Use hash of episode filename as seed for randomness across episodes
106
+ # Use same modulo as compute_norm_stats.py for consistency
107
+ episode_seed = hash(str(episode_file)) % (2**31)
108
+ rng = np.random.RandomState(episode_seed)
109
+ indices = rng.choice(n_chunks, size=n_samples, replace=False)
110
+ action_chunks = action_chunks[indices]
111
+
112
+ return action_chunks
113
+
114
+ except Exception as e:
115
+ print(f"Error processing {episode_file}: {e}")
116
+ return None
117
+
118
+
119
+ def train_fast_tokenizer(
120
+ action_chunks: np.ndarray,
121
+ vocab_size: int = 1024,
122
+ scale: float = 10.0,
123
+ num_epochs: int = 10,
124
+ batch_size: int = 256,
125
+ ) -> AutoProcessor:
126
+ """
127
+ Train FAST tokenizer (BPE on DCT coefficients) on action chunks.
128
+
129
+ Uses the .fit() method to train a new tokenizer on the provided data.
130
+ """
131
+ print(f"Training FAST tokenizer on {len(action_chunks)} action chunks...")
132
+ print(f"Action chunk shape: {action_chunks.shape}")
133
+ print(f"Vocab size: {vocab_size}")
134
+ print(f"DCT scale: {scale}")
135
+
136
+ # Download the tokenizer source code (not pretrained weights)
137
+ # We'll train a new tokenizer on our own data
138
+ base_tokenizer = AutoProcessor.from_pretrained(
139
+ "physical-intelligence/fast",
140
+ trust_remote_code=True
141
+ )
142
+
143
+ # Convert action_chunks array to list of arrays (expected by .fit())
144
+ action_data_list = [action_chunks[i] for i in range(len(action_chunks))]
145
+
146
+ # Train the new tokenizer on our action data using .fit()
147
+ # This trains the BPE tokenizer on DCT coefficients
148
+ print("Training new tokenizer (this may take a few minutes)...")
149
+ tokenizer = base_tokenizer.fit(
150
+ action_data_list,
151
+ scale=scale,
152
+ vocab_size=vocab_size,
153
+ time_horizon=action_chunks.shape[1], # action_horizon
154
+ action_dim=action_chunks.shape[2], # encoded dimensions
155
+ )
156
+ print("✓ Tokenizer training complete!")
157
+
158
+ # Validate it works
159
+ sample_chunk = action_chunks[0]
160
+ encoded = tokenizer(sample_chunk[None])[0]
161
+ if isinstance(encoded, list):
162
+ encoded = np.array(encoded)
163
+ print(f"Sample encoding: {len(encoded)} tokens for chunk shape {sample_chunk.shape}")
164
+
165
+ return tokenizer
166
+
167
+
168
+ def compute_compression_stats(tokenizer, action_chunks: np.ndarray):
169
+ """Compute compression statistics."""
170
+ print("\nComputing compression statistics...")
171
+
172
+ # Sample for stats (use max 1000 chunks for speed)
173
+ sample_size = min(1000, len(action_chunks))
174
+ sample_indices = np.random.RandomState(42).choice(len(action_chunks), size=sample_size, replace=False)
175
+ sample_chunks = action_chunks[sample_indices]
176
+
177
+ token_lengths = []
178
+ for chunk in sample_chunks:
179
+ encoded = tokenizer(chunk[None])[0]
180
+ if isinstance(encoded, list):
181
+ token_lengths.append(len(encoded))
182
+ else:
183
+ token_lengths.append(encoded.shape[0] if hasattr(encoded, 'shape') else len(encoded))
184
+
185
+ token_lengths = np.array(token_lengths)
186
+
187
+ # Compression ratio: (H * D) / avg_tokens
188
+ input_size = action_chunks.shape[1] * action_chunks.shape[2]
189
+ avg_tokens = np.mean(token_lengths)
190
+ compression_ratio = input_size / avg_tokens
191
+
192
+ stats = {
193
+ 'compression_ratio': float(compression_ratio),
194
+ 'mean_token_length': float(np.mean(token_lengths)),
195
+ 'p99_token_length': float(np.percentile(token_lengths, 99)),
196
+ 'min_token_length': float(np.min(token_lengths)),
197
+ 'max_token_length': float(np.max(token_lengths)),
198
+ }
199
+
200
+ print(f"Compression Statistics:")
201
+ print(f" Average compression ratio: {stats['compression_ratio']:.2f}x")
202
+ print(f" Mean token length: {stats['mean_token_length']:.1f}")
203
+ print(f" P99 token length: {stats['p99_token_length']:.0f}")
204
+ print(f" Min token length: {stats['min_token_length']:.0f}")
205
+ print(f" Max token length: {stats['max_token_length']:.0f}")
206
+
207
+ return stats
208
+
209
+
210
+ def main(
211
+ config_name: str,
212
+ max_episodes: int | None = None,
213
+ num_workers: int | None = None,
214
+ sample_fraction: float = 0.1,
215
+ encoded_dims: str = "0:6,7:23",
216
+ vocab_size: int = 1024,
217
+ scale: float = 10.0,
218
+ num_epochs: int = 10,
219
+ ):
220
+ """
221
+ Train FAST tokenizer for action encoding.
222
+
223
+ Args:
224
+ config_name: Training config name
225
+ max_episodes: Max episodes to use (None = all episodes in dataset)
226
+ num_workers: Number of parallel workers
227
+ sample_fraction: Fraction of chunks to sample per episode
228
+ encoded_dims: Comma-separated dimension ranges (e.g., "0:6,7:23")
229
+ vocab_size: FAST vocabulary size (BPE vocab size)
230
+ scale: DCT scaling factor (default: 10.0)
231
+ num_epochs: Training epochs (not used by FAST)
232
+ """
233
+ # Load config
234
+ config = _config.get_config(config_name)
235
+ data_config = config.data.create(config.assets_dirs, config.model)
236
+
237
+ if not data_config.behavior_dataset_root:
238
+ raise ValueError("This script only works with B1K behavior datasets")
239
+
240
+ # Parse encoded dimensions
241
+ encoded_dim_ranges = []
242
+ for range_str in encoded_dims.split(','):
243
+ start, end = map(int, range_str.strip().split(':'))
244
+ encoded_dim_ranges.append((start, end))
245
+
246
+ total_encoded_dims = sum(end - start for start, end in encoded_dim_ranges)
247
+ print(f"Encoding {total_encoded_dims} dimensions: {encoded_dims}")
248
+
249
+ # Get delta transform
250
+ delta_transform = get_delta_transform_from_config(config_name)
251
+ delta_mask = delta_transform.mask
252
+ action_horizon = config.model.action_horizon
253
+
254
+ print(f"Action horizon: {action_horizon}")
255
+ print(f"Delta mask: {delta_mask}")
256
+
257
+ # Find episode files
258
+ data_root = Path(data_config.behavior_dataset_root)
259
+ all_episode_files = sorted(data_root.glob("data/task-*/episode_*.parquet"))
260
+
261
+ episode_files = all_episode_files
262
+ print(f"Using all {len(episode_files)} episodes")
263
+
264
+ if max_episodes is not None:
265
+ episode_files = episode_files[:max_episodes]
266
+ print(f"Limited to {len(episode_files)} episodes")
267
+
268
+ # Process episodes in parallel
269
+ num_workers = num_workers or min(32, len(episode_files))
270
+ args_list = [
271
+ (f, delta_mask, action_horizon, sample_fraction)
272
+ for f in episode_files
273
+ ]
274
+
275
+ print(f"\nProcessing episodes with {num_workers} workers...")
276
+ all_chunks = []
277
+ with ProcessPoolExecutor(max_workers=num_workers) as executor:
278
+ for chunks in executor.map(process_episode_file, args_list):
279
+ if chunks is not None:
280
+ all_chunks.append(chunks)
281
+
282
+ # Concatenate all chunks
283
+ all_chunks = np.concatenate(all_chunks, axis=0)
284
+ print(f"Collected {len(all_chunks)} action chunks")
285
+
286
+ # Extract only encoded dimensions FIRST (before denorm/renorm)
287
+ encoded_chunks = []
288
+ for start, end in encoded_dim_ranges:
289
+ encoded_chunks.append(all_chunks[:, :, start:end])
290
+ encoded_chunks = np.concatenate(encoded_chunks, axis=-1) # [N, H, D_encoded]
291
+ print(f"Extracted {encoded_chunks.shape[-1]} encoded dimensions")
292
+
293
+ # Apply normalization to encoded dimensions only
294
+ # NOTE: For FAST, we ALWAYS use QUANTILE normalization (no per-timestamp)
295
+ # This clips outliers and provides consistent [-1, 1] range for DCT compression
296
+ # At this point, encoded_chunks contains RAW DELTA ACTIONS (not normalized yet)
297
+ print(f"\nBefore normalization - overall stats:")
298
+ print(f" Min: {np.min(encoded_chunks):.4f}, Max: {np.max(encoded_chunks):.4f}")
299
+ print(f" Mean: {np.mean(encoded_chunks):.4f}, Std: {np.std(encoded_chunks):.4f}")
300
+
301
+ norm_stats = data_config.norm_stats
302
+ if norm_stats is not None:
303
+ action_stats = norm_stats['actions']
304
+
305
+ # Build encoded dimension indices
306
+ encoded_dim_indices = []
307
+ for start, end in encoded_dim_ranges:
308
+ encoded_dim_indices.extend(range(start, end))
309
+ encoded_dim_indices = np.array(encoded_dim_indices)
310
+
311
+ # Use QUANTILE normalization: clip to [q01, q99] and map to [-1, 1]
312
+ q01 = action_stats.q01[encoded_dim_indices] # [D_encoded]
313
+ q99 = action_stats.q99[encoded_dim_indices] # [D_encoded]
314
+
315
+ print(f"\nNormalization stats (q01, q99) for encoded dimensions:")
316
+ for i, dim_idx in enumerate(encoded_dim_indices):
317
+ print(f" Orig dim {dim_idx}: q01={q01[i]:7.4f}, q99={q99[i]:7.4f}, range={q99[i]-q01[i]:7.4f}")
318
+
319
+ # Clip to quantile range and normalize to [-1, 1]
320
+ encoded_chunks = np.clip(encoded_chunks, q01, q99)
321
+ encoded_chunks = 2.0 * (encoded_chunks - q01) / np.maximum(q99 - q01, 1e-6) - 1.0
322
+ print(f"\nApplied quantile normalization [q01, q99] → [-1, 1]")
323
+
324
+ print(f"\nAfter normalization - overall stats:")
325
+ print(f" Min: {np.min(encoded_chunks):.4f}, Max: {np.max(encoded_chunks):.4f}")
326
+ print(f" Mean: {np.mean(encoded_chunks):.4f}, Std: {np.std(encoded_chunks):.4f}")
327
+
328
+ print(f"\nPer-dimension stats (after normalization):")
329
+ for d in range(encoded_chunks.shape[-1]):
330
+ dim_data = encoded_chunks[:, :, d]
331
+ print(f" Dim {d}: min={np.min(dim_data):7.4f}, max={np.max(dim_data):7.4f}, "
332
+ f"mean={np.mean(dim_data):7.4f}, std={np.std(dim_data):7.4f}")
333
+ else:
334
+ print("No normalization stats found, using raw delta actions")
335
+
336
+ print(f"Encoded chunks shape: {encoded_chunks.shape}")
337
+
338
+ # Train FAST tokenizer
339
+ tokenizer = train_fast_tokenizer(
340
+ encoded_chunks,
341
+ vocab_size=vocab_size,
342
+ scale=scale,
343
+ num_epochs=num_epochs,
344
+ )
345
+
346
+ # Compute compression statistics
347
+ compression_stats = compute_compression_stats(tokenizer, encoded_chunks)
348
+
349
+ # Save tokenizer to assets directory
350
+ output_dir = config.assets_dirs / data_config.asset_id / "fast_tokenizer"
351
+ output_dir.mkdir(parents=True, exist_ok=True)
352
+
353
+ # Save tokenizer
354
+ tokenizer.save_pretrained(output_dir)
355
+
356
+ # Save metadata
357
+ metadata = {
358
+ 'vocab_size': vocab_size,
359
+ 'scale': scale,
360
+ 'encoded_dims': encoded_dims,
361
+ 'encoded_dim_ranges': encoded_dim_ranges,
362
+ 'total_encoded_dims': total_encoded_dims,
363
+ 'action_horizon': action_horizon,
364
+ 'num_training_chunks': len(encoded_chunks),
365
+ 'compression_stats': compression_stats,
366
+ }
367
+
368
+ with open(output_dir / "metadata.json", 'w') as f:
369
+ json.dump(metadata, f, indent=2)
370
+
371
+ print(f"\n✅ Saved FAST tokenizer to {output_dir}")
372
+ print(f"Metadata: {json.dumps(metadata, indent=2)}")
373
+
374
+
375
+ if __name__ == "__main__":
376
+ tyro.cli(main)
377
+
task 57/policy_code/src/b1k/__init__.py ADDED
@@ -0,0 +1,9 @@
 
 
 
 
 
 
 
 
 
 
1
+ """BEHAVIOR-1K Challenge Solution
2
+
3
+ Built on Pi0.5 from Physical Intelligence (https://www.physicalintelligence.company/)
4
+ for the BEHAVIOR-1K challenge (https://behavior.stanford.edu/challenge/)
5
+
6
+ Uses OpenPI fork: https://github.com/wensi-ai/openpi (behavior branch)
7
+ """
8
+
9
+ __version__ = "0.1.0"
task 57/policy_code/src/b1k/models/__init__.py ADDED
@@ -0,0 +1,6 @@
 
 
 
 
 
 
 
1
+ from b1k.models.pi_behavior import PiBehavior
2
+ from b1k.models.pi_behavior_config import PiBehaviorConfig, TASK_NUM_STAGES, MAX_NUM_STAGES
3
+ from b1k.models.observation import Observation
4
+
5
+ __all__ = ["PiBehavior", "PiBehaviorConfig", "TASK_NUM_STAGES", "MAX_NUM_STAGES", "Observation"]
6
+
task 57/policy_code/src/b1k/models/observation.py ADDED
@@ -0,0 +1,143 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Observation class and preprocessing with FAST auxiliary fields support.
2
+
3
+ Based on openpi with FAST fields added for PI_BEHAVIOR model.
4
+
5
+ Reference: https://github.com/wensi-ai/openpi/blob/behavior/src/openpi/models/model.py
6
+ """
7
+
8
+ from collections.abc import Sequence
9
+ from typing import Generic, TypeVar
10
+ import dataclasses
11
+
12
+ import augmax
13
+ from flax import struct
14
+ import jax
15
+ import jax.numpy as jnp
16
+ import numpy as np
17
+ import torch
18
+
19
+ from openpi.shared import image_tools
20
+ from openpi.shared import array_typing as at
21
+
22
+ ArrayT = TypeVar("ArrayT", bound=jax.Array | torch.Tensor | np.ndarray)
23
+
24
+ IMAGE_KEYS = (
25
+ "base_0_rgb",
26
+ "left_wrist_0_rgb",
27
+ "right_wrist_0_rgb",
28
+ )
29
+ IMAGE_RESOLUTION = (224, 224)
30
+
31
+
32
+ @at.typecheck
33
+ @struct.dataclass
34
+ class Observation(Generic[ArrayT]):
35
+ """Observation with FAST auxiliary fields."""
36
+
37
+ images: dict[str, at.Float[ArrayT, "*b h w c"]]
38
+ image_masks: dict[str, at.Bool[ArrayT, "*b"]]
39
+ state: at.Float[ArrayT, "*b s"]
40
+ tokenized_prompt: at.Int[ArrayT, "*b l"] | None = None
41
+ tokenized_prompt_mask: at.Bool[ArrayT, "*b l"] | None = None
42
+ token_ar_mask: at.Int[ArrayT, "*b l"] | None = None
43
+ token_loss_mask: at.Bool[ArrayT, "*b l"] | None = None
44
+
45
+ fast_tokens: at.Int[ArrayT, "*b t"] | None = None
46
+ fast_token_mask: at.Bool[ArrayT, "*b t"] | None = None
47
+
48
+ @classmethod
49
+ def from_dict(cls, data: at.PyTree[ArrayT]) -> "Observation[ArrayT]":
50
+ """Convert dict to Observation."""
51
+ if ("tokenized_prompt" in data) != ("tokenized_prompt_mask" in data):
52
+ raise ValueError("tokenized_prompt and tokenized_prompt_mask must be provided together.")
53
+
54
+ # Convert uint8 images to float32 [-1, 1]
55
+ for key in data["image"]:
56
+ if data["image"][key].dtype == np.uint8:
57
+ data["image"][key] = data["image"][key].astype(np.float32) / 255.0 * 2.0 - 1.0
58
+ elif hasattr(data["image"][key], "dtype") and data["image"][key].dtype == torch.uint8:
59
+ data["image"][key] = data["image"][key].to(torch.float32).permute(0, 3, 1, 2) / 255.0 * 2.0 - 1.0
60
+
61
+ return cls(
62
+ images=data["image"],
63
+ image_masks=data["image_mask"],
64
+ state=data["state"],
65
+ tokenized_prompt=data.get("tokenized_prompt"),
66
+ tokenized_prompt_mask=data.get("tokenized_prompt_mask"),
67
+ token_ar_mask=data.get("token_ar_mask"),
68
+ token_loss_mask=data.get("token_loss_mask"),
69
+ fast_tokens=data.get("fast_tokens"),
70
+ fast_token_mask=data.get("fast_token_mask"),
71
+ )
72
+
73
+ def to_dict(self) -> at.PyTree[ArrayT]:
74
+ """Convert Observation to dict."""
75
+ result = dataclasses.asdict(self)
76
+ result["image"] = result.pop("images")
77
+ result["image_mask"] = result.pop("image_masks")
78
+ return result
79
+
80
+
81
+ def preprocess_observation(
82
+ rng: at.KeyArrayLike | None,
83
+ observation: Observation,
84
+ *,
85
+ train: bool = False,
86
+ image_keys: Sequence[str] = IMAGE_KEYS,
87
+ image_resolution: tuple[int, int] = IMAGE_RESOLUTION,
88
+ ) -> Observation:
89
+ """Preprocess observations with image augmentation and FAST fields preservation."""
90
+ if not set(image_keys).issubset(observation.images):
91
+ raise ValueError(f"images dict missing keys: expected {image_keys}, got {list(observation.images)}")
92
+
93
+ batch_shape = observation.state.shape[:-1]
94
+
95
+ out_images = {}
96
+ for key in image_keys:
97
+ image = observation.images[key]
98
+ if image.shape[1:3] != image_resolution:
99
+ image = image_tools.resize_with_pad(image, *image_resolution)
100
+
101
+ if train:
102
+ # Convert from [-1, 1] to [0, 1] for augmax
103
+ image = image / 2.0 + 0.5
104
+
105
+ transforms = []
106
+ if "wrist" not in key:
107
+ height, width = image.shape[1:3]
108
+ transforms += [
109
+ augmax.RandomCrop(int(width * 0.95), int(height * 0.95)),
110
+ augmax.Resize(width, height),
111
+ augmax.Rotate((-5, 5)),
112
+ ]
113
+ transforms += [
114
+ augmax.ColorJitter(brightness=0.3, contrast=0.4, saturation=0.5),
115
+ ]
116
+ sub_rngs = jax.random.split(rng, image.shape[0])
117
+ image = jax.vmap(augmax.Chain(*transforms))(sub_rngs, image)
118
+
119
+ # Back to [-1, 1]
120
+ image = image * 2.0 - 1.0
121
+
122
+ out_images[key] = image
123
+
124
+ # Obtain masks
125
+ out_masks = {}
126
+ for key in out_images:
127
+ if key not in observation.image_masks:
128
+ out_masks[key] = jnp.ones(batch_shape, dtype=jnp.bool)
129
+ else:
130
+ out_masks[key] = jnp.asarray(observation.image_masks[key])
131
+
132
+ return Observation(
133
+ images=out_images,
134
+ image_masks=out_masks,
135
+ state=observation.state,
136
+ tokenized_prompt=observation.tokenized_prompt,
137
+ tokenized_prompt_mask=observation.tokenized_prompt_mask,
138
+ token_ar_mask=observation.token_ar_mask,
139
+ token_loss_mask=observation.token_loss_mask,
140
+ fast_tokens=getattr(observation, 'fast_tokens', None),
141
+ fast_token_mask=getattr(observation, 'fast_token_mask', None),
142
+ )
143
+
task 57/policy_code/src/b1k/models/pi_behavior.py ADDED
@@ -0,0 +1,1118 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """The main model for BEHAVIOR-1K challenge.
2
+
3
+ Based on Pi0.5 implementation from PhysicalIntelligence/openpi
4
+ """
5
+
6
+ import logging
7
+ import pathlib
8
+
9
+ import einops
10
+ import flax.nnx as nnx
11
+ import flax.nnx.bridge as nnx_bridge
12
+ import jax
13
+ import jax.numpy as jnp
14
+ from typing_extensions import override
15
+
16
+ from openpi.models import model as _model
17
+ from openpi.models import gemma as _gemma
18
+ from openpi.models import siglip as _siglip
19
+ from openpi.models.pi0 import make_attn_mask, posemb_sincos
20
+ from openpi.shared import array_typing as at
21
+
22
+ # Import from our custom modules
23
+ from b1k.models import pi_behavior_config
24
+ from b1k.models.observation import Observation, preprocess_observation
25
+ from b1k.models.pi_behavior_config import (
26
+ TASK_NUM_STAGES,
27
+ MAX_NUM_STAGES,
28
+ TOTAL_TASK_STAGE_EMBEDDINGS,
29
+ TASK_STAGE_OFFSETS
30
+ )
31
+
32
+ logger = logging.getLogger("b1k")
33
+
34
+
35
+ class KVCacheTransform(nnx.Module):
36
+ """Transforms prefix KV cache by mixing across layers.
37
+
38
+ Each destination layer's K and V become learnable linear combinations
39
+ of all source layers' K and V, plus a bias term. This allows the action
40
+ expert to attend to learned combinations of VLM layers rather than being
41
+ forced to attend layer-by-layer.
42
+
43
+ Initialized as identity transform (k_coeffs = I, bias = 0) so the model
44
+ starts with the same behavior as without transformation.
45
+ """
46
+
47
+ def __init__(self, num_layers: int, head_dim: int, num_kv_heads: int, rngs: nnx.Rngs):
48
+ # K transformation: [dest_layer, src_layer]
49
+ # Initialize as identity so transformation is initially a no-op
50
+ self.k_coeffs = nnx.Param(jnp.eye(num_layers, dtype=jnp.float32))
51
+
52
+ # K bias: [layer, num_kv_heads, head_dim]
53
+ # Initialize as zeros
54
+ self.k_bias = nnx.Param(jnp.zeros((num_layers, num_kv_heads, head_dim), dtype=jnp.float32))
55
+
56
+ # V transformation (independent from K)
57
+ self.v_coeffs = nnx.Param(jnp.eye(num_layers, dtype=jnp.float32))
58
+ self.v_bias = nnx.Param(jnp.zeros((num_layers, num_kv_heads, head_dim), dtype=jnp.float32))
59
+
60
+ def __call__(self, kv_cache: tuple[jnp.ndarray, jnp.ndarray]) -> tuple[jnp.ndarray, jnp.ndarray]:
61
+ """Transform KV cache by mixing across layers.
62
+
63
+ Args:
64
+ kv_cache: Tuple of (cache_k, cache_v) where each has shape
65
+ [num_layers, batch, seq_len, num_kv_heads, head_dim]
66
+
67
+ Returns:
68
+ Transformed (k_new, v_new) with same shape and dtype as input
69
+ """
70
+ cache_k, cache_v = kv_cache
71
+ # Shape: [layers, batch, seq_len, num_kv_heads, head_dim]
72
+
73
+ # Preserve original dtype (important for bfloat16 training)
74
+ original_dtype = cache_k.dtype
75
+
76
+ # Transform K: each destination layer is a weighted combination of all source layers
77
+ # k_new[dest] = sum_src(k_coeffs[dest, src] * cache_k[src]) + k_bias[dest]
78
+ # Einsum: [dest, src] @ [src, batch, seq, heads, dim] -> [dest, batch, seq, heads, dim]
79
+ k_new = jnp.einsum('ds,sbtkh->dbtkh', self.k_coeffs.value, cache_k)
80
+ k_new = k_new + self.k_bias.value[:, None, None, :, :] # Add bias
81
+
82
+ # Transform V (same operation, independent parameters)
83
+ v_new = jnp.einsum('ds,sbtkh->dbtkh', self.v_coeffs.value, cache_v)
84
+ v_new = v_new + self.v_bias.value[:, None, None, :, :]
85
+
86
+ # Cast back to original dtype
87
+ k_new = k_new.astype(original_dtype)
88
+ v_new = v_new.astype(original_dtype)
89
+
90
+ return (k_new, v_new)
91
+
92
+
93
+ class PiBehavior(_model.BaseModel):
94
+ def __init__(self, config: pi_behavior_config.PiBehaviorConfig, rngs: nnx.Rngs):
95
+ super().__init__(config.action_dim, config.action_horizon, config.max_token_len)
96
+
97
+ # Store config for later use
98
+ self.config = config
99
+
100
+ paligemma_config = _gemma.get_config(config.paligemma_variant)
101
+ action_expert_config = _gemma.get_config(config.action_expert_variant)
102
+
103
+ # Initialize Gemma models with AdaRMS (Pi05 style)
104
+ llm = nnx_bridge.ToNNX(
105
+ _gemma.Module(
106
+ configs=[paligemma_config, action_expert_config],
107
+ embed_dtype=config.dtype,
108
+ adarms=True,
109
+ )
110
+ )
111
+ llm.lazy_init(rngs=rngs, method="init", use_adarms=[False, True])
112
+
113
+ # Initialize vision model
114
+ img = nnx_bridge.ToNNX(
115
+ _siglip.Module(
116
+ num_classes=paligemma_config.width,
117
+ variant="So400m/14",
118
+ pool_type="none",
119
+ scan=True,
120
+ dtype_mm=config.dtype,
121
+ )
122
+ )
123
+ img.lazy_init(next(iter(config.fake_obs().images.values())), train=False, rngs=rngs)
124
+
125
+ self.PaliGemma = nnx.Dict(llm=llm, img=img)
126
+
127
+ # KV cache transformation for cross-layer attention
128
+ # Allows action expert to attend to learned combinations of VLM layers
129
+ if config.use_kv_transform:
130
+ self.kv_transform = KVCacheTransform(
131
+ num_layers=paligemma_config.depth,
132
+ head_dim=paligemma_config.head_dim,
133
+ num_kv_heads=paligemma_config.num_kv_heads,
134
+ rngs=rngs
135
+ )
136
+ else:
137
+ self.kv_transform = None
138
+
139
+ # Task embeddings table - trainable embeddings for each task
140
+ self.task_embeddings = nnx.Embed(
141
+ num_embeddings=config.num_tasks,
142
+ features=config.task_embedding_dim,
143
+ rngs=rngs,
144
+ )
145
+
146
+ # Stage predictor - predicts stage from VLM output of base task token
147
+ # Outputs MAX_NUM_STAGES logits, but invalid stages are masked per task
148
+ self.stage_pred_from_vlm = nnx.Linear(paligemma_config.width, MAX_NUM_STAGES, rngs=rngs)
149
+
150
+ # Task + subtask fusion layers
151
+ # Combines task embedding + cos/sin encoded subtask state
152
+ self.subtask_encoding_dim = config.task_embedding_dim // 2 # Half of task embedding dim (1024)
153
+
154
+ # Task-specific stage embeddings (one per stage per task)
155
+ # Total embeddings = sum of stages across all tasks (596 for 5-15 stages per task)
156
+ self.task_stage_embeddings = nnx.Embed(
157
+ num_embeddings=TOTAL_TASK_STAGE_EMBEDDINGS,
158
+ features=self.subtask_encoding_dim,
159
+ rngs=rngs,
160
+ )
161
+
162
+ # Gated fusion layers
163
+ # Input: task_embedding + sincos + task_stage_emb = task_dim + 2*subtask_dim
164
+ fusion_input_dim = config.task_embedding_dim + 2 * self.subtask_encoding_dim
165
+
166
+ # Gate networks to learn how to combine different signals
167
+ self.gate_sincos = nnx.Linear(fusion_input_dim, self.subtask_encoding_dim, rngs=rngs)
168
+ self.gate_task_stage = nnx.Linear(fusion_input_dim, self.subtask_encoding_dim, rngs=rngs)
169
+ self.gate_task = nnx.Linear(fusion_input_dim, config.task_embedding_dim, rngs=rngs)
170
+
171
+ # Fusion networks to create multiple conditioned vectors
172
+ self.fusion_layer1 = nnx.Linear(fusion_input_dim, config.task_embedding_dim * 2, rngs=rngs)
173
+ self.fusion_layer2 = nnx.Linear(config.task_embedding_dim * 2, config.task_embedding_dim, rngs=rngs)
174
+
175
+ # Additional projection for stage-dominant representation (2 signals now)
176
+ self.stage_projection = nnx.Linear(2 * self.subtask_encoding_dim, config.task_embedding_dim, rngs=rngs)
177
+
178
+ # Pi05 style layers
179
+ self.action_in_proj = nnx.Linear(config.action_dim, action_expert_config.width, rngs=rngs)
180
+ self.time_mlp_in = nnx.Linear(action_expert_config.width, action_expert_config.width, rngs=rngs)
181
+ self.time_mlp_out = nnx.Linear(action_expert_config.width, action_expert_config.width, rngs=rngs)
182
+ self.action_out_proj = nnx.Linear(action_expert_config.width, config.action_dim, rngs=rngs)
183
+
184
+ # Correlated noise generation
185
+ # Initialize as NNX Intermediate (excluded from checkpoints, loaded from norm_stats)
186
+ # Full correlation matrix with beta shrinkage for robustness
187
+ flat_dim = config.action_horizon * config.action_dim
188
+ self.action_correlation_cholesky = nnx.Intermediate(
189
+ jnp.eye(flat_dim), # Identity matrix as placeholder
190
+ )
191
+ self.correlation_loaded = False # Track if correlation matrix has been loaded
192
+ self.use_correlated_noise = config.use_correlated_noise
193
+ self.correlation_beta = config.correlation_beta # Shrinkage parameter for regularization
194
+
195
+ # Inpainting cache: stores precomputed matrices for simple correlation-based inpainting
196
+ # Key: num_inpainted_steps (length of inpainted sequence)
197
+ # Value: dict with {O_indices, U_indices, Sigma_UO_SOOinv}
198
+ self.inpainting_cache = {}
199
+
200
+ # FAST auxiliary training components
201
+ if config.use_fast_auxiliary:
202
+ # FAST embedding layer (vocab_size → paligemma_width)
203
+ # Use paligemma width (2048) to match other prefix tokens
204
+ self.fast_token_embedding = nnx.Embed(
205
+ num_embeddings=config.fast_vocab_size,
206
+ features=paligemma_config.width,
207
+ rngs=rngs
208
+ )
209
+
210
+ # FAST projection head (paligemma_width → vocab_size)
211
+ self.fast_token_proj = nnx.Linear(
212
+ paligemma_config.width,
213
+ config.fast_vocab_size,
214
+ rngs=rngs
215
+ )
216
+
217
+ logger.info(f"FAST auxiliary enabled, vocab_size={config.fast_vocab_size}")
218
+
219
+ # This attribute gets automatically set by model.train() and model.eval().
220
+ self.deterministic = True
221
+
222
+ def encode_subtask_state(
223
+ self,
224
+ subtask_state: at.Int[at.Array, " b"],
225
+ task_ids: at.Int[at.Array, " b"]
226
+ ) -> at.Float[at.Array, "b {self.subtask_encoding_dim}"]:
227
+ """Encode subtask state using cos/sin positional encoding, scaled per task.
228
+
229
+ Args:
230
+ subtask_state: Current stage for each sample [B]
231
+ task_ids: Task ID for each sample [B]
232
+
233
+ Returns:
234
+ Positional encodings scaled to [0, 1] range based on task-specific stage count [B, 1024]
235
+ """
236
+ # Get number of stages for each task in batch using JAX array indexing
237
+ # Convert tuple to JAX array inside function to avoid import-time device allocation
238
+ task_num_stages_array = jnp.array(TASK_NUM_STAGES, dtype=jnp.int32)
239
+ task_num_stages = task_num_stages_array[task_ids] # [B] - JAX array indexing
240
+
241
+ # Normalize: stage 0 → 0.0, last stage → 1.0 (per-task scaling)
242
+ # Add maximum to avoid division by zero for edge cases
243
+ normalized_state = subtask_state.astype(jnp.float32) / jnp.maximum(task_num_stages.astype(jnp.float32) - 1.0, 1.0)
244
+
245
+ # Use cos/sin encoding similar to timestep encoding
246
+ return posemb_sincos(
247
+ normalized_state,
248
+ self.subtask_encoding_dim,
249
+ min_period=1e-3,
250
+ max_period=1.0
251
+ )
252
+
253
+ def load_correlation_matrix(self, norm_stats: dict):
254
+ """Load full correlation matrix from normalization statistics and apply shrinkage.
255
+
256
+ This should be called after model initialization when norm_stats are available.
257
+ Applies shrinkage regularization: S_reg = beta * S + (1-beta) * I for robustness.
258
+
259
+ Args:
260
+ norm_stats: Dictionary containing normalization statistics (from normalize.load()),
261
+ with 'actions' key containing NormStats with action_correlation_cholesky field.
262
+
263
+ Raises:
264
+ ValueError: If use_correlated_noise=True but correlation matrix is missing.
265
+ TypeError: If norm_stats structure is incorrect.
266
+ """
267
+ if not self.use_correlated_noise:
268
+ logger.info("Correlated noise disabled in config, skipping correlation matrix loading")
269
+ return
270
+
271
+ # Validate norm_stats is a dict
272
+ if not isinstance(norm_stats, dict):
273
+ raise TypeError(
274
+ f"norm_stats must be a dict, got {type(norm_stats).__name__}. "
275
+ "Ensure norm_stats are loaded using openpi.shared.normalize.load()."
276
+ )
277
+
278
+ # Check 'actions' key exists
279
+ if 'actions' not in norm_stats:
280
+ raise ValueError(
281
+ "use_correlated_noise=True but 'actions' key not found in norm_stats. "
282
+ f"Found keys: {list(norm_stats.keys())}. "
283
+ "Run compute_norm_stats.py with --correlation flag to generate correlation matrix."
284
+ )
285
+
286
+ actions_stats = norm_stats['actions']
287
+
288
+ # Extract correlation matrix (support both dict and attribute access for flexibility)
289
+ if isinstance(actions_stats, dict):
290
+ chol_matrix = actions_stats.get('action_correlation_cholesky')
291
+ access_method = "dict"
292
+ elif hasattr(actions_stats, 'action_correlation_cholesky'):
293
+ chol_matrix = actions_stats.action_correlation_cholesky
294
+ access_method = "attribute"
295
+ else:
296
+ raise TypeError(
297
+ f"norm_stats['actions'] has unexpected type {type(actions_stats).__name__} "
298
+ f"and cannot access 'action_correlation_cholesky'. "
299
+ "Ensure norm_stats are loaded using openpi.shared.normalize.load()."
300
+ )
301
+
302
+ # Strict validation: correlation matrix must exist and be non-None
303
+ if chol_matrix is None:
304
+ raise ValueError(
305
+ "use_correlated_noise=True but 'action_correlation_cholesky' is None in norm_stats['actions']. "
306
+ "This means the correlation matrix was not computed during norm_stats generation. "
307
+ "Run compute_norm_stats.py with --correlation flag to generate correlation matrix."
308
+ )
309
+
310
+ logger.info(f"Successfully accessed correlation matrix via {access_method} access")
311
+
312
+ # Validate correlation matrix shape
313
+ expected_dim = self.action_horizon * self.action_dim
314
+ try:
315
+ L = jnp.array(chol_matrix)
316
+ except Exception as e:
317
+ raise ValueError(
318
+ f"Failed to convert action_correlation_cholesky to array: {e}. "
319
+ "The correlation matrix may be corrupted or in an invalid format."
320
+ )
321
+
322
+ if L.ndim != 2 or L.shape[0] != L.shape[1]:
323
+ raise ValueError(
324
+ f"action_correlation_cholesky must be a square 2D matrix, got shape {L.shape}. "
325
+ f"Expected shape: ({expected_dim}, {expected_dim})"
326
+ )
327
+
328
+ if L.shape[0] != expected_dim:
329
+ raise ValueError(
330
+ f"action_correlation_cholesky has wrong dimensions: {L.shape[0]}x{L.shape[0]}. "
331
+ f"Expected {expected_dim}x{expected_dim} (action_horizon={self.action_horizon} * action_dim={self.action_dim}). "
332
+ "This indicates the correlation matrix was computed for a different action space configuration."
333
+ )
334
+
335
+ # Reconstruct covariance matrix from Cholesky
336
+ Sigma = L @ L.T
337
+
338
+ # Apply shrinkage regularization: Σ_reg = beta * Σ + (1-beta) * I
339
+ beta = self.correlation_beta
340
+ logger.info(f"Applying shrinkage regularization with beta={beta:.2f}")
341
+
342
+ Sigma_reg = beta * Sigma + (1 - beta) * jnp.eye(Sigma.shape[0])
343
+
344
+ # Compute Cholesky decomposition of regularized covariance
345
+ try:
346
+ L_reg = jnp.linalg.cholesky(Sigma_reg)
347
+ except Exception as e:
348
+ raise RuntimeError(
349
+ f"Cholesky decomposition failed on regularized covariance: {e}. "
350
+ "This indicates the regularized correlation matrix is not positive definite. "
351
+ f"Current beta={beta:.2f}. Try decreasing correlation_beta closer to 0.0 for more shrinkage/regularization."
352
+ )
353
+
354
+ # Update the Intermediate value
355
+ self.action_correlation_cholesky.value = L_reg
356
+ self.correlation_loaded = True
357
+
358
+ logger.info(
359
+ f"✓ Loaded correlation matrix with shape {L_reg.shape} "
360
+ f"(beta={beta:.2f} shrinkage applied)"
361
+ )
362
+ logger.info(
363
+ f" Memory usage: {L_reg.nbytes / 1024 / 1024:.2f} MB"
364
+ )
365
+
366
+ def generate_correlated_noise(
367
+ self,
368
+ rng: at.KeyArrayLike,
369
+ batch_size: int,
370
+ ) -> at.Float[at.Array, "b {self.action_horizon} {self.action_dim}"]:
371
+ """Generate correlated noise matching action covariance structure.
372
+
373
+ Uses full correlation matrix with optional beta shrinkage for robustness.
374
+
375
+ Args:
376
+ rng: Random key for noise generation
377
+ batch_size: Number of noise samples to generate
378
+
379
+ Returns:
380
+ Correlated noise with shape [batch_size, action_horizon, action_dim]
381
+
382
+ Raises:
383
+ RuntimeError: If use_correlated_noise=True but correlation matrix not loaded.
384
+ """
385
+ if not self.use_correlated_noise:
386
+ # Independent Gaussian noise when correlated noise is disabled
387
+ return jax.random.normal(rng, (batch_size, self.action_horizon, self.action_dim))
388
+
389
+ if not self.correlation_loaded:
390
+ raise RuntimeError(
391
+ "use_correlated_noise=True but correlation matrix is not loaded. "
392
+ "Ensure load_correlation_matrix() was called during model initialization. "
393
+ "Run compute_norm_stats.py with --correlation flag to generate correlation matrix."
394
+ )
395
+
396
+ # Generate standard correlated noise using Cholesky decomposition
397
+ flat_dim = self.action_horizon * self.action_dim
398
+ standard_normal = jax.random.normal(rng, (batch_size, flat_dim))
399
+ correlated_flat = standard_normal @ self.action_correlation_cholesky.value.T
400
+ correlated_noise = correlated_flat.reshape(batch_size, self.action_horizon, self.action_dim)
401
+ return correlated_noise
402
+
403
+ def _precompute_correction_matrix(
404
+ self,
405
+ O_indices: at.Int[at.Array, " nO"],
406
+ U_indices: at.Int[at.Array, " nU"],
407
+ ) -> dict:
408
+ """Precompute matrix for correlation-aware inpainting correction.
409
+
410
+ Computes Σ_{UO}Σ_{OO}^{-1} which propagates corrections from O to U
411
+ while preserving correlation structure.
412
+
413
+ Args:
414
+ O_indices: Flat indices of inpainted dimensions [|O|]
415
+ U_indices: Flat indices of free dimensions [|U|]
416
+
417
+ Returns:
418
+ Dictionary with {O_indices, U_indices, correction_matrix}
419
+
420
+ Raises:
421
+ RuntimeError: If correlation matrix is not loaded
422
+ """
423
+ if not self.correlation_loaded:
424
+ raise RuntimeError(
425
+ "Cannot precompute correction matrix: correlation matrix not loaded. "
426
+ "Call load_correlation_matrix() first."
427
+ )
428
+
429
+ L = self.action_correlation_cholesky.value
430
+ Sigma = L @ L.T # Full covariance matrix [hd, hd]
431
+
432
+ # Extract submatrices
433
+ Sigma_OO = Sigma[jnp.ix_(O_indices, O_indices)] # [|O|, |O|]
434
+ Sigma_UO = Sigma[jnp.ix_(U_indices, O_indices)] # [|U|, |O|]
435
+
436
+ # Compute correction matrix: Σ_{UO} @ Σ_{OO}^{-1}
437
+ # This propagates corrections from O to U
438
+ eps_OO = 1e-6 * jnp.maximum(jnp.mean(jnp.diag(Sigma_OO)), 1.0)
439
+ Sigma_OO_reg = Sigma_OO + eps_OO * jnp.eye(Sigma_OO.shape[0])
440
+
441
+ # Solve Σ_{OO}_reg @ X = Σ_{UO}.T for X, then transpose
442
+ correction_matrix = jax.scipy.linalg.solve(
443
+ Sigma_OO_reg, Sigma_UO.T, assume_a='pos'
444
+ ).T # [|U|, |O|]
445
+
446
+ return {
447
+ 'O_indices': O_indices,
448
+ 'U_indices': U_indices,
449
+ 'correction_matrix': correction_matrix, # Σ_{UO}Σ_{OO}^{-1}
450
+ }
451
+
452
+ def fuse_task_and_subtask(
453
+ self, task_embedding: at.Float[at.Array, "b d"], task_ids: at.Int[at.Array, " b"], subtask_state: at.Int[at.Array, " b"]
454
+ ) -> at.Float[at.Array, "b n d"]:
455
+ """Fuse task embedding with subtask state encoding using multiple representations.
456
+
457
+ Returns multiple vectors that are differently conditioned by the subtask state:
458
+ 1. Task-gated representation (task embedding modulated by subtask)
459
+ 2. Balanced fusion (task + subtask combined)
460
+ 3. Stage-dominant representation (subtask features projected to task space)
461
+ 4. Pure stage representation (concatenated learned embeddings)
462
+
463
+ All output representations have dimension 2048 (task_embedding_dim).
464
+
465
+ Args:
466
+ task_embedding: Base task embedding [b, 2048]
467
+ task_ids: Task IDs for task-specific stage embeddings [b]
468
+ subtask_state: Subtask state indices [b]
469
+
470
+ Returns:
471
+ Multiple fused embeddings [b, 4, 2048]
472
+ """
473
+ # Get subtask representations
474
+ sincos_encoding = self.encode_subtask_state(subtask_state, task_ids) # [b, 1024]
475
+
476
+ # Task-specific stage embedding with corrected indexing
477
+ # Use vectorized lookup: offset + stage for each task
478
+ # Convert tuple to JAX array inside function to avoid import-time device allocation
479
+ task_stage_offsets_array = jnp.array(TASK_STAGE_OFFSETS, dtype=jnp.int32)
480
+ task_stage_offsets = task_stage_offsets_array[task_ids] # [b] - JAX array indexing
481
+ task_stage_idx = task_stage_offsets + subtask_state # [b]
482
+ task_stage_embedding = self.task_stage_embeddings(task_stage_idx) # [b, 1024]
483
+
484
+ # Concatenate inputs for gating: task (2048) + sincos (1024) + task_stage (1024) = 4096
485
+ all_inputs = jnp.concatenate([
486
+ task_embedding, # [b, 2048]
487
+ sincos_encoding, # [b, 1024]
488
+ task_stage_embedding # [b, 1024]
489
+ ], axis=-1) # [b, 4096]
490
+
491
+ # Learn gates for each component (sigmoid to get 0-1 scaling)
492
+ gate_sincos = nnx.sigmoid(self.gate_sincos(all_inputs)) # [b, 1024]
493
+ gate_task_stage = nnx.sigmoid(self.gate_task_stage(all_inputs)) # [b, 1024]
494
+ gate_task = nnx.sigmoid(self.gate_task(all_inputs)) # [b, 2048]
495
+
496
+ # 1. Task-gated representation: task embedding modulated by subtask info [b, 2048]
497
+ task_gated = task_embedding * gate_task
498
+
499
+ # 2. Balanced fusion: combine all signals through fusion network [b, 2048]
500
+ x = self.fusion_layer1(all_inputs) # [b, 4096]
501
+ x = nnx.relu(x)
502
+ balanced_fusion = self.fusion_layer2(x) # [b, 2048]
503
+
504
+ # 3. Stage-dominant: weighted combination of stage signals, then project [b, 2048]
505
+ gated_stage_features = jnp.concatenate([
506
+ sincos_encoding * gate_sincos, # [b, 1024]
507
+ task_stage_embedding * gate_task_stage # [b, 1024]
508
+ ], axis=-1) # [b, 2048]
509
+ stage_dominant = self.stage_projection(gated_stage_features) # [b, 2048]
510
+
511
+ # 4. Pure stage: concatenate the embeddings (already 2048) [b, 2048]
512
+ pure_stage = jnp.concatenate([sincos_encoding, task_stage_embedding], axis=-1)
513
+
514
+ # Stack all four representations [b, 4, 2048]
515
+ fused_embeddings = jnp.stack([task_gated, balanced_fusion, stage_dominant, pure_stage], axis=1)
516
+
517
+ return fused_embeddings
518
+
519
+ @at.typecheck
520
+ def embed_prefix(
521
+ self,
522
+ obs: Observation
523
+ ) -> tuple[
524
+ at.Float[at.Array, "b s emb"],
525
+ at.Bool[at.Array, "b s"],
526
+ at.Bool[at.Array, " s"]
527
+ ]:
528
+ """
529
+ Embed prefix: images + task + state + FAST_tokens (if provided).
530
+
531
+ Args:
532
+ obs: Observation (may include fast_tokens and fast_token_mask)
533
+
534
+ Returns:
535
+ tokens, input_mask, ar_mask
536
+ """
537
+ input_mask = []
538
+ ar_mask = []
539
+ tokens = []
540
+
541
+ # Embed images
542
+ image_token_list = []
543
+ # Respect freeze_vision_backbone config: if frozen, always use train=False
544
+ # If not frozen, use the model's training state (self.deterministic)
545
+ vision_train_mode = (not self.deterministic) and (not self.config.freeze_vision_backbone)
546
+
547
+ for name in obs.images:
548
+ image_tokens, _ = self.PaliGemma.img(obs.images[name], train=vision_train_mode)
549
+ image_token_list.append(image_tokens) # Store for subtask prediction
550
+
551
+ tokens.append(image_tokens)
552
+ input_mask.append(
553
+ einops.repeat(
554
+ obs.image_masks[name],
555
+ "b -> b s",
556
+ s=image_tokens.shape[1],
557
+ )
558
+ )
559
+ # Image tokens attend to each other
560
+ ar_mask += [False] * image_tokens.shape[1]
561
+
562
+ # Add task embeddings with subtask state fusion
563
+ if obs.tokenized_prompt is not None:
564
+ # obs.tokenized_prompt now contains task_ids (shape: [batch_size, 2])
565
+ task_ids = obs.tokenized_prompt[:, 0] # Extract task_id: [batch_size]
566
+ base_task_embedding = self.task_embeddings(task_ids) # shape: [batch_size, embed_dim]
567
+
568
+ # ALWAYS use the input subtask state - never use predicted state inside model
569
+ if obs.tokenized_prompt.shape[1] > 1: # If we have [task_id, subtask_state]
570
+ subtask_state = obs.tokenized_prompt[:, 1] # Use input subtask state
571
+ else:
572
+ raise ValueError("subtask_state must be provided in tokenized_prompt for PI_BEHAVIOR model")
573
+
574
+ # Fuse task embedding with subtask state - returns [b, 4, d] with multiple representations
575
+ fused_task_embeddings = self.fuse_task_and_subtask(base_task_embedding, task_ids, subtask_state)
576
+
577
+ # Create task token sequence: [base_task, task_gated, balanced_fusion, stage_dominant, pure_stage]
578
+ task_sequence = jnp.concatenate([
579
+ base_task_embedding[:, None, :], # [b, 1, d] - base task token
580
+ fused_task_embeddings # [b, 4, d] - stage-conditioned tokens
581
+ ], axis=1) # [b, 5, d]
582
+
583
+ tokens.append(task_sequence)
584
+ # All task tokens are valid
585
+ task_mask = jnp.ones((obs.tokenized_prompt.shape[0], 5), dtype=jnp.bool_)
586
+ input_mask.append(task_mask)
587
+ # Hierarchical attention: base task (False) then stage tokens (True, False, False, False)
588
+ # Base task attends to images bidirectionally
589
+ # Stage tokens attend to images+task but not vice versa
590
+ ar_mask += [False] + [True, False, False, False]
591
+
592
+ # Add state as discrete tokens (Pi05 style)
593
+ # Discretize state into bins
594
+ discretized_state = jnp.digitize(obs.state, bins=jnp.linspace(-1, 1, 256 + 1)[:-1]) - 1
595
+ discretized_state = jnp.clip(discretized_state, 0, 255) # Ensure valid range
596
+
597
+ # Embed each dimension of the discretized state
598
+ state_tokens = []
599
+ for i in range(obs.state.shape[-1]):
600
+ state_dim_tokens = self.PaliGemma.llm(discretized_state[:, i:i+1], method="embed")
601
+ state_tokens.append(state_dim_tokens)
602
+
603
+ if state_tokens:
604
+ state_tokens = jnp.concatenate(state_tokens, axis=1) # shape: [batch_size, state_dim, embed_dim]
605
+ tokens.append(state_tokens)
606
+ input_mask.append(jnp.ones((obs.state.shape[0], obs.state.shape[-1]), dtype=jnp.bool_))
607
+ # State tokens have full bidirectional attention with all prefix tokens
608
+ # (images, task, stages, and other state tokens)
609
+ ar_mask += [False] * state_tokens.shape[1]
610
+
611
+ # FAST tokens (from observation if provided)
612
+ if self.config.use_fast_auxiliary and obs.fast_tokens is not None:
613
+ fast_tokens = obs.fast_tokens # [B, T]
614
+ fast_token_mask = obs.fast_token_mask # [B, T]
615
+
616
+ # Teacher forcing: shift right [BOS, tok0, tok1, ..., tok_{T-1}]
617
+ bos_token = jnp.zeros((fast_tokens.shape[0], 1), dtype=jnp.int32)
618
+ shifted_tokens = jnp.concatenate([bos_token, fast_tokens[:, :-1]], axis=1)
619
+
620
+ # Shift mask too: [True, mask_0, mask_1, ..., mask_{T-1}]
621
+ bos_mask = jnp.ones((fast_tokens.shape[0], 1), dtype=jnp.bool_)
622
+ shifted_mask = jnp.concatenate([bos_mask, fast_token_mask[:, :-1]], axis=1)
623
+
624
+ # Embed using FAST embedding layer (NOT Paligemma!)
625
+ fast_token_emb = self.fast_token_embedding(shifted_tokens) # [B, T, D]
626
+
627
+ tokens.append(fast_token_emb)
628
+ input_mask.append(shifted_mask) # Use the actual token mask
629
+ # Causal for FAST: ALL tokens are causal (pure autoregressive)
630
+ ar_mask += [True] * shifted_tokens.shape[1]
631
+
632
+ tokens = jnp.concatenate(tokens, axis=1)
633
+ input_mask = jnp.concatenate(input_mask, axis=1)
634
+ ar_mask = jnp.array(ar_mask)
635
+ return tokens, input_mask, ar_mask
636
+
637
+ @at.typecheck
638
+ def embed_suffix(
639
+ self, obs: Observation, noisy_actions: _model.Actions, timestep: at.Float[at.Array, " b"]
640
+ ) -> tuple[
641
+ at.Float[at.Array, "b s emb"],
642
+ at.Bool[at.Array, "b s"],
643
+ at.Bool[at.Array, " s"],
644
+ at.Float[at.Array, "b emb"],
645
+ ]:
646
+ input_mask = []
647
+ ar_mask = []
648
+ tokens = []
649
+
650
+ # Pi05 style: no explicit state token in suffix (it's in prefix as discrete tokens)
651
+
652
+ action_tokens = self.action_in_proj(noisy_actions)
653
+ # Embed timestep using sine-cosine positional encoding
654
+ time_emb = posemb_sincos(timestep, self.action_in_proj.out_features, min_period=4e-3, max_period=4.0)
655
+
656
+ # Pi05 style: time MLP for adaRMS
657
+ time_emb = self.time_mlp_in(time_emb)
658
+ time_emb = nnx.swish(time_emb)
659
+ time_emb = self.time_mlp_out(time_emb)
660
+ time_emb = nnx.swish(time_emb)
661
+ action_expert_tokens = action_tokens
662
+ adarms_cond = time_emb
663
+
664
+ tokens.append(action_expert_tokens)
665
+ input_mask.append(jnp.ones(action_expert_tokens.shape[:2], dtype=jnp.bool_))
666
+
667
+ # image/task/state inputs do not attend to action tokens
668
+ ar_mask += [True] + ([False] * (self.action_horizon - 1))
669
+
670
+ tokens = jnp.concatenate(tokens, axis=1)
671
+ input_mask = jnp.concatenate(input_mask, axis=1)
672
+ ar_mask = jnp.array(ar_mask)
673
+ return tokens, input_mask, ar_mask, adarms_cond
674
+
675
+ @override
676
+ def compute_loss(
677
+ self, rng: at.KeyArrayLike, observation: Observation, actions: _model.Actions, *, train: bool = False
678
+ ) -> at.Float[at.Array, "*b ah"]:
679
+ """Not used - we only use compute_detailed_loss() for training."""
680
+ raise NotImplementedError("Use compute_detailed_loss() instead")
681
+
682
+ @override
683
+ def compute_detailed_loss(
684
+ self, rng: at.KeyArrayLike, observation: Observation, actions: _model.Actions, *, train: bool = False, num_flow_samples: int = 1
685
+ ) -> dict[str, at.Float[at.Array, "*b"]]:
686
+ """
687
+ Compute detailed loss with multiple flow matching samples.
688
+
689
+ Simplified approach using KV cache:
690
+ - Compute prefix KV cache once (with FAST tokens)
691
+ - Remove FAST tokens from cache (action expert doesn't attend to FAST)
692
+ - Process N flow samples independently, each reusing the same cached prefix
693
+ - Each sample has different noise and different time
694
+ - Average losses across samples
695
+ """
696
+ losses = {}
697
+
698
+ preprocess_rng, rng = jax.random.split(rng)
699
+ observation = preprocess_observation(preprocess_rng, observation, train=train)
700
+
701
+ batch_size = actions.shape[0]
702
+
703
+ # 1. Embed prefix once (includes FAST tokens if provided in observation)
704
+ prefix_tokens, prefix_mask, prefix_ar_mask = self.embed_prefix(observation)
705
+
706
+ # 2. Compute prefix KV cache
707
+ prefix_attn_mask = make_attn_mask(prefix_mask, prefix_ar_mask)
708
+ positions_prefix = jnp.cumsum(prefix_mask, axis=1) - 1
709
+ (prefix_out, _), kv_cache_full = self.PaliGemma.llm(
710
+ [prefix_tokens, None],
711
+ mask=prefix_attn_mask,
712
+ positions=positions_prefix
713
+ )
714
+
715
+ # 3. Predict stage from VLM output of base task token
716
+ # Base task token is the first token after all image tokens
717
+ # Image tokens all have ar_mask=False, task starts with ar_mask=False (base) then True (stage tokens)
718
+ # Structure: [images (all False)] [base_task (False)] [stages (True, False, False, False)]
719
+ # Find first True (first stage token), base task is at that index - 1
720
+ first_stage_token_idx = jnp.argmax(prefix_ar_mask) # Returns index of first True
721
+ base_task_token_idx = first_stage_token_idx - 1
722
+ base_task_output = prefix_out[:, base_task_token_idx, :]
723
+ subtask_logits = self.stage_pred_from_vlm(base_task_output) # [B, MAX_NUM_STAGES]
724
+
725
+ # Mask out invalid stages for each task (vectorized JAX operations)
726
+ task_ids = observation.tokenized_prompt[:, 0] # [B]
727
+ task_num_stages_array = jnp.array(TASK_NUM_STAGES, dtype=jnp.int32)
728
+ task_num_stages = task_num_stages_array[task_ids] # [B] - JAX array indexing
729
+ stage_range = jnp.arange(MAX_NUM_STAGES) # [15]
730
+ valid_mask = stage_range[None, :] < task_num_stages[:, None] # [B, 15]
731
+ subtask_logits = jnp.where(valid_mask, subtask_logits, -jnp.inf) # Mask invalid stages
732
+
733
+ # 4. Extract FAST loss from prefix output (before removing from cache)
734
+ fast_loss_value = 0.0
735
+ fast_len = 0
736
+ fast_targets = observation.fast_tokens
737
+ fast_token_mask = observation.fast_token_mask
738
+
739
+ if self.config.use_fast_auxiliary and fast_targets is not None:
740
+ fast_len = fast_targets.shape[1]
741
+ fast_start_idx = prefix_tokens.shape[1] - fast_len
742
+ fast_outputs = prefix_out[:, fast_start_idx:, :] # [B, T, D]
743
+
744
+ # Project to FAST vocab
745
+ fast_logits = self.fast_token_proj(fast_outputs) # [B, T, vocab_size]
746
+
747
+ # Cross-entropy loss with teacher forcing
748
+ pred_logits = fast_logits # [B, T, vocab]
749
+ target_tokens = fast_targets # [B, T]
750
+ loss_mask = fast_token_mask # [B, T]
751
+
752
+ log_probs = jax.nn.log_softmax(pred_logits, axis=-1)
753
+ target_log_probs = jnp.take_along_axis(
754
+ log_probs,
755
+ target_tokens[:, :, None],
756
+ axis=-1
757
+ ).squeeze(-1) # [B, T]
758
+
759
+ fast_token_loss = -target_log_probs # [B, T]
760
+
761
+ # Apply mask and normalize by number of valid tokens
762
+ masked_loss = fast_token_loss * loss_mask # [B, T]
763
+ num_valid_tokens = jnp.maximum(jnp.sum(loss_mask, axis=-1), 1) # [B]
764
+ losses["fast_loss"] = jnp.sum(masked_loss, axis=-1) / num_valid_tokens # [B]
765
+
766
+ # Accuracy (only on valid tokens)
767
+ pred_tokens = jnp.argmax(pred_logits, axis=-1)
768
+ correct = (pred_tokens == target_tokens) * loss_mask
769
+ losses["fast_accuracy"] = jnp.sum(correct, axis=-1) / num_valid_tokens
770
+
771
+ fast_loss_value = self.config.fast_loss_weight * jnp.mean(losses["fast_loss"])
772
+ elif fast_targets is not None:
773
+ # FAST auxiliary is disabled but data contains FAST tokens
774
+ raise ValueError(
775
+ "use_fast_auxiliary=False but observation contains fast_tokens. "
776
+ "Either enable use_fast_auxiliary in config or ensure data doesn't contain fast_tokens."
777
+ )
778
+
779
+ # 5. Remove FAST tokens from KV cache (action expert doesn't attend to FAST)
780
+ # KV cache shape: [layers, batch, seq_len, num_kv_heads, head_dim]
781
+ if fast_len > 0:
782
+ cache_k, cache_v = kv_cache_full
783
+ # Remove last fast_len tokens from sequence dimension
784
+ cache_k = cache_k[:, :, :-fast_len, :, :]
785
+ cache_v = cache_v[:, :, :-fast_len, :, :]
786
+ kv_cache_for_actions = (cache_k, cache_v)
787
+ prefix_len_for_actions = prefix_tokens.shape[1] - fast_len
788
+ # Truncate prefix mask and ar_mask for action expert
789
+ prefix_mask_for_actions = prefix_mask[:, :-fast_len]
790
+ prefix_ar_mask_for_actions = prefix_ar_mask[:-fast_len]
791
+ else:
792
+ kv_cache_for_actions = kv_cache_full
793
+ prefix_len_for_actions = prefix_tokens.shape[1]
794
+ prefix_mask_for_actions = prefix_mask
795
+ prefix_ar_mask_for_actions = prefix_ar_mask
796
+
797
+ # 6. Knowledge insulation: stop gradients from action expert to VLM
798
+ # This must happen BEFORE kv_transform so transform still receives gradients
799
+ if self.config.use_knowledge_insulation:
800
+ kv_cache_for_actions = jax.tree.map(jax.lax.stop_gradient, kv_cache_for_actions)
801
+
802
+ # 7. Transform KV cache (after stop_gradient, so it receives action expert gradients)
803
+ if self.kv_transform is not None:
804
+ kv_cache_for_actions = self.kv_transform(kv_cache_for_actions)
805
+
806
+ # 8. Define single flow sample processing
807
+ def process_one_flow_sample(sample_rng):
808
+ """Process one flow sample using the original cached prefix."""
809
+ noise_rng, time_rng = jax.random.split(sample_rng)
810
+
811
+ # Generate different noise and time for this sample
812
+ noise = self.generate_correlated_noise(noise_rng, batch_size)
813
+ time = jax.random.beta(time_rng, 1.5, 1, (batch_size,)) * 0.999 + 0.001
814
+
815
+ # Compute noisy actions and target velocity
816
+ time_expanded = time[:, None, None]
817
+ x_t = time_expanded * noise + (1 - time_expanded) * actions
818
+ u_t = noise - actions
819
+
820
+ # Embed suffix for this sample
821
+ suffix_tokens, suffix_mask, suffix_ar_mask, adarms_cond = self.embed_suffix(
822
+ observation, x_t, time
823
+ )
824
+
825
+ # Build attention mask: suffix attends to prefix (without FAST) + itself
826
+ # When using KV cache, mask shape should be [batch, suffix_len, prefix_len + suffix_len]
827
+ suffix_attn_mask = make_attn_mask(suffix_mask, suffix_ar_mask)
828
+ prefix_attn_mask = einops.repeat(
829
+ prefix_mask_for_actions, "b p -> b s p", s=suffix_tokens.shape[1]
830
+ )
831
+ full_attn_mask = jnp.concatenate([prefix_attn_mask, suffix_attn_mask], axis=-1)
832
+
833
+ # Positions for suffix start after cached prefix
834
+ suffix_positions = prefix_len_for_actions + jnp.cumsum(suffix_mask, axis=-1) - 1
835
+
836
+ # Forward pass with cached prefix (discard returned cache - don't modify original!)
837
+ (_, suffix_out), _ = self.PaliGemma.llm(
838
+ [None, suffix_tokens],
839
+ mask=full_attn_mask,
840
+ positions=suffix_positions,
841
+ kv_cache=kv_cache_for_actions, # Original cache, reused for all samples
842
+ adarms_cond=[None, adarms_cond]
843
+ )
844
+
845
+ # Compute velocity and loss
846
+ v_t = self.action_out_proj(suffix_out[:, -self.action_horizon:])
847
+ action_loss = jnp.square(v_t - u_t) # [B, H, D]
848
+
849
+ return action_loss
850
+
851
+ # 9. Vectorize over N flow samples
852
+ # Disable type checking inside vmap (jaxtyping doesn't handle traced values well)
853
+ flow_rngs = jax.random.split(rng, num_flow_samples)
854
+ with at.disable_typechecking():
855
+ all_action_losses = jax.vmap(process_one_flow_sample)(flow_rngs) # [N, B, H, D]
856
+
857
+ # 10. Average over flow samples
858
+ action_loss = jnp.mean(all_action_losses, axis=0) # [B, H, D]
859
+
860
+ # 11. Build per-dimension action losses
861
+ # Base velocity (x,y,z)
862
+ losses["action_loss_base_vel_x"] = jnp.mean(action_loss[..., 0], axis=-1)
863
+ losses["action_loss_base_vel_y"] = jnp.mean(action_loss[..., 1], axis=-1)
864
+ losses["action_loss_base_vel_z"] = jnp.mean(action_loss[..., 2], axis=-1)
865
+
866
+ # Trunk joints (4)
867
+ for i in range(4):
868
+ losses[f"action_loss_trunk_{i}"] = jnp.mean(action_loss[..., 3+i], axis=-1)
869
+
870
+ # Left arm joints (7)
871
+ for i in range(7):
872
+ losses[f"action_loss_left_arm_{i}"] = jnp.mean(action_loss[..., 7+i], axis=-1)
873
+
874
+ # Left gripper
875
+ losses["action_loss_left_gripper"] = jnp.mean(action_loss[..., 14], axis=-1)
876
+
877
+ # Right arm joints (7)
878
+ for i in range(7):
879
+ losses[f"action_loss_right_arm_{i}"] = jnp.mean(action_loss[..., 15+i], axis=-1)
880
+
881
+ # Right gripper
882
+ losses["action_loss_right_gripper"] = jnp.mean(action_loss[..., 22], axis=-1)
883
+
884
+ # Total action loss: mean over horizon (H) and action dims (D) -> [B]
885
+ losses["action_loss"] = jnp.mean(action_loss, axis=(-2, -1))
886
+
887
+ # 12. Add subtask loss during training
888
+ subtask_loss_value = 0.0
889
+ if train and observation.tokenized_prompt.shape[1] > 1:
890
+ ground_truth_subtask = observation.tokenized_prompt[:, 1]
891
+ subtask_loss = -jax.nn.log_softmax(subtask_logits)[
892
+ jnp.arange(ground_truth_subtask.shape[0]), ground_truth_subtask
893
+ ]
894
+ losses["subtask_loss"] = jnp.mean(subtask_loss)
895
+ losses["subtask_accuracy"] = jnp.mean(
896
+ jnp.argmax(subtask_logits, axis=-1) == ground_truth_subtask
897
+ )
898
+ subtask_loss_value = self.config.subtask_loss_weight * jnp.mean(subtask_loss)
899
+
900
+ # 13. Total loss
901
+ losses["total_loss"] = losses["action_loss"] + subtask_loss_value + fast_loss_value
902
+
903
+ return losses
904
+
905
+ @override
906
+ def sample_actions(
907
+ self,
908
+ rng: at.KeyArrayLike,
909
+ observation: Observation,
910
+ *,
911
+ num_steps: int | at.Int[at.Array, ""] = 20,
912
+ noise: at.Float[at.Array, "b ah ad"] | None = None,
913
+ initial_actions: at.Float[at.Array, "b n ad"] | None = None,
914
+ ) -> _model.Actions:
915
+ observation = preprocess_observation(None, observation, train=False)
916
+ # Note that we use the convention more common in diffusion literature, where t=1 is noise and t=0 is the target
917
+ # distribution. yes, this is the opposite of the pi0 paper, and I'm sorry.
918
+ dt = -1.0 / num_steps
919
+ batch_size = observation.state.shape[0]
920
+
921
+ # Generate or constrain noise based on inpainting requirements
922
+ if initial_actions is not None:
923
+ # INPAINTING PATH: Construct constrained noise z that satisfies initial_actions
924
+ num_initial_actions = initial_actions.shape[1]
925
+ input_action_dim = initial_actions.shape[2]
926
+
927
+ # Pad initial_actions to full model dimensions (32D) and action_horizon (30)
928
+ if input_action_dim < self.action_dim:
929
+ action_padding = jnp.zeros((batch_size, num_initial_actions, self.action_dim - input_action_dim))
930
+ initial_actions_full_dim = jnp.concatenate([initial_actions, action_padding], axis=2)
931
+ else:
932
+ initial_actions_full_dim = initial_actions[:, :, :self.action_dim]
933
+
934
+ if num_initial_actions < self.action_horizon:
935
+ seq_padding = jnp.zeros((batch_size, self.action_horizon - num_initial_actions, self.action_dim))
936
+ initial_actions_padded = jnp.concatenate([initial_actions_full_dim, seq_padding], axis=1)
937
+ else:
938
+ initial_actions_padded = initial_actions_full_dim[:, :self.action_horizon]
939
+
940
+ # Compute O and U indices for inpainting (JIT-safe: static list comprehensions)
941
+ flat_dim = self.action_horizon * self.action_dim
942
+
943
+ # Build O_indices: first num_initial_actions timesteps, first input_action_dim dimensions
944
+ O_indices = jnp.array([
945
+ t * self.action_dim + d
946
+ for t in range(num_initial_actions)
947
+ for d in range(input_action_dim)
948
+ ], dtype=jnp.int32)
949
+
950
+ # Build U_indices: all other indices (JIT-safe: static list comprehension)
951
+ # Python set operations happen at trace time (before JIT), so this is safe
952
+ O_set = {t * self.action_dim + d for t in range(num_initial_actions) for d in range(input_action_dim)}
953
+ U_indices = jnp.array([
954
+ i for i in range(flat_dim) if i not in O_set
955
+ ], dtype=jnp.int32)
956
+
957
+ # Generate noise
958
+ rng, noise_rng = jax.random.split(rng)
959
+
960
+ if self.correlation_loaded:
961
+ # CORRELATED NOISE: Sample with correlation matrix
962
+ noise = self.generate_correlated_noise(noise_rng, batch_size)
963
+ else:
964
+ # FALLBACK: Independent noise
965
+ noise = jax.random.normal(noise_rng, (batch_size, self.action_horizon, self.action_dim))
966
+
967
+ # Extract fixed z_O and x0_O for constraint enforcement
968
+ noise_flat = noise.reshape(batch_size, flat_dim)
969
+ fixed_z_O = noise_flat[:, O_indices] # [b, |O|] - fixed noise for inpainting
970
+ x0_O = initial_actions_padded.reshape(batch_size, flat_dim)[:, O_indices] # [b, |O|] - target actions
971
+
972
+ # Precompute correction matrix for correlation-aware inpainting
973
+ inpainting_cache = None
974
+ if self.correlation_loaded:
975
+ cache_key = (num_initial_actions, input_action_dim)
976
+ if cache_key not in self.inpainting_cache:
977
+ logger.info(f"Computing correction matrix for {num_initial_actions} steps, {input_action_dim} dims...")
978
+ self.inpainting_cache[cache_key] = self._precompute_correction_matrix(O_indices, U_indices)
979
+ inpainting_cache = self.inpainting_cache[cache_key]
980
+
981
+ else:
982
+ # NO INPAINTING: Standard noise generation
983
+ if noise is None:
984
+ rng, noise_rng = jax.random.split(rng)
985
+ noise = self.generate_correlated_noise(noise_rng, batch_size)
986
+
987
+ fixed_z_O = None
988
+ x0_O = None
989
+ O_indices = None
990
+ inpainting_cache = None
991
+
992
+ # Split RNG for step loop
993
+ rng, step_rng = jax.random.split(rng)
994
+
995
+ # Ensure FAST tokens are never used during inference
996
+ if observation.fast_tokens is not None:
997
+ raise ValueError(
998
+ "FAST tokens must not be provided during inference (sample_actions). "
999
+ "FAST tokens are only used during training for auxiliary loss. "
1000
+ "Set observation.fast_tokens=None before calling sample_actions."
1001
+ )
1002
+
1003
+ # First fill KV cache with a forward pass of the prefix (no FAST tokens during inference)
1004
+ prefix_tokens, prefix_mask, prefix_ar_mask = self.embed_prefix(observation)
1005
+ prefix_attn_mask = make_attn_mask(prefix_mask, prefix_ar_mask)
1006
+ positions = jnp.cumsum(prefix_mask, axis=1) - 1
1007
+ (prefix_out, _), kv_cache = self.PaliGemma.llm([prefix_tokens, None], mask=prefix_attn_mask, positions=positions)
1008
+
1009
+ # Predict stage from VLM output of base task token
1010
+ # Find base task token position (same logic as in compute_detailed_loss)
1011
+ first_stage_token_idx = jnp.argmax(prefix_ar_mask) # Returns index of first True
1012
+ base_task_token_idx = first_stage_token_idx - 1
1013
+ base_task_output = prefix_out[:, base_task_token_idx, :]
1014
+ subtask_logits = self.stage_pred_from_vlm(base_task_output) # [B, MAX_NUM_STAGES]
1015
+
1016
+ # Mask out invalid stages for each task (vectorized JAX operations)
1017
+ task_ids = observation.tokenized_prompt[:, 0] # [B]
1018
+ task_num_stages_array = jnp.array(TASK_NUM_STAGES, dtype=jnp.int32)
1019
+ task_num_stages = task_num_stages_array[task_ids] # [B] - JAX array indexing
1020
+ stage_range = jnp.arange(MAX_NUM_STAGES) # [15]
1021
+ valid_mask = stage_range[None, :] < task_num_stages[:, None] # [B, 15]
1022
+ subtask_logits = jnp.where(valid_mask, subtask_logits, -jnp.inf)
1023
+
1024
+ # Transform KV cache for cross-layer attention
1025
+ if self.kv_transform is not None:
1026
+ kv_cache = self.kv_transform(kv_cache)
1027
+
1028
+ def step(carry):
1029
+ x_t, time, step_rng = carry
1030
+
1031
+ # Use config value for time threshold
1032
+ TIME_THRESHOLD_INPAINT = self.config.time_threshold_inpaint
1033
+
1034
+ # Model forward pass
1035
+ suffix_tokens, suffix_mask, suffix_ar_mask, adarms_cond = self.embed_suffix(
1036
+ observation, x_t, jnp.broadcast_to(time, batch_size)
1037
+ )
1038
+ suffix_attn_mask = make_attn_mask(suffix_mask, suffix_ar_mask)
1039
+ prefix_attn_mask = einops.repeat(prefix_mask, "b p -> b s p", s=suffix_tokens.shape[1])
1040
+ full_attn_mask = jnp.concatenate([prefix_attn_mask, suffix_attn_mask], axis=-1)
1041
+ assert full_attn_mask.shape == (
1042
+ batch_size,
1043
+ suffix_tokens.shape[1],
1044
+ prefix_tokens.shape[1] + suffix_tokens.shape[1],
1045
+ )
1046
+ positions = jnp.sum(prefix_mask, axis=-1)[:, None] + jnp.cumsum(suffix_mask, axis=-1) - 1
1047
+
1048
+ (prefix_out, suffix_out), _ = self.PaliGemma.llm(
1049
+ [None, suffix_tokens],
1050
+ mask=full_attn_mask,
1051
+ positions=positions,
1052
+ kv_cache=kv_cache,
1053
+ adarms_cond=[None, adarms_cond],
1054
+ )
1055
+ assert prefix_out is None
1056
+ v_t = self.action_out_proj(suffix_out[:, -self.action_horizon :])
1057
+
1058
+ # Euler step: x_{t+dt} = x_t + dt * v_t
1059
+ x_t_new = x_t + dt * v_t
1060
+
1061
+ # Apply correlation-aware inpainting correction
1062
+ # Only enforce when time > TIME_THRESHOLD_INPAINT (let model be free in final steps)
1063
+ if fixed_z_O is not None:
1064
+ time_new = time + dt
1065
+
1066
+ def apply_correlated_correction(x):
1067
+ x_flat = x.reshape(batch_size, -1)
1068
+
1069
+ # Compute desired state at O: x_t[O] = (1-t)*x0[O] + t*z_O
1070
+ x_desired_O = (1.0 - time_new) * x0_O + time_new * fixed_z_O # [b, |O|]
1071
+
1072
+ # Compute correction at O
1073
+ delta_O = x_desired_O - x_flat[:, O_indices] # [b, |O|]
1074
+
1075
+ # Apply hard constraint at O
1076
+ x_flat = x_flat.at[:, O_indices].set(x_desired_O)
1077
+
1078
+ # If correlation matrix available, propagate correction to U
1079
+ if inpainting_cache is not None:
1080
+ correction_matrix = inpainting_cache['correction_matrix'] # [|U|, |O|]
1081
+ U_indices_cached = inpainting_cache['U_indices']
1082
+
1083
+ # Compute correlated correction: δ_U = Σ_{UO}Σ_{OO}^{-1} @ δ_O
1084
+ delta_U = delta_O @ correction_matrix.T # [b, |U|]
1085
+
1086
+ # Skip if correction too large (indicates instability)
1087
+ max_correction = jnp.max(jnp.abs(delta_U))
1088
+ x_flat = jax.lax.cond(
1089
+ # Prevents exploding corrections in case of noisy out of distribution initial actions
1090
+ max_correction <= 1.0,
1091
+ lambda x: x.at[:, U_indices_cached].add(delta_U),
1092
+ lambda x: x,
1093
+ x_flat
1094
+ )
1095
+
1096
+ # Sanity check: if Σ = I, correction_matrix = 0, so delta_U = 0 that is correct
1097
+ # If the correlation is 1 everywhere we will go to the flat prediction that is correct
1098
+
1099
+ return x_flat.reshape(batch_size, self.action_horizon, self.action_dim)
1100
+
1101
+ # Only apply correction when NEW time > threshold
1102
+ x_t_new = jax.lax.cond(
1103
+ time_new > TIME_THRESHOLD_INPAINT,
1104
+ apply_correlated_correction,
1105
+ lambda x: x,
1106
+ x_t_new
1107
+ )
1108
+
1109
+ return x_t_new, time + dt, step_rng
1110
+
1111
+ def cond(carry):
1112
+ x_t, time, step_rng = carry
1113
+ # Robust to floating-point error
1114
+ return time >= -dt / 2
1115
+
1116
+ x_0, _, _ = jax.lax.while_loop(cond, step, (noise, 1.0, step_rng))
1117
+
1118
+ return x_0, subtask_logits
task 57/policy_code/src/b1k/models/pi_behavior_config.py ADDED
@@ -0,0 +1,172 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """PI_BEHAVIOR Model Configuration
2
+
3
+ Configuration for PI_BEHAVIOR model on BEHAVIOR-1K challenge.
4
+ """
5
+
6
+ import dataclasses
7
+ import json
8
+ import pathlib
9
+ from typing import TYPE_CHECKING
10
+
11
+ import flax.nnx as nnx
12
+ import jax
13
+ import jax.numpy as jnp
14
+ from typing_extensions import override
15
+
16
+ from openpi.models import model as _model
17
+ from openpi.models import gemma as _gemma
18
+ from openpi.shared import array_typing as at
19
+ import openpi.shared.nnx_utils as nnx_utils
20
+
21
+ from b1k.models.observation import Observation
22
+
23
+ if TYPE_CHECKING:
24
+ from b1k.models.pi_behavior import PiBehavior
25
+
26
+
27
+ # Per-task stage counts (based on avg_episode_length / 900, capped between 5-15)
28
+ # Use tuple for immutability and to avoid JAX device allocation at import time
29
+ TASK_NUM_STAGES = (
30
+ 5, 6, 15, 15, 14, 12, 9, 15, 10, 15, # Tasks 0-9
31
+ 7, 13, 10, 15, 15, 15, 15, 11, 13, 12, # Tasks 10-19
32
+ 14, 15, 9, 15, 15, 15, 15, 15, 15, 15, # Tasks 20-29
33
+ 11, 10, 10, 13, 5, 5, 14, 6, 8, 10, # Tasks 30-39
34
+ 5, 15, 8, 15, 12, 11, 9, 14, 15, 15, # Tasks 40-49
35
+ )
36
+
37
+ MAX_NUM_STAGES = 15 # Maximum stages per task
38
+ TOTAL_TASK_STAGE_EMBEDDINGS = sum(TASK_NUM_STAGES) # 596 total embeddings
39
+
40
+ # Cumulative offsets for indexing into task_stage_embeddings (as tuple)
41
+ TASK_STAGE_OFFSETS = tuple([0] + [sum(TASK_NUM_STAGES[:i+1]) for i in range(len(TASK_NUM_STAGES) - 1)])
42
+
43
+
44
+ @dataclasses.dataclass(frozen=True)
45
+ class PiBehaviorConfig(_model.BaseModelConfig):
46
+ dtype: str = "bfloat16"
47
+ paligemma_variant: _gemma.Variant = "gemma_2b"
48
+ action_expert_variant: _gemma.Variant = "gemma_300m"
49
+
50
+ # Set the model specific defaults.
51
+ action_dim: int = 32
52
+ action_horizon: int = 30
53
+ max_token_len: int = 200 # Only used for compatibility, not for actual tokenization
54
+
55
+ # Number of tasks in the behavior dataset
56
+ num_tasks: int = 50
57
+ # Task embedding dimension - will match the paligemma width
58
+ task_embedding_dim: int = None # type: ignore
59
+ # Maximum number of subtask states across all tasks
60
+ max_num_subtask_states: int = MAX_NUM_STAGES
61
+
62
+ # Path to task data JSON file for initialization
63
+ task_data_path: str = "b1k/BEHAVIOR-1K/docs/challenge/task_data.json"
64
+
65
+ # Whether to use correlated noise matching action covariance structure
66
+ # Requires correlation matrix in norm_stats (computed by compute_norm_stats.py)
67
+ use_correlated_noise: bool = True
68
+
69
+ # Shrinkage parameter for correlation regularization
70
+ # Applied as: S_regularized = beta * S + (1-beta) * I
71
+ # beta=1.0 means full correlation (no shrinkage)
72
+ # beta=0.7 means 70% correlation + 30% independence (recommended for robustness)
73
+ # beta=0.0 means independence (no correlation)
74
+ correlation_beta: float = 0.5
75
+
76
+ # FAST auxiliary training configuration
77
+ use_fast_auxiliary: bool = False # Enable FAST during training
78
+ fast_loss_weight: float = 0.1 # Weight for FAST loss (vs flow loss)
79
+
80
+ # Action dimensions to encode with FAST (default: 0:6, 7:23 = 22 dims)
81
+ # Format: "0:6,7:23" or list of tuples [(0, 6), (7, 23)]
82
+ fast_encoded_dims: str | list[tuple[int, int]] = "0:6,7:23"
83
+
84
+ # FAST tokenizer vocab size
85
+ fast_vocab_size: int = 1024
86
+
87
+ # Max FAST tokens to predict (truncate if exceeded)
88
+ max_fast_tokens: int = 32
89
+
90
+ # FAST tokenizer path (set during initialization, relative to assets_dir/asset_id)
91
+ fast_tokenizer_path: str | None = None
92
+
93
+ # KV cache transformation for cross-layer attention between VLM and action expert
94
+ # Allows each action expert layer to attend to a learned combination of all VLM layers
95
+ use_kv_transform: bool = True
96
+
97
+ # Knowledge insulation: stop action expert gradients from flowing to VLM backbone
98
+ # VLM trains on FAST tokens only, action expert on flow matching with frozen VLM features
99
+ # Implements approach from https://www.physicalintelligence.company/research/knowledge_insulation
100
+ use_knowledge_insulation: bool = True
101
+
102
+ # Subtask/stage prediction auxiliary loss weight (relative to action loss)
103
+ # Higher values emphasize stage prediction accuracy at the expense of action quality
104
+ subtask_loss_weight: float = 0.1
105
+
106
+ # Time threshold for inpainting during inference
107
+ # Stop enforcing inpainting constraint when t < threshold (let model be free in final steps)
108
+ time_threshold_inpaint: float = 0.3
109
+
110
+ # Vision backbone finetuning control
111
+ freeze_vision_backbone: bool = True
112
+
113
+ def __post_init__(self):
114
+ if self.task_embedding_dim is None:
115
+ paligemma_config = _gemma.get_config(self.paligemma_variant)
116
+ object.__setattr__(self, "task_embedding_dim", paligemma_config.width)
117
+
118
+ def get_fast_dim_ranges(self) -> list[tuple[int, int]]:
119
+ """Parse fast_encoded_dims into list of ranges."""
120
+ if isinstance(self.fast_encoded_dims, str):
121
+ ranges = []
122
+ for range_str in self.fast_encoded_dims.split(','):
123
+ start, end = map(int, range_str.strip().split(':'))
124
+ ranges.append((start, end))
125
+ return ranges
126
+ return self.fast_encoded_dims
127
+
128
+ def get_total_fast_dims(self) -> int:
129
+ """Get total number of dimensions encoded by FAST."""
130
+ return sum(end - start for start, end in self.get_fast_dim_ranges())
131
+
132
+ @property
133
+ @override
134
+ def model_type(self):
135
+ return "pi_behavior"
136
+
137
+ @override
138
+ def create(self, rng: at.KeyArrayLike) -> "PiBehavior":
139
+ from b1k.models.pi_behavior import PiBehavior
140
+
141
+ return PiBehavior(self, rngs=nnx.Rngs(rng))
142
+
143
+ @override
144
+ def inputs_spec(self, *, batch_size: int = 1) -> tuple["Observation", _model.Actions]:
145
+ image_spec = jax.ShapeDtypeStruct([batch_size, *_model.IMAGE_RESOLUTION, 3], jnp.float32)
146
+ image_mask_spec = jax.ShapeDtypeStruct([batch_size], jnp.bool_)
147
+
148
+ with at.disable_typechecking():
149
+ obs_kwargs = {
150
+ "images": {
151
+ "base_0_rgb": image_spec,
152
+ "left_wrist_0_rgb": image_spec,
153
+ "right_wrist_0_rgb": image_spec,
154
+ },
155
+ "image_masks": {
156
+ "base_0_rgb": image_mask_spec,
157
+ "left_wrist_0_rgb": image_mask_spec,
158
+ "right_wrist_0_rgb": image_mask_spec,
159
+ },
160
+ "state": jax.ShapeDtypeStruct([batch_size, self.action_dim], jnp.float32),
161
+ "tokenized_prompt": jax.ShapeDtypeStruct([batch_size, 2], jnp.int32),
162
+ "tokenized_prompt_mask": jax.ShapeDtypeStruct([batch_size, 2], bool),
163
+ }
164
+
165
+ if self.use_fast_auxiliary:
166
+ obs_kwargs["fast_tokens"] = jax.ShapeDtypeStruct([batch_size, self.max_fast_tokens], jnp.int32)
167
+ obs_kwargs["fast_token_mask"] = jax.ShapeDtypeStruct([batch_size, self.max_fast_tokens], bool)
168
+
169
+ observation_spec = Observation(**obs_kwargs)
170
+
171
+ action_spec = jax.ShapeDtypeStruct([batch_size, self.action_horizon, self.action_dim], jnp.float32)
172
+ return observation_spec, action_spec
task 57/policy_code/src/b1k/policies/__init__.py ADDED
@@ -0,0 +1,4 @@
 
 
 
 
 
1
+ from b1k.policies.b1k_policy import B1kInputs, B1kOutputs, extract_state_from_proprio
2
+
3
+ __all__ = ["B1kInputs", "B1kOutputs", "extract_state_from_proprio"]
4
+
task 57/policy_code/src/b1k/policies/b1k_policy.py ADDED
@@ -0,0 +1,166 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """B1K Policy Transforms
2
+
3
+ Transforms BEHAVIOR-1K observations to model format.
4
+
5
+ Reference: https://github.com/wensi-ai/openpi/blob/behavior/src/openpi/policies/b1k_policy.py
6
+ """
7
+
8
+ import dataclasses
9
+
10
+ import einops
11
+ import numpy as np
12
+
13
+ from openpi import transforms
14
+ from openpi.models import model as _model
15
+
16
+ # 2026 BEHAVIOR-1K R1Pro proprioception layout (61-dim observation.state), copied from
17
+ # BEHAVIOR-1K/OmniGibson/omnigibson/eval/utils/eval_utils.py. The 2025 solution imported the
18
+ # 2025 256-dim layout from omnigibson.learning; the field names are unchanged, only the offsets.
19
+ PROPRIOCEPTION_INDICES = {
20
+ "R1Pro": {
21
+ "base_qvel": np.s_[0:3],
22
+ "arm_left_qpos": np.s_[3:10],
23
+ "gripper_left_qpos": np.s_[24:26],
24
+ "arm_right_qpos": np.s_[28:35],
25
+ "gripper_right_qpos": np.s_[49:51],
26
+ "trunk_qpos": np.s_[53:57],
27
+ }
28
+ }
29
+
30
+
31
+ def make_b1k_example() -> dict:
32
+ """Creates a random input example for the Droid policy."""
33
+ return {
34
+ "observation/egocentric_camera": np.random.randint(256, size=(224, 224, 3), dtype=np.uint8),
35
+ "observation/wrist_image_left": np.random.randint(256, size=(224, 224, 3), dtype=np.uint8),
36
+ "observation/wrist_image_right": np.random.randint(256, size=(224, 224, 3), dtype=np.uint8),
37
+ "observation/joint_position": np.random.rand(23),
38
+ "prompt": "do something",
39
+ }
40
+
41
+ def extract_state_from_proprio(proprio_data):
42
+ """
43
+ We assume perfect correlation for the two gripper fingers.
44
+ """
45
+ # extract joint position
46
+ # base_qvel is zeroed: in the 2025 data it was ~0 (state std ~0.01), so the 2025 model never learned
47
+ # to use it, while the 2026 data records real base velocities (std ~0.1); under the checkpoint's
48
+ # norm stats those would be 5-50 sigma out of distribution. Zero matches what the model was trained on.
49
+ base_qvel = np.zeros_like(proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["base_qvel"]]) # 3
50
+ trunk_qpos = proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["trunk_qpos"]] # 4
51
+ arm_left_qpos = proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["arm_left_qpos"]] # 7
52
+ arm_right_qpos = proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["arm_right_qpos"]] # 7
53
+
54
+ # Extract raw gripper widths and normalize to [-1, 1] to match action space
55
+ left_gripper_raw = proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["gripper_left_qpos"]].sum(axis=-1, keepdims=True)
56
+ right_gripper_raw = proprio_data[..., PROPRIOCEPTION_INDICES["R1Pro"]["gripper_right_qpos"]].sum(axis=-1, keepdims=True)
57
+
58
+ # Normalize gripper widths from [0, 0.1] to [-1, 1]
59
+ # Based on statistics: physical range is [0, 0.1], action range is [-1, 1]
60
+ # Formula: normalized = 2 * (raw / max_width) - 1
61
+ MAX_GRIPPER_WIDTH = 0.1 # From statistics q99 values
62
+ left_gripper_width = 2.0 * (left_gripper_raw / MAX_GRIPPER_WIDTH) - 1.0
63
+ right_gripper_width = 2.0 * (right_gripper_raw / MAX_GRIPPER_WIDTH) - 1.0
64
+
65
+ # Original baseline uses incorrect order for the state
66
+ return np.concatenate([
67
+ base_qvel,
68
+ trunk_qpos,
69
+ arm_left_qpos,
70
+ left_gripper_width, # Now normalized [-1, 1]
71
+ arm_right_qpos,
72
+ right_gripper_width, # Now normalized [-1, 1]
73
+ ], axis=-1)
74
+
75
+
76
+ def _parse_image(image) -> np.ndarray:
77
+ image = np.asarray(image)
78
+ if np.issubdtype(image.dtype, np.floating):
79
+ image = (255 * image).astype(np.uint8)
80
+ if image.shape[0] == 3:
81
+ image = einops.rearrange(image, "c h w -> h w c")
82
+ return image
83
+
84
+
85
+ @dataclasses.dataclass(frozen=True)
86
+ class B1kInputs(transforms.DataTransformFn):
87
+ # Determines which model will be used (not actually used in B1K, kept for compatibility)
88
+ model_type: _model.ModelType | str = _model.ModelType.PI0
89
+
90
+ def __call__(self, data: dict) -> dict:
91
+
92
+ proprio_data = data["observation/state"]
93
+ # extract joint position
94
+ state = extract_state_from_proprio(proprio_data)
95
+ if "actions" in data:
96
+ action = data["actions"]
97
+
98
+ # Possibly need to parse images to uint8 (H,W,C) since LeRobot automatically
99
+ # stores as float32 (C,H,W), gets skipped for policy inference
100
+ base_image = _parse_image(data["observation/egocentric_camera"])
101
+ wrist_image_left = _parse_image(data["observation/wrist_image_left"])
102
+ wrist_image_right = _parse_image(data["observation/wrist_image_right"])
103
+
104
+ # For B1K, always use 3 cameras (base, left_wrist, right_wrist)
105
+ names = ("base_0_rgb", "left_wrist_0_rgb", "right_wrist_0_rgb")
106
+ images = (base_image, wrist_image_left, wrist_image_right)
107
+ image_masks = (np.True_, np.True_, np.True_)
108
+
109
+ inputs = {
110
+ "state": state,
111
+ "image": dict(zip(names, images, strict=True)),
112
+ "image_mask": dict(zip(names, image_masks, strict=True)),
113
+ }
114
+
115
+ if "actions" in data:
116
+ inputs["actions"] = action
117
+
118
+ if "prompt" in data:
119
+ inputs["prompt"] = data["prompt"]
120
+
121
+ # Preserve task_index for PI_BEHAVIOR model
122
+ if "task_index" in data:
123
+ inputs["task_index"] = data["task_index"]
124
+
125
+ # Preserve tokenized_prompt for PI_BEHAVIOR model
126
+ if "tokenized_prompt" in data:
127
+ inputs["tokenized_prompt"] = data["tokenized_prompt"]
128
+ if "tokenized_prompt_mask" in data:
129
+ inputs["tokenized_prompt_mask"] = data["tokenized_prompt_mask"]
130
+
131
+ # Preserve subtask_state for PI_BEHAVIOR model
132
+ if "subtask_state" in data:
133
+ inputs["subtask_state"] = data["subtask_state"]
134
+
135
+ # Preserve timestamp and episode_index for subtask state computation
136
+ if "timestamp" in data:
137
+ inputs["timestamp"] = data["timestamp"]
138
+ if "episode_index" in data:
139
+ inputs["episode_index"] = data["episode_index"]
140
+
141
+ # Preserve initial_actions for inpainting
142
+ if "initial_actions" in data:
143
+ initial_actions = data["initial_actions"]
144
+ # Pad initial_actions from 23 dimensions to 32 dimensions (model's action_dim)
145
+ if initial_actions.shape[-1] < 32:
146
+ padding_dim = 32 - initial_actions.shape[-1]
147
+ padding = np.zeros(initial_actions.shape[:-1] + (padding_dim,))
148
+ initial_actions = np.concatenate([initial_actions, padding], axis=-1)
149
+ inputs["initial_actions"] = initial_actions
150
+
151
+ return inputs
152
+
153
+
154
+ @dataclasses.dataclass(frozen=True)
155
+ class B1kOutputs(transforms.DataTransformFn):
156
+ def __call__(self, data: dict) -> dict:
157
+ # Return actions (truncated to 23 dims) and preserve subtask predictions
158
+ result = {"actions": np.asarray(data["actions"][:, :23])}
159
+
160
+ # Preserve subtask prediction fields for PI_BEHAVIOR models
161
+ if "subtask_logits" in data:
162
+ result["subtask_logits"] = data["subtask_logits"]
163
+ if "predicted_stage" in data:
164
+ result["predicted_stage"] = data["predicted_stage"]
165
+
166
+ return result
task 57/policy_code/src/b1k/policies/checkpoint_switcher.py ADDED
@@ -0,0 +1,181 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Multi-task checkpoint switching for BEHAVIOR-1K.
2
+
3
+ Was used in the final submission to allow changing checkpoints based on task_id received from the server.
4
+ """
5
+
6
+ import json
7
+ import logging
8
+ import gc
9
+ from typing import Any, TYPE_CHECKING
10
+
11
+ from openpi.policies import policy as _policy
12
+
13
+ # Import B1K-specific policy_config
14
+ from b1k.policies import policy_config as _policy_config
15
+
16
+ if TYPE_CHECKING:
17
+ from b1k.training import config as _config
18
+
19
+
20
+ class CheckpointSwitcher:
21
+ """Loads different checkpoints per task. Only one checkpoint in memory at a time.
22
+ All 50 tasks must be mapped in task_checkpoint_mapping.json.
23
+ """
24
+
25
+ def __init__(
26
+ self,
27
+ config_path: str,
28
+ training_config: "TrainConfig",
29
+ sample_kwargs: dict[str, Any] | None = None,
30
+ ):
31
+ """Initialize the checkpoint switcher.
32
+
33
+ Args:
34
+ config_path: Path to task_checkpoint_mapping.json (REQUIRED)
35
+ training_config: Training config for loading policies
36
+ sample_kwargs: kwargs for policy sampling (e.g., num_steps)
37
+ """
38
+ if not config_path:
39
+ raise ValueError("config_path is required for checkpoint switching")
40
+
41
+ self.config_path = config_path
42
+ self.training_config = training_config
43
+ self.sample_kwargs = sample_kwargs or {}
44
+
45
+ # Task ID → checkpoint name mapping (built from config)
46
+ self.task_to_checkpoint: dict[int, str] = {}
47
+ # Checkpoint name → path mapping
48
+ self.checkpoint_paths: dict[str, str] = {}
49
+
50
+ # Currently loaded checkpoint
51
+ self.current_policy: _policy.Policy | None = None
52
+ self.current_checkpoint_name: str | None = None
53
+
54
+ # Load and validate mapping
55
+ self._load_mapping()
56
+ self._validate_all_tasks_covered()
57
+
58
+ logging.info(f"Checkpoint switcher initialized with {len(self.checkpoint_paths)} checkpoints")
59
+ logging.info(f"All 50 tasks explicitly mapped to checkpoints")
60
+
61
+ def _load_mapping(self):
62
+ """Load and validate the checkpoint→tasks mapping."""
63
+ try:
64
+ with open(self.config_path, 'r') as f:
65
+ config = json.load(f)
66
+ except FileNotFoundError:
67
+ logging.error(f"Checkpoint mapping file not found: {self.config_path}")
68
+ raise
69
+ except json.JSONDecodeError as e:
70
+ logging.error(f"Invalid JSON in checkpoint mapping file: {e}")
71
+ raise
72
+
73
+ if "checkpoints" not in config:
74
+ raise ValueError("Checkpoint mapping file must contain 'checkpoints' key")
75
+
76
+ # Build task→checkpoint and checkpoint→path mappings
77
+ seen_tasks = set()
78
+ for checkpoint_name, checkpoint_info in config["checkpoints"].items():
79
+ if "path" not in checkpoint_info:
80
+ raise ValueError(f"Checkpoint '{checkpoint_name}' missing 'path' field")
81
+ if "tasks" not in checkpoint_info:
82
+ raise ValueError(f"Checkpoint '{checkpoint_name}' missing 'tasks' field")
83
+
84
+ checkpoint_path = checkpoint_info["path"]
85
+ tasks = checkpoint_info["tasks"]
86
+
87
+ # Validate tasks are unique
88
+ for task_id in tasks:
89
+ if task_id in seen_tasks:
90
+ raise ValueError(f"Task {task_id} is assigned to multiple checkpoints")
91
+ seen_tasks.add(task_id)
92
+
93
+ self.checkpoint_paths[checkpoint_name] = checkpoint_path
94
+ for task_id in tasks:
95
+ self.task_to_checkpoint[task_id] = checkpoint_name
96
+
97
+ logging.info(f"Checkpoint '{checkpoint_name}' -> {len(tasks)} tasks: {sorted(tasks)}")
98
+
99
+ def _validate_all_tasks_covered(self):
100
+ """Validate that all 50 tasks are explicitly mapped to checkpoints."""
101
+ all_tasks = set(range(50))
102
+ mapped_tasks = set(self.task_to_checkpoint.keys())
103
+ missing_tasks = sorted(all_tasks - mapped_tasks)
104
+
105
+ if missing_tasks:
106
+ raise ValueError(
107
+ f"All 50 tasks must be explicitly mapped to checkpoints. "
108
+ f"Missing tasks: {missing_tasks}"
109
+ )
110
+
111
+ def get_checkpoint_for_task(self, task_id: int) -> str:
112
+ """Get which checkpoint should handle this task.
113
+
114
+ Args:
115
+ task_id: Task ID (0-49)
116
+
117
+ Returns:
118
+ Checkpoint name
119
+
120
+ Raises:
121
+ ValueError: If task_id not mapped (should never happen after validation)
122
+ """
123
+ if task_id not in self.task_to_checkpoint:
124
+ raise ValueError(f"Task {task_id} not mapped to any checkpoint")
125
+
126
+ return self.task_to_checkpoint[task_id]
127
+
128
+ def get_policy_for_task(self, task_id: int) -> _policy.Policy:
129
+ """Get policy for task_id, loading new checkpoint if needed.
130
+
131
+ Args:
132
+ task_id: Task ID (0-49)
133
+
134
+ Returns:
135
+ Policy for the requested task
136
+ """
137
+ target_checkpoint = self.get_checkpoint_for_task(task_id)
138
+
139
+ # If already loaded, return it
140
+ if self.current_checkpoint_name == target_checkpoint and self.current_policy is not None:
141
+ logging.debug(f"Task {task_id} using already-loaded checkpoint '{target_checkpoint}'")
142
+ return self.current_policy
143
+
144
+ # Need to switch checkpoints
145
+ logging.info(f"Task {task_id} requires checkpoint '{target_checkpoint}'")
146
+
147
+ # Unload current policy and free JAX/GPU memory
148
+ if self.current_policy is not None:
149
+ logging.info(f"Unloading checkpoint '{self.current_checkpoint_name}'")
150
+ del self.current_policy
151
+ self.current_policy = None
152
+
153
+ # Force garbage collection for JAX models
154
+ gc.collect()
155
+
156
+ # Clear JAX compilation cache and device memory
157
+ try:
158
+ import jax
159
+ jax.clear_caches()
160
+ logging.info("Cleared JAX caches and device memory")
161
+ except Exception as e:
162
+ logging.warning(f"Could not clear JAX caches: {e}")
163
+
164
+ # Load new checkpoint
165
+ checkpoint_path = self.checkpoint_paths[target_checkpoint]
166
+ logging.info(f"Loading checkpoint from: {checkpoint_path}")
167
+
168
+ try:
169
+ self.current_policy = _policy_config.create_trained_policy(
170
+ self.training_config,
171
+ checkpoint_path,
172
+ sample_kwargs=self.sample_kwargs
173
+ )
174
+ self.current_checkpoint_name = target_checkpoint
175
+ logging.info(f"Successfully loaded checkpoint '{target_checkpoint}'")
176
+ except Exception as e:
177
+ logging.error(f"Failed to load checkpoint '{target_checkpoint}': {e}")
178
+ raise
179
+
180
+ return self.current_policy
181
+
task 57/policy_code/src/b1k/policies/pi_behavior_policy.py ADDED
@@ -0,0 +1,125 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Minimal Policy subclass for PiBehavior models that handles tuple unpacking.
2
+
3
+ Reference: https://github.com/PhysicalIntelligence/openpi/blob/behavior/src/openpi/policies/policy.py
4
+ """
5
+
6
+ import time
7
+ from typing import Any
8
+
9
+ import jax
10
+ import jax.numpy as jnp
11
+ import numpy as np
12
+ from typing_extensions import override
13
+
14
+ from openpi.policies.policy import Policy
15
+ from b1k.models.observation import Observation # Use our custom Observation with FAST fields
16
+
17
+
18
+ class PiBehaviorPolicy(Policy):
19
+ """Policy for PiBehavior models - only difference is unpacking the tuple return.
20
+
21
+ PiBehavior.sample_actions() returns (actions, subtask_logits) instead of just actions.
22
+ This minimal subclass unpacks the tuple before output transforms are applied.
23
+ """
24
+
25
+ @override
26
+ def infer(self, obs: dict, *, noise: np.ndarray | None = None, initial_actions: np.ndarray | None = None) -> dict:
27
+ """Infer with PiBehavior-specific tuple unpacking.
28
+
29
+ Identical to parent Policy.infer() except:
30
+ 1. Accepts initial_actions parameter for rolling inpainting
31
+ 2. Unpacks (actions, subtask_logits) tuple before output transforms
32
+ """
33
+ # Reuse all parent logic for input processing
34
+ inputs = jax.tree.map(lambda x: x, obs)
35
+ inputs = self._input_transform(inputs)
36
+ inputs = jax.tree.map(lambda x: jnp.asarray(x)[np.newaxis, ...], inputs)
37
+ self._rng, sample_rng = jax.random.split(self._rng)
38
+
39
+ # Prepare sample_kwargs
40
+ sample_kwargs = dict(self._sample_kwargs)
41
+ if noise is not None:
42
+ noise = jnp.asarray(noise)
43
+ if noise.ndim == 2:
44
+ noise = noise[None, ...]
45
+ sample_kwargs["noise"] = noise
46
+
47
+ if initial_actions is not None:
48
+ # Create training-format observation batch for proper transform processing
49
+ training_obs = {}
50
+
51
+ # Map evaluation keys to training keys if needed
52
+ if "observation/state" in obs:
53
+ training_obs["observation/state"] = obs["observation/state"]
54
+ elif "state" in obs:
55
+ training_obs["observation/state"] = obs["state"]
56
+
57
+ if "observation/egocentric_camera" in obs:
58
+ training_obs["observation/egocentric_camera"] = obs["observation/egocentric_camera"]
59
+ elif "image" in obs and "base_0_rgb" in obs["image"]:
60
+ training_obs["observation/egocentric_camera"] = obs["image"]["base_0_rgb"]
61
+
62
+ if "observation/wrist_image_left" in obs:
63
+ training_obs["observation/wrist_image_left"] = obs["observation/wrist_image_left"]
64
+ elif "image" in obs and "left_wrist_0_rgb" in obs["image"]:
65
+ training_obs["observation/wrist_image_left"] = obs["image"]["left_wrist_0_rgb"]
66
+
67
+ if "observation/wrist_image_right" in obs:
68
+ training_obs["observation/wrist_image_right"] = obs["observation/wrist_image_right"]
69
+ elif "image" in obs and "right_wrist_0_rgb" in obs["image"]:
70
+ training_obs["observation/wrist_image_right"] = obs["image"]["right_wrist_0_rgb"]
71
+
72
+ # Copy any other keys that might be needed (tokenized_prompt, subtask_state, etc.)
73
+ for key in obs:
74
+ if key not in training_obs and key not in ["image", "state"]:
75
+ training_obs[key] = obs[key]
76
+
77
+ initial_batch = {
78
+ **training_obs, # Include all observation data in training format
79
+ "actions": initial_actions # Add initial_actions as the actions field
80
+ }
81
+
82
+ # Apply the full input transform pipeline (delta transforms + normalization)
83
+ transformed_batch = self._input_transform(initial_batch)
84
+ normalized_initial_actions = transformed_batch["actions"]
85
+
86
+ # Convert to JAX and add batch dim
87
+ initial_actions = jnp.asarray(normalized_initial_actions)
88
+ if initial_actions.ndim == 2:
89
+ initial_actions = initial_actions[None, ...]
90
+ sample_kwargs["initial_actions"] = initial_actions
91
+
92
+ observation = Observation.from_dict(inputs)
93
+ start_time = time.monotonic()
94
+
95
+ # ONLY DIFFERENCE: Unpack tuple return from PiBehavior.sample_actions
96
+ actions, subtask_logits = self._sample_actions(sample_rng, observation, **sample_kwargs)
97
+
98
+ outputs = {
99
+ "state": inputs["state"],
100
+ "actions": actions, # Now an array, not a tuple!
101
+ "subtask_logits": subtask_logits,
102
+ }
103
+
104
+ model_time = time.monotonic() - start_time
105
+
106
+ # Convert to numpy (same as parent)
107
+ outputs = {
108
+ k: np.asarray(v[0, ...]) if isinstance(v, (jnp.ndarray, np.ndarray)) else v
109
+ for k, v in outputs.items()
110
+ }
111
+
112
+ # Apply output transforms (now works because actions is an array)
113
+ outputs = self._output_transform(outputs)
114
+
115
+ # Add convenience field
116
+ outputs["predicted_stage"] = int(np.argmax(outputs["subtask_logits"]))
117
+
118
+ outputs["policy_timing"] = {
119
+ "infer_ms": model_time * 1000,
120
+ }
121
+ return outputs
122
+
123
+ @property
124
+ def metadata(self) -> dict[str, Any]:
125
+ return self._metadata
task 57/policy_code/src/b1k/policies/policy_config.py ADDED
@@ -0,0 +1,119 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Policy configuration for B1K - loads checkpoints and creates policies.
2
+
3
+ Exact copy of openpi.policies.policy_config but imports b1k.models.pi_behavior.PiBehavior.
4
+ """
5
+
6
+ import logging
7
+ import os
8
+ import pathlib
9
+ from typing import Any
10
+
11
+ import numpy as np
12
+ import jax.numpy as jnp
13
+
14
+ import openpi.models.model as _model
15
+ import openpi.policies.policy as _policy
16
+ import openpi.shared.download as download
17
+ import openpi.transforms as transforms
18
+
19
+ # Import B1K-specific modules
20
+ from b1k.models.pi_behavior import PiBehavior
21
+ from b1k.policies.pi_behavior_policy import PiBehaviorPolicy
22
+ from b1k.training import checkpoints as _checkpoints
23
+ from b1k.training import config as _config
24
+ from b1k import transforms as b1k_transforms
25
+ from b1k.transforms_normalize import NormalizeWithPerTimestamp, UnnormalizeWithPerTimestamp
26
+
27
+
28
+ def create_trained_policy(
29
+ train_config: _config.TrainConfig,
30
+ checkpoint_dir: pathlib.Path | str,
31
+ *,
32
+ repack_transforms: transforms.Group | None = None,
33
+ sample_kwargs: dict[str, Any] | None = None,
34
+ default_prompt: str | None = None,
35
+ norm_stats: dict[str, transforms.NormStats] | None = None,
36
+ pytorch_device: str | None = None,
37
+ ) -> _policy.Policy:
38
+ """Create a policy from a trained checkpoint - EXACT COPY from openpi with b1k imports."""
39
+ repack_transforms = repack_transforms or transforms.Group()
40
+ checkpoint_dir = download.maybe_download(str(checkpoint_dir))
41
+
42
+ # Detect PyTorch model
43
+ is_pytorch = (checkpoint_dir / "pytorch_model.safetensors").exists() or (checkpoint_dir / "pytorch_model.pt").exists()
44
+
45
+ if is_pytorch:
46
+ raise NotImplementedError("PyTorch inference not supported in b1k")
47
+
48
+ # JAX model loading - load directly as bfloat16 to save memory (12GB vs 24GB)
49
+ model = train_config.model.load(_model.restore_params(checkpoint_dir / "params", dtype=jnp.bfloat16))
50
+
51
+ # Get data config
52
+ data_config = train_config.data.create(train_config.assets_dirs, train_config.model)
53
+
54
+ # Load norm stats if not provided
55
+ if norm_stats is None:
56
+ if data_config.asset_id is None:
57
+ raise ValueError("Asset id is required to load norm stats.")
58
+ norm_stats = _checkpoints.load_norm_stats(checkpoint_dir / "assets", data_config.asset_id)
59
+
60
+ # Load correlation matrix for PiBehavior models
61
+ if isinstance(model, PiBehavior):
62
+ if norm_stats is None:
63
+ raise ValueError("PiBehavior requires norm_stats but none found.")
64
+ model.load_correlation_matrix(norm_stats)
65
+ logging.info("Loaded correlation matrix for inference")
66
+
67
+ # Determine the device for PyTorch (not used for b1k but kept for compatibility)
68
+ if is_pytorch and pytorch_device is None:
69
+ try:
70
+ import torch
71
+ pytorch_device = "cuda" if torch.cuda.is_available() else "cpu"
72
+ except ImportError:
73
+ pytorch_device = "cpu"
74
+
75
+ # For PI_BEHAVIOR models during inference, skip training-specific transforms
76
+ model_transforms_inputs = []
77
+ for transform in data_config.model_transforms.inputs:
78
+ # Skip training-specific transforms during inference
79
+ if isinstance(transform, (b1k_transforms.ComputeSubtaskStateFromMeta, b1k_transforms.TaskIndexToTaskId, b1k_transforms.TokenizeFASTActions)):
80
+ continue
81
+ model_transforms_inputs.append(transform)
82
+
83
+ # Build input transform pipeline (skip data_config.repack_transforms - has 'actions' mapping for training)
84
+ input_transforms = [
85
+ *repack_transforms.inputs,
86
+ transforms.InjectDefaultPrompt(default_prompt),
87
+ *data_config.data_transforms.inputs,
88
+ NormalizeWithPerTimestamp(norm_stats, use_quantiles=data_config.use_quantile_norm, use_per_timestamp=data_config.use_per_timestamp_norm),
89
+ *model_transforms_inputs,
90
+ ]
91
+
92
+ # Build output transform pipeline
93
+ output_transforms = [
94
+ *data_config.model_transforms.outputs,
95
+ UnnormalizeWithPerTimestamp(norm_stats, use_quantiles=data_config.use_quantile_norm, use_per_timestamp=data_config.use_per_timestamp_norm),
96
+ *data_config.data_transforms.outputs,
97
+ *repack_transforms.outputs,
98
+ ]
99
+
100
+ # Use custom PiBehaviorPolicy for PiBehavior models (handles tuple unpacking)
101
+ if isinstance(model, PiBehavior):
102
+ return PiBehaviorPolicy(
103
+ model,
104
+ transforms=input_transforms,
105
+ output_transforms=output_transforms,
106
+ sample_kwargs=sample_kwargs,
107
+ metadata=train_config.policy_metadata,
108
+ )
109
+ else:
110
+ return _policy.Policy(
111
+ model,
112
+ transforms=input_transforms,
113
+ output_transforms=output_transforms,
114
+ sample_kwargs=sample_kwargs,
115
+ metadata=train_config.policy_metadata,
116
+ is_pytorch=is_pytorch,
117
+ pytorch_device=pytorch_device if is_pytorch else "cpu",
118
+ )
119
+
task 57/policy_code/src/b1k/shared/__init__.py ADDED
@@ -0,0 +1,4 @@
 
 
 
 
 
1
+ from b1k.shared import normalize
2
+
3
+ __all__ = ["normalize"]
4
+
task 57/policy_code/src/b1k/shared/correction_rules.py ADDED
@@ -0,0 +1,333 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ Evaluation tricks for improving success rates on BEHAVIOR-1K tasks.
3
+
4
+ This module contains:
5
+ 1. Correction rules: Task-specific problem resolution (stage reset, gripper recovery)
6
+ 2. Gripper variation check: Disable compression when grippers have large movement
7
+
8
+ Enable via --apply-eval-tricks flag (default: False)
9
+ """
10
+
11
+ import logging
12
+ import numpy as np
13
+ from typing import Tuple, Optional
14
+
15
+ logger = logging.getLogger(__name__)
16
+
17
+
18
+ # ============================================================================
19
+ # GLOBAL CONSTANTS
20
+ # ============================================================================
21
+
22
+ # Gripper thresholds (normalized [-1, 1] range)
23
+ # Grippers are normalized from raw [0, 0.1] to [-1, 1]:
24
+ # -1.0 = fully closed (raw 0.0)
25
+ # -0.9 = mostly closed (raw 0.005)
26
+ # 0.0 = half open (raw 0.05)
27
+ # 0.9 = mostly open (raw 0.095)
28
+ # 1.0 = fully open (raw 0.1)
29
+ OPEN_THRESHOLD = 0.90
30
+ CLOSED_THRESHOLD = -0.98 # very strict threshold to avoid false positives
31
+
32
+ # Gripper variation threshold for compression check
33
+ GRIPPER_VARIATION_THRESHOLD = 0.2
34
+
35
+ # State indices
36
+ LEFT_GRIPPER_IDX = 14
37
+ RIGHT_GRIPPER_IDX = 22
38
+ LEFT_ARM_START_IDX = 7 # Left arm: state[7:14]
39
+ RIGHT_ARM_START_IDX = 15 # Right arm: state[15:22]
40
+ LEFT_ARM_END_IDX = 14
41
+ RIGHT_ARM_END_IDX = 22
42
+
43
+ # Base positions for arms (7 joints each)
44
+ BASE_POSITION_LEFT_ARM = np.array([-0.2, 0.0, 0.1, -1.4, 0.1, 0.7, 0.0])
45
+ BASE_POSITION_RIGHT_ARM = np.array([-0.2, 0.0, 0.1, -1.4, 0.1, 0.7, 0.0])
46
+
47
+
48
+ # ============================================================================
49
+ # GRIPPER CORRECTION POLICY
50
+ # ============================================================================
51
+
52
+ # Tasks where grippers should ALWAYS be open (any closure is an error)
53
+ ALWAYS_OPEN_LEFT_GRIPPER_TASKS = {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, 42, 43, 44, 45, 47, 48}
54
+ ALWAYS_OPEN_RIGHT_GRIPPER_TASKS = {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, 34, 35, 36, 37, 42, 43, 44, 47, 48, 49}
55
+
56
+ # Tasks where closure is not allowed BEFORE specified stage
57
+ # Format: {task_id: {'left': min_stage, 'right': min_stage}}
58
+ MIN_STAGE_FOR_CLOSURE = {
59
+ 0: {'left': 2, 'right': 2}, # Radio: allow closing from stage 2+
60
+ 30: {'right': 6}, # setting_the_fire
61
+ 31: {'right': 8}, # clean_boxing_gloves
62
+ 32: {'right': 5}, # wash_a_baseball_cap
63
+ 33: {'right': 11}, # wash_dog_toys
64
+ 40: {'right': 4}, # make_microwave_popcorn
65
+ 41: {'left': 14, 'right': 14}, # cook_cabbage
66
+ 45: {'right': 10}, # cook_hot_dogs
67
+ 46: {'left': 8, 'right': 8}, # cook_bacon
68
+ 49: {'left': 14}, # make_pizza
69
+ }
70
+
71
+ # Tasks where right gripper is ALWAYS allowed to close (exceptions)
72
+ RIGHT_GRIPPER_ALWAYS_ALLOWED = {38, 39} # spraying_for_bugs, spraying_fruit_trees
73
+
74
+
75
+ # ============================================================================
76
+ # CORRECTION RULES - ADD NEW RULES HERE
77
+ # ============================================================================
78
+
79
+ def task0_stage4_reset_to_stage2(
80
+ task_id: int,
81
+ stage: int,
82
+ state: np.ndarray,
83
+ actions: np.ndarray
84
+ ) -> Optional[Tuple[np.ndarray, int]]:
85
+ # Probably not very important for the final solution
86
+ """
87
+ Task 0 (Radio), Stage 4: Reset to stage 2 with arm/gripper recovery.
88
+
89
+ PRIMARY: task_id=0 AND stage=4 → reset to stage 2 (always)
90
+
91
+ SECONDARY (conditional):
92
+ - If both grippers are fully open or fully closed:
93
+ → Open closed grippers
94
+
95
+ Args:
96
+ task_id: Current task ID
97
+ stage: Current stage
98
+ state: Current state (23-dim)
99
+ actions: Predicted actions (horizon, 23)
100
+
101
+ Returns:
102
+ (corrected_actions, corrected_stage) if rule applies, None otherwise
103
+ """
104
+ # Check primary condition
105
+ if task_id != 0 or stage < 2:
106
+ return None
107
+
108
+ # PRIMARY: Always reset stage
109
+ if stage == 4:
110
+ logger.info(f"🔧 CORRECTION RULE: Task 0 (Radio), Stage 4 → Stage 2")
111
+ corrected_stage = 2
112
+ else:
113
+ corrected_stage = stage
114
+
115
+ # SECONDARY: Check gripper states
116
+ left_gripper = state[LEFT_GRIPPER_IDX]
117
+ right_gripper = state[RIGHT_GRIPPER_IDX]
118
+
119
+ left_closed = left_gripper < CLOSED_THRESHOLD
120
+ right_closed = right_gripper < CLOSED_THRESHOLD
121
+ left_open = left_gripper > OPEN_THRESHOLD
122
+ right_open = right_gripper > OPEN_THRESHOLD
123
+
124
+ # Check if any gripper is in middle position (not fully open or closed)
125
+ left_middle = not (left_open or left_closed)
126
+ right_middle = not (right_open or right_closed)
127
+
128
+ change_action = False
129
+
130
+ # Copy current state to all actions as baseline
131
+ corrected_actions = np.tile(state, (actions.shape[0], 1))
132
+
133
+ if left_closed and not right_middle:
134
+ corrected_actions[:, LEFT_GRIPPER_IDX] = 1.0
135
+ change_action = True
136
+ logger.info(f" - Opening left gripper → 1.0")
137
+ if right_closed and not left_middle:
138
+ corrected_actions[:, RIGHT_GRIPPER_IDX] = 1.0
139
+ change_action = True
140
+ logger.info(f" - Opening right gripper → 1.0")
141
+
142
+ if not change_action:
143
+ if stage == corrected_stage:
144
+ return None
145
+
146
+ corrected_actions = actions
147
+
148
+ return corrected_actions, corrected_stage
149
+
150
+
151
+ def general_gripper_correction(
152
+ task_id: int,
153
+ stage: int,
154
+ state: np.ndarray,
155
+ actions: np.ndarray
156
+ ) -> Optional[Tuple[np.ndarray, int]]:
157
+ """
158
+ General gripper correction rule for all tasks.
159
+
160
+ Applies task-specific policies:
161
+ 1. Always-open tasks: Open gripper if closed (any stage)
162
+ 2. Stage-restricted tasks: Open gripper if closed before allowed stage
163
+ 3. Exceptions: Skip tasks where gripper is intentionally closed (e.g., spray bottles)
164
+
165
+ Works in conjunction with task-specific rules:
166
+ - Task 0: This rule handles stages ≤1, task-specific rule handles stage 4
167
+
168
+ Args:
169
+ task_id: Current task ID
170
+ stage: Current stage
171
+ state: np.ndarray,
172
+ actions: Predicted actions (horizon, 23)
173
+
174
+ Returns:
175
+ (corrected_actions, corrected_stage) if rule applies, None otherwise
176
+ """
177
+ # Get gripper states
178
+ left_gripper = state[LEFT_GRIPPER_IDX]
179
+ right_gripper = state[RIGHT_GRIPPER_IDX]
180
+
181
+ left_closed = left_gripper < CLOSED_THRESHOLD
182
+ right_closed = right_gripper < CLOSED_THRESHOLD
183
+
184
+ # Determine if correction is needed
185
+ left_needs_opening = False
186
+ right_needs_opening = False
187
+
188
+ # Check left gripper
189
+ if left_closed:
190
+ # Always open if task requires it
191
+ if task_id in ALWAYS_OPEN_LEFT_GRIPPER_TASKS:
192
+ left_needs_opening = True
193
+ # Check stage restriction
194
+ elif task_id in MIN_STAGE_FOR_CLOSURE:
195
+ min_stage_left = MIN_STAGE_FOR_CLOSURE[task_id].get('left')
196
+ if min_stage_left is not None and stage < min_stage_left:
197
+ left_needs_opening = True
198
+
199
+ # Check right gripper
200
+ if right_closed:
201
+ # Exception: Always allow for spray bottle tasks
202
+ if task_id in RIGHT_GRIPPER_ALWAYS_ALLOWED:
203
+ pass # Never correct these tasks
204
+ # Always open if task requires it
205
+ elif task_id in ALWAYS_OPEN_RIGHT_GRIPPER_TASKS:
206
+ right_needs_opening = True
207
+ # Check stage restriction
208
+ elif task_id in MIN_STAGE_FOR_CLOSURE:
209
+ min_stage_right = MIN_STAGE_FOR_CLOSURE[task_id].get('right')
210
+ if min_stage_right is not None and stage < min_stage_right:
211
+ right_needs_opening = True
212
+
213
+ # Apply correction if needed
214
+ if left_needs_opening or right_needs_opening:
215
+ logger.info(f"🔧 GENERAL GRIPPER CORRECTION: Task {task_id}, Stage {stage}")
216
+
217
+ # Copy current state to all actions
218
+ corrected_actions = np.tile(state, (actions.shape[0], 1))
219
+
220
+ if left_needs_opening:
221
+ logger.info(f" Left gripper closed ({left_gripper:.4f}) → Opening to 1.0")
222
+ corrected_actions[:, LEFT_GRIPPER_IDX] = 1.0
223
+
224
+ if right_needs_opening:
225
+ logger.info(f" Right gripper closed ({right_gripper:.4f}) → Opening to 1.0")
226
+ corrected_actions[:, RIGHT_GRIPPER_IDX] = 1.0
227
+
228
+ # Keep stage unchanged
229
+ return corrected_actions, stage
230
+
231
+ return None # No correction needed
232
+
233
+
234
+ # ============================================================================
235
+ # RULE REGISTRY - ORGANIZE BY TASK_ID
236
+ # ============================================================================
237
+
238
+ # Task-specific rules (these run BEFORE the general rule)
239
+ # Dictionary mapping task_id -> list of rule functions
240
+ TASK_SPECIFIC_RULES = {
241
+ 0: [task0_stage4_reset_to_stage2], # Radio has special stage 4 handling
242
+ # Add more task-specific rules here as needed
243
+ }
244
+
245
+ # General rules that apply to ALL tasks (run after task-specific rules)
246
+ GENERAL_RULES = [
247
+ general_gripper_correction, # Applies to all tasks with gripper policies
248
+ ]
249
+
250
+
251
+ # ============================================================================
252
+ # RULE APPLICATION
253
+ # ============================================================================
254
+
255
+ def apply_correction_rules(
256
+ task_id: int,
257
+ stage: int,
258
+ state: np.ndarray,
259
+ actions: np.ndarray
260
+ ) -> Tuple[np.ndarray, int]:
261
+ """
262
+ Apply correction rules for the given task_id.
263
+
264
+ Rules are applied in order until one matches:
265
+ 1. Task-specific rules (e.g., radio's special stage 4 handling)
266
+ 2. General rules (e.g., gripper correction policies)
267
+
268
+ Only the first matching rule is applied per call.
269
+
270
+ Args:
271
+ task_id: Current task ID
272
+ stage: Current stage
273
+ state: Current state (23-dim)
274
+ actions: Predicted actions (horizon, 23)
275
+
276
+ Returns:
277
+ (actions, stage) - corrected or original if no rule applies
278
+ """
279
+ # Try task-specific rules first
280
+ task_specific_rules = TASK_SPECIFIC_RULES.get(task_id, [])
281
+ for rule in task_specific_rules:
282
+ result = rule(task_id, stage, state, actions)
283
+ if result is not None:
284
+ corrected_actions, corrected_stage = result
285
+ return corrected_actions, corrected_stage
286
+
287
+ # Try general rules (apply to all tasks)
288
+ for rule in GENERAL_RULES:
289
+ result = rule(task_id, stage, state, actions)
290
+ if result is not None:
291
+ corrected_actions, corrected_stage = result
292
+ return corrected_actions, corrected_stage
293
+
294
+ # No rules matched, return original
295
+ return actions, stage
296
+
297
+
298
+ # ============================================================================
299
+ # GRIPPER VARIATION CHECK
300
+ # ============================================================================
301
+
302
+ def check_gripper_variation(
303
+ actions: np.ndarray,
304
+ num_actions_to_check: int
305
+ ) -> Tuple[bool, float, float]:
306
+ """
307
+ Check if gripper actions have high variation.
308
+
309
+ High variation in gripper positions suggests complex manipulation that
310
+ should not be compressed via interpolation.
311
+
312
+ Args:
313
+ actions: Predicted actions (horizon, 23)
314
+ num_actions_to_check: Number of actions to check (e.g., actions_to_execute)
315
+
316
+ Returns:
317
+ (has_high_variation, left_variation, right_variation)
318
+ """
319
+ # Check only the actions we plan to execute
320
+ actions_to_check = actions[:num_actions_to_check]
321
+
322
+ # Extract gripper actions
323
+ left_gripper_actions = actions_to_check[:, LEFT_GRIPPER_IDX]
324
+ right_gripper_actions = actions_to_check[:, RIGHT_GRIPPER_IDX]
325
+
326
+ left_variation = float(np.max(left_gripper_actions) - np.min(left_gripper_actions))
327
+ right_variation = float(np.max(right_gripper_actions) - np.min(right_gripper_actions))
328
+
329
+ has_high_variation = (left_variation > GRIPPER_VARIATION_THRESHOLD or
330
+ right_variation > GRIPPER_VARIATION_THRESHOLD)
331
+
332
+ return has_high_variation, left_variation, right_variation
333
+
task 57/policy_code/src/b1k/shared/eval_b1k_wrapper.py ADDED
@@ -0,0 +1,307 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """B1K policy wrapper with action compression, rolling inpainting, and stage voting."""
2
+
3
+ import logging
4
+ import numpy as np
5
+ import torch
6
+ import dataclasses
7
+ from collections import deque
8
+
9
+ from openpi_client.base_policy import BasePolicy
10
+ from openpi_client.image_tools import resize_with_pad
11
+ from b1k.policies.b1k_policy import extract_state_from_proprio
12
+ from b1k.models.pi_behavior_config import TASK_NUM_STAGES
13
+ from b1k.shared.correction_rules import apply_correction_rules, check_gripper_variation
14
+ from b1k.policies.b1k_policy import PROPRIOCEPTION_INDICES # 2026 layout (was omnigibson 2025)
15
+
16
+ logger = logging.getLogger(__name__)
17
+
18
+ RESIZE_SIZE = 224
19
+
20
+
21
+ @dataclasses.dataclass
22
+ class B1KWrapperConfig:
23
+ """Configuration for B1K policy wrapper execution parameters."""
24
+ actions_to_execute: int = 26
25
+ actions_to_keep: int = 4
26
+ execute_in_n_steps: int = 20
27
+ history_len: int = 3
28
+ votes_to_promote: int = 2
29
+ time_threshold_inpaint: float = 0.3
30
+ num_steps: int = 20
31
+ apply_eval_tricks: bool = True
32
+
33
+
34
+ class B1KPolicyWrapper():
35
+ """B1K policy wrapper for PI_BEHAVIOR models with action compression, rolling inpainting, and stage voting."""
36
+
37
+ def __init__(
38
+ self,
39
+ policy: BasePolicy,
40
+ text_prompt: str = "PI_BEHAVIOR model (task-conditioned)", # Not used, kept for compatibility
41
+ action_horizon: int = 30,
42
+ task_id: int | None = None,
43
+ config: B1KWrapperConfig = None,
44
+ checkpoint_switcher = None,
45
+ ) -> None:
46
+ self.base_policy = policy
47
+ self.policy = policy
48
+ self.checkpoint_switcher = checkpoint_switcher
49
+ self.text_prompt = text_prompt
50
+ self.action_horizon = action_horizon
51
+ self.config = config if config is not None else B1KWrapperConfig()
52
+
53
+ # Validate configuration
54
+ if self.config.actions_to_execute + self.config.actions_to_keep > self.action_horizon:
55
+ raise ValueError(
56
+ f"actions_to_execute + actions_to_keep exceeds action_horizon"
57
+ )
58
+
59
+ # PI_BEHAVIOR specific (always True for B1K)
60
+ self.task_id = task_id
61
+ self.current_stage = 0
62
+ self.prediction_history = deque([], maxlen=self.config.history_len)
63
+
64
+ # Control loop variables
65
+ self.last_actions = None
66
+ self.action_index = 0
67
+ self.step_count = 0
68
+ self.prediction_count = 0
69
+ self.next_initial_actions = None
70
+
71
+ def reset(self):
72
+ """Reset policy state."""
73
+ self.policy.reset()
74
+ self.last_actions = None
75
+ self.action_index = 0
76
+ self.step_count = 0
77
+ self.prediction_count = 0
78
+ self.next_initial_actions = None
79
+ self.current_stage = 0
80
+ self.prediction_history.clear()
81
+ logger.info(f"Policy reset - Task ID: {self.task_id}, Action horizon: {self.action_horizon}")
82
+
83
+ def _handle_task_change(self, new_task_id):
84
+ """Handle task ID change by switching checkpoint and resetting state."""
85
+ if self.task_id != new_task_id:
86
+ old_task_id = self.task_id
87
+ self.task_id = new_task_id
88
+
89
+ logger.info(f"🔄 Task change detected: {old_task_id} → {new_task_id} (max stages: {TASK_NUM_STAGES[new_task_id]})")
90
+
91
+ if self.checkpoint_switcher:
92
+ new_policy = self.checkpoint_switcher.get_policy_for_task(new_task_id)
93
+ if new_policy is not self.policy:
94
+ logger.info(f"📦 Switching checkpoint: task {old_task_id} → {new_task_id}")
95
+ self.base_policy = new_policy
96
+ self.policy = new_policy
97
+ self.policy.reset()
98
+
99
+ self.current_stage = 0
100
+ self.prediction_history.clear()
101
+ self.last_actions = None
102
+ self.action_index = 0
103
+ self.next_initial_actions = None
104
+
105
+ def process_obs(self, obs: dict) -> dict:
106
+ """Process observation to match model input format."""
107
+ prop_state = obs["robot_r1::proprio"]
108
+
109
+ head_original = obs["robot_r1::robot_r1:zed_link:Camera:0::rgb"][..., :3]
110
+ left_original = obs["robot_r1::robot_r1:left_realsense_link:Camera:0::rgb"][..., :3]
111
+ right_original = obs["robot_r1::robot_r1:right_realsense_link:Camera:0::rgb"][..., :3]
112
+
113
+ # Resize images
114
+ head_resized = resize_with_pad(head_original, RESIZE_SIZE, RESIZE_SIZE)
115
+ left_resized = resize_with_pad(left_original, RESIZE_SIZE, RESIZE_SIZE)
116
+ right_resized = resize_with_pad(right_original, RESIZE_SIZE, RESIZE_SIZE)
117
+
118
+ return {
119
+ "observation/egocentric_camera": head_resized,
120
+ "observation/wrist_image_left": left_resized,
121
+ "observation/wrist_image_right": right_resized,
122
+ "observation/state": prop_state,
123
+ "prompt": self.text_prompt,
124
+ }
125
+
126
+ def update_current_stage(self, predicted_subtask_logits):
127
+ """Update current stage using majority voting."""
128
+ if self.task_id is None:
129
+ return
130
+
131
+ max_stage = TASK_NUM_STAGES[self.task_id] - 1
132
+ predicted_stage = int(np.argmax(predicted_subtask_logits))
133
+
134
+ if predicted_stage > max_stage:
135
+ predicted_stage = max_stage
136
+
137
+ self.prediction_history.append(predicted_stage)
138
+
139
+ if len(self.prediction_history) == self.config.history_len:
140
+ next_stage = self.current_stage + 1
141
+
142
+ if next_stage <= max_stage:
143
+ votes_for_next = sum(1 for pred in self.prediction_history if pred == next_stage)
144
+ votes_to_skip = sum(1 for pred in self.prediction_history if pred == next_stage + 1)
145
+ votes_to_go_back = sum(1 for pred in self.prediction_history if pred == self.current_stage - 1)
146
+
147
+ if votes_for_next >= self.config.votes_to_promote:
148
+ old_stage = self.current_stage
149
+ self.current_stage = next_stage
150
+ self.prediction_history.clear()
151
+ logger.info(f"⬆️ Stage advanced: {old_stage} → {self.current_stage} (task {self.task_id}, step {self.step_count})")
152
+ elif votes_to_skip == self.config.history_len:
153
+ old_stage = self.current_stage
154
+ self.current_stage = next_stage
155
+ self.prediction_history.clear()
156
+ logger.info(f"⏭️ Stage skipped: {old_stage} → {self.current_stage} (task {self.task_id}, step {self.step_count})")
157
+ elif votes_to_go_back == self.config.history_len and self.current_stage > 0:
158
+ old_stage = self.current_stage
159
+ self.current_stage -= 1
160
+ self.prediction_history.clear()
161
+ logger.info(f"⬅️ Stage went back: {old_stage} → {self.current_stage} (task {self.task_id}, step {self.step_count})")
162
+
163
+ def prepare_batch_for_pi_behavior(self, batch):
164
+ """Prepare batch for PI_BEHAVIOR model by adding task_id and current_stage."""
165
+ task_id = self.task_id if self.task_id is not None else -1
166
+ batch_copy = batch.copy()
167
+ if "prompt" in batch_copy:
168
+ del batch_copy["prompt"]
169
+
170
+ batch_copy["tokenized_prompt"] = np.array([task_id, self.current_stage], dtype=np.int32)
171
+ batch_copy["tokenized_prompt_mask"] = np.array([True, True], dtype=bool)
172
+ batch_copy["subtask_state"] = np.array(self.current_stage, dtype=np.int32)
173
+
174
+ return batch_copy
175
+
176
+ def _interpolate_actions(self, actions, target_steps):
177
+ """Interpolate actions using cubic spline."""
178
+ from scipy.interpolate import interp1d
179
+
180
+ original_indices = np.linspace(0, len(actions)-1, len(actions))
181
+ target_indices = np.linspace(0, len(actions)-1, target_steps)
182
+
183
+ interpolated = np.zeros((target_steps, actions.shape[1]))
184
+ for dim in range(actions.shape[1]):
185
+ f = interp1d(original_indices, actions[:, dim], kind='cubic')
186
+ interpolated[:, dim] = f(target_indices)
187
+
188
+ return interpolated
189
+
190
+ def act(self, obs: dict) -> torch.Tensor:
191
+ """Main action function."""
192
+
193
+ # Extract task_id from observations
194
+ if "task_id" in obs:
195
+ new_task_id = int(obs["task_id"][0])
196
+ self._handle_task_change(new_task_id)
197
+
198
+ raw_state = obs["robot_r1::proprio"]
199
+ current_state = extract_state_from_proprio(raw_state)
200
+
201
+ # Check if we need new actions
202
+ if self.last_actions is None or self.action_index >= self.config.execute_in_n_steps:
203
+
204
+ # Process observation
205
+ model_input = self.process_obs(obs)
206
+ model_input = self.prepare_batch_for_pi_behavior(model_input)
207
+
208
+ # Add rolling inpainting if available
209
+ if self.next_initial_actions is not None and ("initial_actions" not in model_input or model_input["initial_actions"] is None):
210
+ model_input["initial_actions"] = self.next_initial_actions
211
+
212
+ # Get prediction
213
+ if "initial_actions" in model_input and model_input["initial_actions"] is not None:
214
+ output = self.policy.infer(model_input, initial_actions=model_input["initial_actions"])
215
+ else:
216
+ output = self.policy.infer(model_input)
217
+
218
+ actions = output["actions"]
219
+
220
+ # Ensure correct shape
221
+ if len(actions.shape) == 3:
222
+ actions = actions[0]
223
+ if actions.shape[1] > 23:
224
+ actions = actions[:, :23]
225
+
226
+ # Apply eval tricks if enabled
227
+ should_compress = self.config.execute_in_n_steps < self.config.actions_to_execute
228
+
229
+ if self.config.apply_eval_tricks:
230
+ if self.task_id is not None:
231
+ actions_before = actions.copy()
232
+ actions, corrected_stage = apply_correction_rules(
233
+ self.task_id, self.current_stage, current_state, actions
234
+ )
235
+
236
+ # Log if stage was corrected
237
+ if corrected_stage != self.current_stage:
238
+ logger.info(f"🔧 Correction rule: Stage corrected {self.current_stage} → {corrected_stage} (task {self.task_id}, step {self.step_count})")
239
+ self.current_stage = corrected_stage
240
+ self.prediction_history.clear()
241
+
242
+ # Log if actions were modified
243
+ if not np.allclose(actions_before, actions, rtol=1e-3):
244
+ max_diff = np.max(np.abs(actions_before - actions))
245
+ logger.info(f"🔧 Correction rule: Actions modified (max diff: {max_diff:.4f}, task {self.task_id}, stage {self.current_stage})")
246
+
247
+ if should_compress:
248
+ has_high_variation, mean_var, max_var = check_gripper_variation(
249
+ actions, self.config.actions_to_execute
250
+ )
251
+ if has_high_variation:
252
+ should_compress = False
253
+ logger.info(f"🔧 Gripper variation: Compression disabled (mean: {mean_var:.4f}, max: {max_var:.4f})")
254
+
255
+ # Determine execution parameters
256
+ actions_to_execute = self.config.actions_to_execute if should_compress else self.config.execute_in_n_steps
257
+ execute_steps = self.config.execute_in_n_steps
258
+
259
+ # Save actions for next inpainting (before compression)
260
+ inpainting_start = actions_to_execute
261
+ inpainting_end = inpainting_start + self.config.actions_to_keep
262
+
263
+ if len(actions) >= inpainting_end:
264
+ self.next_initial_actions = actions[inpainting_start:inpainting_end].copy()
265
+ else:
266
+ self.next_initial_actions = None
267
+
268
+ # Extract and compress actions
269
+ self.last_actions = actions[:actions_to_execute].copy()
270
+
271
+ if should_compress:
272
+ compressed_actions = self._interpolate_actions(self.last_actions, execute_steps)
273
+ compression_factor = actions_to_execute / execute_steps
274
+ compressed_actions[:, :3] *= compression_factor # Scale velocities
275
+ self.last_actions = compressed_actions
276
+
277
+ self.action_index = 0
278
+ self.prediction_count += 1
279
+
280
+ # Log prediction details (at lower frequency, every 10 predictions)
281
+ if self.prediction_count % 10 == 0:
282
+ compression_status = f"compressed {actions_to_execute}→{execute_steps}" if should_compress else f"uncompressed ({execute_steps})"
283
+ logger.info(f"🎯 Prediction #{self.prediction_count} | Actions: {compression_status} | Inpainting: {self.next_initial_actions is not None}")
284
+
285
+ # Update stage based on model predictions
286
+ if "subtask_logits" in output:
287
+ self.update_current_stage(output["subtask_logits"])
288
+
289
+ # Get current action from sequence
290
+ if self.action_index >= len(self.last_actions):
291
+ self.action_index = 0
292
+
293
+ current_action = self.last_actions[self.action_index]
294
+ self.action_index += 1
295
+ self.step_count += 1
296
+
297
+ # Log progress every 100 steps
298
+ if self.step_count % 100 == 0:
299
+ logger.info(f"📊 Step {self.step_count} | Task: {self.task_id} | Stage: {self.current_stage}/{TASK_NUM_STAGES[self.task_id]-1} | Predictions: {self.prediction_count}")
300
+
301
+ # Convert to torch tensor
302
+ action_tensor = torch.from_numpy(current_action).float()
303
+ if len(action_tensor) > 23:
304
+ action_tensor = action_tensor[:23]
305
+
306
+ return action_tensor
307
+
task 57/policy_code/src/b1k/shared/normalize.py ADDED
@@ -0,0 +1,172 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Normalization statistics.
2
+
3
+ Supports per-timestamp normalization.
4
+ """
5
+
6
+ import json
7
+ import pathlib
8
+
9
+ import numpy as np
10
+ import numpydantic
11
+ import pydantic
12
+
13
+
14
+ @pydantic.dataclasses.dataclass
15
+ class NormStats:
16
+ mean: numpydantic.NDArray
17
+ std: numpydantic.NDArray
18
+ q01: numpydantic.NDArray | None = None # 1st quantile
19
+ q99: numpydantic.NDArray | None = None # 99th quantile
20
+
21
+ # Per-timestamp normalization stats (shape: [action_horizon, action_dim])
22
+ # If None, use regular normalization
23
+ per_timestamp_mean: numpydantic.NDArray | None = None
24
+ per_timestamp_std: numpydantic.NDArray | None = None
25
+ per_timestamp_q01: numpydantic.NDArray | None = None
26
+ per_timestamp_q99: numpydantic.NDArray | None = None
27
+
28
+ # Action correlation matrix for correlated noise generation (legacy - full matrix)
29
+ # Shape: [action_horizon * action_dim, action_horizon * action_dim]
30
+ # Cholesky decomposition of the empirical covariance matrix
31
+ action_correlation_cholesky: numpydantic.NDArray | None = None
32
+
33
+ # Averaged correlation matrices (for analysis)
34
+ # Spatial correlation: correlation between action dimensions (averaged across timesteps)
35
+ # Shape: [action_dim, action_dim]
36
+ action_correlation_spatial: numpydantic.NDArray | None = None
37
+
38
+ # Temporal correlation: correlation across time (averaged across dimensions)
39
+ # Shape: [action_horizon, action_horizon]
40
+ action_correlation_temporal: numpydantic.NDArray | None = None
41
+
42
+
43
+ class RunningStats:
44
+ """Compute running statistics of a batch of vectors."""
45
+
46
+ def __init__(self):
47
+ self._count = 0
48
+ self._mean = None
49
+ self._mean_of_squares = None
50
+ self._min = None
51
+ self._max = None
52
+ self._histograms = None
53
+ self._bin_edges = None
54
+ self._num_quantile_bins = 5000 # for computing quantiles on the fly
55
+
56
+ def update(self, batch: np.ndarray) -> None:
57
+ """
58
+ Update the running statistics with a batch of vectors.
59
+
60
+ Args:
61
+ vectors (np.ndarray): An array where all dimensions except the last are batch dimensions.
62
+ """
63
+ batch = batch.reshape(-1, batch.shape[-1])
64
+ num_elements, vector_length = batch.shape
65
+ if self._count == 0:
66
+ self._mean = np.mean(batch, axis=0)
67
+ self._mean_of_squares = np.mean(batch**2, axis=0)
68
+ self._min = np.min(batch, axis=0)
69
+ self._max = np.max(batch, axis=0)
70
+ self._histograms = [np.zeros(self._num_quantile_bins) for _ in range(vector_length)]
71
+ self._bin_edges = [
72
+ np.linspace(self._min[i] - 1e-10, self._max[i] + 1e-10, self._num_quantile_bins + 1)
73
+ for i in range(vector_length)
74
+ ]
75
+ else:
76
+ if vector_length != self._mean.size:
77
+ raise ValueError("The length of new vectors does not match the initialized vector length.")
78
+ new_max = np.max(batch, axis=0)
79
+ new_min = np.min(batch, axis=0)
80
+ max_changed = np.any(new_max > self._max)
81
+ min_changed = np.any(new_min < self._min)
82
+ self._max = np.maximum(self._max, new_max)
83
+ self._min = np.minimum(self._min, new_min)
84
+
85
+ if max_changed or min_changed:
86
+ self._adjust_histograms()
87
+
88
+ self._count += num_elements
89
+
90
+ batch_mean = np.mean(batch, axis=0)
91
+ batch_mean_of_squares = np.mean(batch**2, axis=0)
92
+
93
+ # Update running mean and mean of squares.
94
+ self._mean += (batch_mean - self._mean) * (num_elements / self._count)
95
+ self._mean_of_squares += (batch_mean_of_squares - self._mean_of_squares) * (num_elements / self._count)
96
+
97
+ self._update_histograms(batch)
98
+
99
+ def get_statistics(self) -> NormStats:
100
+ """
101
+ Compute and return the statistics of the vectors processed so far.
102
+
103
+ Returns:
104
+ dict: A dictionary containing the computed statistics.
105
+ """
106
+ if self._count < 2:
107
+ raise ValueError("Cannot compute statistics for less than 2 vectors.")
108
+
109
+ variance = self._mean_of_squares - self._mean**2
110
+ stddev = np.sqrt(np.maximum(0, variance))
111
+ q01, q99 = self._compute_quantiles([0.01, 0.99])
112
+ return NormStats(mean=self._mean, std=stddev, q01=q01, q99=q99)
113
+
114
+ def _adjust_histograms(self):
115
+ """Adjust histograms when min or max changes."""
116
+ for i in range(len(self._histograms)):
117
+ old_edges = self._bin_edges[i]
118
+ new_edges = np.linspace(self._min[i], self._max[i], self._num_quantile_bins + 1)
119
+
120
+ # Redistribute the existing histogram counts to the new bins
121
+ new_hist, _ = np.histogram(old_edges[:-1], bins=new_edges, weights=self._histograms[i])
122
+
123
+ self._histograms[i] = new_hist
124
+ self._bin_edges[i] = new_edges
125
+
126
+ def _update_histograms(self, batch: np.ndarray) -> None:
127
+ """Update histograms with new vectors."""
128
+ for i in range(batch.shape[1]):
129
+ hist, _ = np.histogram(batch[:, i], bins=self._bin_edges[i])
130
+ self._histograms[i] += hist
131
+
132
+ def _compute_quantiles(self, quantiles):
133
+ """Compute quantiles based on histograms."""
134
+ results = []
135
+ for q in quantiles:
136
+ target_count = q * self._count
137
+ q_values = []
138
+ for hist, edges in zip(self._histograms, self._bin_edges, strict=True):
139
+ cumsum = np.cumsum(hist)
140
+ idx = np.searchsorted(cumsum, target_count)
141
+ q_values.append(edges[idx])
142
+ results.append(np.array(q_values))
143
+ return results
144
+
145
+
146
+ class _NormStatsDict(pydantic.BaseModel):
147
+ norm_stats: dict[str, NormStats]
148
+
149
+
150
+ def serialize_json(norm_stats: dict[str, NormStats]) -> str:
151
+ """Serialize the running statistics to a JSON string."""
152
+ return _NormStatsDict(norm_stats=norm_stats).model_dump_json(indent=2)
153
+
154
+
155
+ def deserialize_json(data: str) -> dict[str, NormStats]:
156
+ """Deserialize the running statistics from a JSON string."""
157
+ return _NormStatsDict(**json.loads(data)).norm_stats
158
+
159
+
160
+ def save(directory: pathlib.Path | str, norm_stats: dict[str, NormStats]) -> None:
161
+ """Save the normalization stats to a directory."""
162
+ path = pathlib.Path(directory) / "norm_stats.json"
163
+ path.parent.mkdir(parents=True, exist_ok=True)
164
+ path.write_text(serialize_json(norm_stats))
165
+
166
+
167
+ def load(directory: pathlib.Path | str) -> dict[str, NormStats]:
168
+ """Load the normalization stats from a directory."""
169
+ path = pathlib.Path(directory) / "norm_stats.json"
170
+ if not path.exists():
171
+ raise FileNotFoundError(f"Norm stats file not found at: {path}")
172
+ return deserialize_json(path.read_text())
task 57/policy_code/src/b1k/training/__init__.py ADDED
@@ -0,0 +1,6 @@
 
 
 
 
 
 
 
1
+ from b1k.training import config
2
+ from b1k.training import data_loader
3
+ from b1k.training import weight_loaders
4
+
5
+ __all__ = ["config", "data_loader", "weight_loaders"]
6
+
task 57/policy_code/src/b1k/training/checkpoints.py ADDED
@@ -0,0 +1,209 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Checkpoint management with FAST tokenizer saving.
2
+
3
+ Reference: https://github.com/Physical-Intelligence/openpi
4
+ """
5
+
6
+ from __future__ import annotations
7
+
8
+ import asyncio
9
+ import concurrent.futures as futures
10
+ import dataclasses
11
+ import logging
12
+ import shutil
13
+ from typing import Protocol
14
+
15
+ from etils import epath
16
+ import flax.traverse_util
17
+ import jax
18
+ import flax.nnx as nnx
19
+
20
+ import orbax.checkpoint as ocp
21
+ import orbax.checkpoint.future as future
22
+
23
+ from openpi.shared import array_typing as at
24
+ import openpi.training.data_loader as _data_loader
25
+ import openpi.training.utils as training_utils
26
+
27
+ # Use our custom normalize (has per_timestamp fields and can save/load them)
28
+ from b1k.shared import normalize as _normalize
29
+
30
+
31
+ def initialize_checkpoint_dir(
32
+ checkpoint_dir: epath.Path | str, *, keep_period: int | None, overwrite: bool, resume: bool
33
+ ) -> tuple[ocp.CheckpointManager, bool]:
34
+ checkpoint_dir = epath.Path(checkpoint_dir).resolve()
35
+ resuming = False
36
+ if checkpoint_dir.exists():
37
+ if overwrite:
38
+ checkpoint_dir.rmtree()
39
+ checkpoint_dir.mkdir(parents=True, exist_ok=True)
40
+ logging.info(f"Wiped checkpoint directory {checkpoint_dir}")
41
+ elif resume:
42
+ resuming = True
43
+ else:
44
+ raise FileExistsError(
45
+ f"Checkpoint directory {checkpoint_dir} already exists. Use --overwrite or --resume "
46
+ "to indicate how to handle it."
47
+ )
48
+
49
+ checkpoint_dir.mkdir(parents=True, exist_ok=True)
50
+
51
+ mngr = ocp.CheckpointManager(
52
+ checkpoint_dir,
53
+ item_handlers={
54
+ "assets": CallbackHandler(),
55
+ "train_state": ocp.PyTreeCheckpointHandler(),
56
+ "params": ocp.PyTreeCheckpointHandler(),
57
+ },
58
+ options=ocp.CheckpointManagerOptions(
59
+ max_to_keep=1,
60
+ keep_period=keep_period,
61
+ create=False,
62
+ async_options=ocp.AsyncOptions(timeout_secs=7200),
63
+ ),
64
+ )
65
+
66
+ # Special case: the checkpoint directory exists and the user requests to resume training, but the training run did
67
+ # not get to the first checkpoint saved. In this case, we don't actually want the train script to try and restore a
68
+ # checkpoint, since it will fail.
69
+ if resuming and tuple(mngr.all_steps()) in [(), (0,)]:
70
+ logging.info("Checkpoint directory exists, but does not contain any checkpoints. Aborting resume.")
71
+ resuming = False
72
+
73
+ return mngr, resuming
74
+
75
+
76
+ def save_state(
77
+ checkpoint_manager: ocp.CheckpointManager,
78
+ state: training_utils.TrainState,
79
+ data_loader: _data_loader.DataLoader,
80
+ step: int,
81
+ ):
82
+ def save_assets(directory: epath.Path):
83
+ # Save the normalization stats.
84
+ data_config = data_loader.data_config()
85
+ norm_stats = data_config.norm_stats
86
+ if norm_stats is not None and data_config.asset_id is not None:
87
+ _normalize.save(directory / data_config.asset_id, norm_stats)
88
+
89
+ # Save FAST tokenizer if it exists
90
+ model = nnx.merge(state.model_def, state.params)
91
+ # Check if model has FAST enabled (indicated by having fast_token_embedding)
92
+ if hasattr(model, 'fast_token_embedding'):
93
+ # Get the checkpoint directory to determine assets base dir
94
+ # Directory structure: checkpoint_dir/step/assets/asset_id/fast_tokenizer
95
+ # We need to copy from: assets_base_dir/asset_id/fast_tokenizer
96
+ # The assets_base_dir is stored in the config, but we can infer it from checkpoint_dir
97
+ checkpoint_dir = checkpoint_manager.directory
98
+
99
+ # Infer assets_base_dir from checkpoint structure
100
+ # checkpoint_dir is like: ./outputs/checkpoints/config_name/exp_name
101
+ # assets_base_dir is like: ./outputs/assets/config_name
102
+ parts = checkpoint_dir.parts
103
+ if 'checkpoints' in parts:
104
+ idx = parts.index('checkpoints')
105
+ assets_base_parts = parts[:idx] + ('assets',) + (parts[idx + 1],) # Keep config_name
106
+ assets_base_dir = epath.Path(*assets_base_parts)
107
+ else:
108
+ # Fallback: try relative to checkpoint dir
109
+ assets_base_dir = checkpoint_dir.parent.parent / 'assets' / checkpoint_dir.parent.name
110
+
111
+ fast_tokenizer_source = assets_base_dir / data_config.asset_id / "fast_tokenizer"
112
+ fast_tokenizer_dest = directory / data_config.asset_id / "fast_tokenizer"
113
+
114
+ if fast_tokenizer_source.exists():
115
+ # Create parent directory
116
+ fast_tokenizer_dest.parent.mkdir(parents=True, exist_ok=True)
117
+ # Copy tokenizer directory
118
+ shutil.copytree(fast_tokenizer_source, fast_tokenizer_dest, dirs_exist_ok=True)
119
+ logging.info(f"Saved FAST tokenizer to checkpoint: {fast_tokenizer_dest}")
120
+
121
+ # Split params that can be used for inference into a separate item.
122
+ with at.disable_typechecking():
123
+ train_state, params = _split_params(state)
124
+ items = {
125
+ "assets": save_assets,
126
+ "train_state": train_state,
127
+ "params": {"params": params},
128
+ }
129
+ checkpoint_manager.save(step, items)
130
+
131
+
132
+ def restore_state(
133
+ checkpoint_manager: ocp.CheckpointManager,
134
+ state: training_utils.TrainState,
135
+ data_loader: _data_loader.DataLoader,
136
+ step: int | None = None,
137
+ ) -> training_utils.TrainState:
138
+ del data_loader
139
+
140
+ with at.disable_typechecking():
141
+ # Split params that can be used for inference into a separate item.
142
+ train_state, params = _split_params(state)
143
+
144
+ restored = checkpoint_manager.restore(
145
+ step,
146
+ items={
147
+ "train_state": train_state,
148
+ "params": {"params": params},
149
+ },
150
+ )
151
+
152
+ return _merge_params(restored["train_state"], restored["params"])
153
+
154
+
155
+ def load_norm_stats(assets_dir: epath.Path | str, asset_id: str) -> dict[str, _normalize.NormStats] | None:
156
+ norm_stats_dir = epath.Path(assets_dir) / asset_id
157
+ norm_stats = _normalize.load(norm_stats_dir)
158
+ logging.info(f"Loaded norm stats from {norm_stats_dir}")
159
+ return norm_stats
160
+
161
+
162
+ class Callback(Protocol):
163
+ def __call__(self, directory: epath.Path) -> None: ...
164
+
165
+
166
+ class CallbackHandler(ocp.AsyncCheckpointHandler):
167
+ """A CheckpointHandler for calling an arbitrary function asynchronously. Only for saving, not for restoring."""
168
+
169
+ def save(self, directory: epath.Path, args: CallbackSave):
170
+ if jax.process_index() == 0:
171
+ args.callback(directory)
172
+
173
+ async def async_save(self, directory: epath.Path, args: CallbackSave) -> list[futures.Future]:
174
+ return [future.CommitFutureAwaitingContractedSignals(asyncio.to_thread(self.save, directory, args))]
175
+
176
+ def restore(self, *args, **kwargs):
177
+ raise NotImplementedError("CallbackHandler does not support restore")
178
+
179
+
180
+ @ocp.args.register_with_handler(CallbackHandler, for_save=True)
181
+ @dataclasses.dataclass
182
+ class CallbackSave(ocp.args.CheckpointArgs):
183
+ callback: Callback
184
+
185
+
186
+ @ocp.args.register_with_handler(CallbackHandler, for_restore=True)
187
+ class CallbackRestore(ocp.args.CheckpointArgs): ...
188
+
189
+
190
+ def _split_params(state: training_utils.TrainState) -> tuple[training_utils.TrainState, at.Params]:
191
+ if state.ema_params is not None:
192
+ params = state.ema_params
193
+ train_state = dataclasses.replace(state, ema_params=None)
194
+ else:
195
+ params = state.params
196
+ train_state = dataclasses.replace(state, params={})
197
+ return train_state, params
198
+
199
+
200
+ def _merge_params(train_state: training_utils.TrainState, params: dict[str, at.Params]) -> training_utils.TrainState:
201
+ # Revert the logic inside `_split_params`.
202
+ # If the restored train_state has an empty `params` dict, it means we're in the non-EMA case
203
+ # and we should restore the main training params.
204
+ if not train_state.params:
205
+ # Non-EMA case: The saved 'params' are the training weights.
206
+ return dataclasses.replace(train_state, params=params["params"])
207
+ else:
208
+ # EMA case: The saved 'params' are the ema_params, and train_state already has the training params.
209
+ return dataclasses.replace(train_state, ema_params=params["params"])
task 57/policy_code/src/b1k/training/config.py ADDED
@@ -0,0 +1,559 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Training configuration for BEHAVIOR-1K challenge.
2
+
3
+ Reference: https://github.com/Physical-Intelligence/openpi
4
+ """
5
+
6
+ import abc
7
+ from collections.abc import Sequence
8
+ import dataclasses
9
+ import difflib
10
+ import logging
11
+ import os
12
+ import pathlib
13
+ from typing import Any, Literal, List, Protocol, TypeAlias
14
+
15
+ import etils.epath as epath
16
+ import flax.nnx as nnx
17
+ from typing_extensions import override
18
+ import tyro
19
+
20
+ import openpi.shared.nnx_utils as nnx_utils
21
+
22
+ # Import from OpenPI
23
+ import openpi.models.model as _model
24
+ import openpi.models.pi0_config as pi0_config
25
+ import openpi.policies.aloha_policy as aloha_policy
26
+ import openpi.policies.droid_policy as droid_policy
27
+ import openpi.policies.libero_policy as libero_policy
28
+ import openpi.shared.download as _download
29
+ import openpi.training.droid_rlds_dataset as droid_rlds_dataset
30
+ import openpi.training.optimizer as _optimizer
31
+ import openpi.transforms as _transforms
32
+
33
+ # Import from B1K custom modules
34
+ from b1k.models import pi_behavior_config
35
+ from b1k.policies import b1k_policy
36
+ from b1k.shared import normalize as _normalize
37
+ from b1k.training import weight_loaders
38
+ from b1k import transforms as b1k_transforms
39
+
40
+ ModelType: TypeAlias = _model.ModelType
41
+ Filter: TypeAlias = nnx.filterlib.Filter
42
+
43
+
44
+ @dataclasses.dataclass(frozen=True)
45
+ class AssetsConfig:
46
+ """Determines the location of assets (e.g., norm stats) that will be used to set up the data pipeline.
47
+
48
+ These assets will be replicated inside the checkpoint under the `assets/asset_id` directory.
49
+ """
50
+ # Assets directory. If not provided, the config assets_dirs will be used.
51
+ assets_dir: str | None = None
52
+ # Asset id. If not provided, the repo id will be used.
53
+ asset_id: str | None = None
54
+
55
+
56
+ @dataclasses.dataclass(frozen=True)
57
+ class DataConfig:
58
+ # LeRobot repo id. If None, fake data will be created.
59
+ repo_id: str | None = None
60
+ # Directory within the assets directory containing the data assets.
61
+ asset_id: str | None = None
62
+ # Contains precomputed normalization stats. If None, normalization will not be performed.
63
+ norm_stats: dict[str, _transforms.NormStats] | None = None
64
+
65
+ # Used to adopt the inputs from a dataset specific format to a common format
66
+ repack_transforms: _transforms.Group = dataclasses.field(default_factory=_transforms.Group)
67
+ # Data transforms, typically include robot specific transformations.
68
+ data_transforms: _transforms.Group = dataclasses.field(default_factory=_transforms.Group)
69
+ # Model specific transforms. Will be applied after the data is normalized.
70
+ model_transforms: _transforms.Group = dataclasses.field(default_factory=_transforms.Group)
71
+
72
+ # If true, will use quantile normalization. Otherwise, normal z-score normalization will be used.
73
+ use_quantile_norm: bool = False
74
+ # If true, will use per-timestamp normalization for actions
75
+ use_per_timestamp_norm: bool = False
76
+
77
+ # Names of keys that will be used by the data loader to generate the action sequence.
78
+ action_sequence_keys: Sequence[str] = ("actions",)
79
+
80
+ # If true, will use the LeRobot dataset task to define the prompt (not used for PI_BEHAVIOR).
81
+ prompt_from_task: bool = False
82
+
83
+ # Only used for RLDS data loader.
84
+ rlds_data_dir: str | None = None
85
+
86
+ # Only used for B1K data loader.
87
+ behavior_dataset_root: str | None = None
88
+
89
+ # Action space for DROID dataset.
90
+ action_space: droid_rlds_dataset.DroidActionSpace | None = None
91
+ # Path to the data filter file for DROID dataset
92
+ filter_dict_path: str | None = None
93
+
94
+ # Episodes index to use for training
95
+ episodes_index: List[int] | None = None
96
+
97
+ # If set, load a local LeRobot v3 (2026 challenge format) dataset from here instead of the
98
+ # 2025 BehaviorLeRobotDataset.
99
+ v3_dataset_root: str | None = None
100
+
101
+
102
+ class GroupFactory(Protocol):
103
+ def __call__(self, model_config: _model.BaseModelConfig) -> _transforms.Group:
104
+ """Create a group."""
105
+
106
+
107
+ @dataclasses.dataclass(frozen=True)
108
+ class ModelTransformFactory(GroupFactory):
109
+ """Creates model transforms for B1K."""
110
+
111
+ default_prompt: str | None = None # Not used (task embeddings instead)
112
+
113
+ def __call__(self, model_config: _model.BaseModelConfig) -> _transforms.Group:
114
+ return _transforms.Group(
115
+ inputs=[
116
+ _transforms.ResizeImages(224, 224),
117
+ b1k_transforms.ComputeSubtaskStateFromMeta(dataset=None),
118
+ b1k_transforms.TaskIndexToTaskId(),
119
+ _transforms.PadStatesAndActions(model_config.action_dim),
120
+ ],
121
+ )
122
+
123
+
124
+ @dataclasses.dataclass(frozen=True)
125
+ class DataConfigFactory(abc.ABC):
126
+ # The LeRobot repo id.
127
+ repo_id: str = tyro.MISSING
128
+ # Determines how the assets will be loaded.
129
+ assets: AssetsConfig = dataclasses.field(default_factory=AssetsConfig)
130
+ # Base config that will be updated by the factory.
131
+ base_config: tyro.conf.Suppress[DataConfig | None] = None
132
+
133
+ @abc.abstractmethod
134
+ def create(self, assets_dirs: pathlib.Path, model_config: _model.BaseModelConfig) -> DataConfig:
135
+ """Create a data config."""
136
+
137
+ def create_base_config(self, assets_dirs: pathlib.Path, model_config: _model.BaseModelConfig) -> DataConfig:
138
+ repo_id = self.repo_id if self.repo_id is not tyro.MISSING else None
139
+ asset_id = self.assets.asset_id or repo_id
140
+ return dataclasses.replace(
141
+ self.base_config or DataConfig(),
142
+ repo_id=repo_id,
143
+ asset_id=asset_id,
144
+ norm_stats=self._load_norm_stats(epath.Path(self.assets.assets_dir or assets_dirs), asset_id),
145
+ use_quantile_norm=False, # Always use z-score normalization for B1K
146
+ )
147
+
148
+ def _load_norm_stats(self, assets_dir: epath.Path, asset_id: str | None) -> dict[str, _transforms.NormStats] | None:
149
+ if asset_id is None:
150
+ return None
151
+ try:
152
+ data_assets_dir = str(assets_dir / asset_id)
153
+ norm_stats = _normalize.load(_download.maybe_download(data_assets_dir))
154
+ logging.info(f"Loaded norm stats from {data_assets_dir}")
155
+ return norm_stats
156
+ except FileNotFoundError:
157
+ logging.info(f"Norm stats not found in {data_assets_dir}, skipping.")
158
+ return None
159
+
160
+
161
+ @dataclasses.dataclass(frozen=True)
162
+ class LeRobotB1KDataConfig(DataConfigFactory):
163
+ """Data configuration for BEHAVIOR-1K dataset."""
164
+
165
+ action_sequence_keys: Sequence[str] = ("action",)
166
+ use_delta_joint_actions: bool = False
167
+
168
+ # FAST auxiliary tokenization (only for PI_BEHAVIOR with use_fast_auxiliary)
169
+ use_fast_tokenization: bool = False
170
+
171
+ # Dataset keys of the head / left wrist / right wrist RGB streams (2025 defaults).
172
+ camera_keys: tuple[str, str, str] = (
173
+ "observation.images.rgb.head",
174
+ "observation.images.rgb.left_wrist",
175
+ "observation.images.rgb.right_wrist",
176
+ )
177
+ # If set, every sample is trained as this model task slot (see transforms.OverrideTaskIndex).
178
+ task_index_override: int | None = None
179
+
180
+ @override
181
+ def create(self, assets_dirs: pathlib.Path, model_config: _model.BaseModelConfig) -> DataConfig:
182
+ # Repack transforms for B1K observations
183
+ repack_mapping = {
184
+ "observation/egocentric_camera": self.camera_keys[0],
185
+ "observation/wrist_image_left": self.camera_keys[1],
186
+ "observation/wrist_image_right": self.camera_keys[2],
187
+ "observation/state": "observation.state",
188
+ "actions": "action",
189
+ "task_index": "task_index", # Always preserve task_index
190
+ "timestamp": "timestamp", # Preserve timestamp for subtask state computation
191
+ "episode_index": "episode_index", # Preserve episode_index for episode length lookup
192
+ "index": "index", # Preserve index
193
+ }
194
+
195
+ repack_inputs = [_transforms.RepackTransform(repack_mapping)]
196
+ if self.task_index_override is not None:
197
+ repack_inputs.append(b1k_transforms.OverrideTaskIndex(self.task_index_override))
198
+ repack_transform = _transforms.Group(inputs=repack_inputs)
199
+
200
+ # Prepare data for policy training
201
+ data_transforms = _transforms.Group(
202
+ inputs=[b1k_policy.B1kInputs(model_type=model_config.model_type)],
203
+ outputs=[b1k_policy.B1kOutputs()],
204
+ )
205
+
206
+ # Delta action transforms
207
+ if self.use_delta_joint_actions:
208
+ delta_action_mask = _transforms.make_bool_mask(-3, 3, -1, 7, -1, 7, -1)
209
+ else:
210
+ delta_action_mask = _transforms.make_bool_mask(-23)
211
+
212
+ data_transforms = data_transforms.push(
213
+ inputs=[_transforms.DeltaActions(delta_action_mask)],
214
+ outputs=[_transforms.AbsoluteActions(delta_action_mask)],
215
+ )
216
+
217
+ # Model transforms (subtask state, task ID, padding)
218
+ model_transforms = ModelTransformFactory()(model_config)
219
+
220
+ # FAST tokenization (if enabled for PI_BEHAVIOR)
221
+ if self.use_fast_tokenization and hasattr(model_config, 'use_fast_auxiliary') and model_config.use_fast_auxiliary:
222
+ asset_id = self.assets.asset_id or self.repo_id
223
+ tokenizer_path = pathlib.Path(self.assets.assets_dir or assets_dirs) / asset_id / "fast_tokenizer"
224
+
225
+ # Get base config to access norm_stats
226
+ base_config = self.create_base_config(assets_dirs, model_config)
227
+
228
+ # Only add transform if tokenizer directory exists
229
+ if tokenizer_path.exists():
230
+ model_transforms = model_transforms.push(
231
+ inputs=[b1k_transforms.TokenizeFASTActions(
232
+ tokenizer_path=str(tokenizer_path),
233
+ encoded_dim_ranges=model_config.get_fast_dim_ranges(),
234
+ max_fast_tokens=model_config.max_fast_tokens,
235
+ norm_stats=base_config.norm_stats,
236
+ use_per_timestamp=base_config.use_per_timestamp_norm,
237
+ )],
238
+ )
239
+ else:
240
+ logging.warning(
241
+ f"FAST tokenizer not found at {tokenizer_path}. "
242
+ "FAST auxiliary training will be disabled (inference mode)."
243
+ )
244
+
245
+ return dataclasses.replace(
246
+ self.create_base_config(assets_dirs, model_config),
247
+ repack_transforms=repack_transform,
248
+ data_transforms=data_transforms,
249
+ model_transforms=model_transforms,
250
+ action_sequence_keys=self.action_sequence_keys,
251
+ )
252
+
253
+
254
+ @dataclasses.dataclass(frozen=True)
255
+ class TrainConfig:
256
+ # Name of the config. Must be unique. Will be used to reference this config.
257
+ name: tyro.conf.Suppress[str]
258
+ # Project name.
259
+ project_name: str = "B1K"
260
+ # Experiment name. Will be used to name the metadata and checkpoint directories.
261
+ exp_name: str = tyro.MISSING
262
+
263
+ # Defines the model config (PI_BEHAVIOR only for B1K).
264
+ model: _model.BaseModelConfig = dataclasses.field(default_factory=pi_behavior_config.PiBehaviorConfig)
265
+
266
+ # A weight loader can optionally load (possibly partial) weights from disk after the model is initialized.
267
+ weight_loader: weight_loaders.WeightLoader = dataclasses.field(default_factory=weight_loaders.NoOpWeightLoader)
268
+
269
+ # Note: PyTorch support removed - JAX only
270
+
271
+ lr_schedule: _optimizer.LRScheduleConfig = dataclasses.field(default_factory=_optimizer.CosineDecaySchedule)
272
+ optimizer: _optimizer.OptimizerConfig = dataclasses.field(default_factory=_optimizer.AdamW)
273
+ ema_decay: float | None = 0.99
274
+
275
+ # Specifies which weights should be frozen.
276
+ freeze_filter: tyro.conf.Suppress[Filter] = dataclasses.field(default_factory=nnx.Nothing)
277
+
278
+ # Determines the data to be trained on.
279
+ data: DataConfigFactory = dataclasses.field(default_factory=LeRobotB1KDataConfig)
280
+
281
+ # Base directory for config assets (e.g., norm stats).
282
+ assets_base_dir: str = "./assets"
283
+ # Base directory for checkpoints.
284
+ checkpoint_base_dir: str = "./checkpoints"
285
+
286
+ # Random seed that will be used by random generators during training.
287
+ seed: int | None = None
288
+ # Global batch size.
289
+ batch_size: int = 32
290
+ # Number of workers to use for the data loader.
291
+ num_workers: int = 2
292
+ # Number of train steps (batches) to run.
293
+ num_train_steps: int = 30_000
294
+
295
+ # How often (in steps) to log training metrics.
296
+ log_interval: int = 100
297
+ # How often (in steps) to save checkpoints.
298
+ save_interval: int = 1000
299
+ # If set, any existing checkpoints matching step % keep_period == 0 will not be deleted.
300
+ keep_period: int | None = 5000
301
+
302
+ # If true, will overwrite the checkpoint directory if it already exists.
303
+ overwrite: bool = False
304
+ # If true, will resume training from the last checkpoint.
305
+ resume: bool = False
306
+
307
+ # If true, will enable wandb logging.
308
+ wandb_enabled: bool = True
309
+
310
+ # Used to pass metadata to the policy server.
311
+ policy_metadata: dict[str, Any] | None = None
312
+
313
+ # FSDP configuration for model sharding across devices.
314
+ fsdp_devices: int = 1
315
+
316
+ # Validation configuration
317
+ val_log_interval: int = 100
318
+ val_batch_size: int | None = None
319
+ val_num_batches: int = 10
320
+ val_repo_id: str | None = None
321
+ val_episodes_index: List[int] | None = None
322
+
323
+ # Number of flow matching samples per training step
324
+ num_flow_samples: int = 1
325
+
326
+ @property
327
+ def assets_dirs(self) -> pathlib.Path:
328
+ """Get the assets directory for this config."""
329
+ return (pathlib.Path(self.assets_base_dir) / self.name).resolve()
330
+
331
+ @property
332
+ def checkpoint_dir(self) -> pathlib.Path:
333
+ """Get the checkpoint directory for this config."""
334
+ if not self.exp_name:
335
+ raise ValueError("--exp_name must be set")
336
+ return (pathlib.Path(self.checkpoint_base_dir) / self.name / self.exp_name).resolve()
337
+
338
+ @property
339
+ def trainable_filter(self) -> nnx.filterlib.Filter:
340
+ """Get the filter for the trainable parameters."""
341
+ return nnx.All(nnx.Param, nnx.Not(self.freeze_filter))
342
+
343
+ def __post_init__(self) -> None:
344
+ if self.resume and self.overwrite:
345
+ raise ValueError("Cannot resume and overwrite at the same time.")
346
+
347
+
348
+ # B1K Training Configurations
349
+ _CONFIGS = [
350
+ TrainConfig(
351
+ name="pi_behavior_b1k_fast",
352
+ exp_name="openpi",
353
+ project_name="B1K",
354
+ model=pi_behavior_config.PiBehaviorConfig(
355
+ action_horizon=30,
356
+ action_dim=32,
357
+ use_correlated_noise=True,
358
+ correlation_beta=0.5,
359
+ # FAST auxiliary training
360
+ use_fast_auxiliary=True,
361
+ fast_loss_weight=0.05,
362
+ fast_encoded_dims="0:6,7:23", # Encode 22 dimensions
363
+ fast_vocab_size=1024,
364
+ max_fast_tokens=200,
365
+ use_kv_transform=True,
366
+ use_knowledge_insulation=False,
367
+ subtask_loss_weight=0.1,
368
+ freeze_vision_backbone=True,
369
+ ),
370
+ data=LeRobotB1KDataConfig(
371
+ repo_id="IliaLarchenko/behavior_224_rgb",
372
+ base_config=DataConfig(
373
+ prompt_from_task=False, # No text prompts for PI_BEHAVIOR
374
+ behavior_dataset_root="~/data/behavior_224_rgb",
375
+ use_per_timestamp_norm=True, # Enable per-timestamp normalization
376
+ ),
377
+ use_delta_joint_actions=True,
378
+ use_fast_tokenization=True, # Enable FAST tokenization in data pipeline
379
+ ),
380
+ lr_schedule=_optimizer.CosineDecaySchedule(
381
+ warmup_steps=1000,
382
+ peak_lr=1e-4,
383
+ decay_steps=20_000,
384
+ decay_lr=1e-5,
385
+ ),
386
+ num_flow_samples=15,
387
+ weight_loader=weight_loaders.PiBehaviorWeightLoader("gs://openpi-assets/checkpoints/pi05_base/params"),
388
+ num_train_steps=200_000,
389
+ assets_base_dir="./outputs/assets",
390
+ checkpoint_base_dir="./outputs/checkpoints",
391
+ num_workers=80,
392
+ save_interval=500,
393
+ keep_period=2000,
394
+ ),
395
+ ]
396
+
397
+ # Fine-tune of the 2025 1st-place checkpoint_3 (IliaLarchenko/behavior_submission) on 2026 task 57,
398
+ # sweeping_garage, with LoRA on both Gemma towers. Task 57 is trained in model slot 36 (clean_a_patio,
399
+ # the closest checkpoint_3 task) so every parameter shape matches the checkpoint. Norm stats and the
400
+ # FAST tokenizer are the checkpoint's own (assets/IliaLarchenko/behavior_224_rgb), so the inputs the
401
+ # model sees are normalized exactly as in its original training.
402
+ _WINNER_CKPT = "/workspace/models/b1k_2025_winner/checkpoint_3"
403
+ _SWEEP_LORA_MODEL = pi_behavior_config.PiBehaviorConfig(
404
+ paligemma_variant="gemma_2b_lora",
405
+ action_expert_variant="gemma_300m_lora",
406
+ action_horizon=30,
407
+ action_dim=32,
408
+ use_correlated_noise=True,
409
+ correlation_beta=0.5,
410
+ use_fast_auxiliary=True,
411
+ fast_loss_weight=0.05,
412
+ fast_encoded_dims="0:6,7:23",
413
+ fast_vocab_size=1024,
414
+ max_fast_tokens=200,
415
+ use_kv_transform=True,
416
+ use_knowledge_insulation=False,
417
+ subtask_loss_weight=0.1,
418
+ freeze_vision_backbone=True,
419
+ )
420
+ _CONFIGS.append(
421
+ TrainConfig(
422
+ name="pi_behavior_sweep_lora",
423
+ exp_name="sweeping_garage_from_winner_ckpt3",
424
+ model=_SWEEP_LORA_MODEL,
425
+ data=LeRobotB1KDataConfig(
426
+ repo_id="behavior-1k/2026-challenge-demos",
427
+ assets=AssetsConfig(assets_dir=f"{_WINNER_CKPT}/assets", asset_id="IliaLarchenko/behavior_224_rgb"),
428
+ base_config=DataConfig(
429
+ prompt_from_task=False,
430
+ v3_dataset_root="/workspace/2026-challenge-demos",
431
+ episodes_index=list(range(200)),
432
+ use_per_timestamp_norm=True,
433
+ ),
434
+ camera_keys=(
435
+ "observation.rgb.zed_link_camera_0",
436
+ "observation.rgb.left_realsense_link_camera_0",
437
+ "observation.rgb.right_realsense_link_camera_0",
438
+ ),
439
+ task_index_override=36,
440
+ use_delta_joint_actions=True,
441
+ use_fast_tokenization=True,
442
+ ),
443
+ weight_loader=weight_loaders.PiBehaviorWeightLoader(f"{_WINNER_CKPT}/params"),
444
+ # Freeze both Gemma towers except their LoRA adapters, and the SigLIP vision tower.
445
+ # Small heads (task/stage embeddings, fusion, kv_transform, action/time projections) stay trainable.
446
+ freeze_filter=nnx.Any(
447
+ nnx.All(nnx_utils.PathRegex(".*llm.*"), nnx.Not(nnx_utils.PathRegex(".*lora.*"))),
448
+ nnx_utils.PathRegex(".*img.*"),
449
+ ),
450
+ ema_decay=None,
451
+ lr_schedule=_optimizer.CosineDecaySchedule(
452
+ warmup_steps=500,
453
+ peak_lr=5e-5,
454
+ decay_steps=12_000,
455
+ decay_lr=5e-6,
456
+ ),
457
+ num_flow_samples=15,
458
+ batch_size=32,
459
+ num_train_steps=12_000,
460
+ assets_base_dir="./outputs/assets",
461
+ checkpoint_base_dir="/workspace/b1k_winner_ft/outputs/checkpoints",
462
+ num_workers=12,
463
+ save_interval=1000,
464
+ keep_period=3000,
465
+ val_log_interval=0,
466
+ wandb_enabled=False,
467
+ )
468
+ )
469
+
470
+ # Continuation: start from the 12k fine-tune's final params with a fresh, gentler schedule (re-warm to 2e-5).
471
+ _CONFIGS.append(
472
+ dataclasses.replace(
473
+ _CONFIGS[-1],
474
+ name="pi_behavior_sweep_lora_cont",
475
+ exp_name="sweeping_garage_cont8k",
476
+ weight_loader=weight_loaders.PiBehaviorWeightLoader(
477
+ "/workspace/b1k_winner_ft/outputs/checkpoints/pi_behavior_sweep_lora/sweeping_garage_from_winner_ckpt3/11999/params"
478
+ ),
479
+ lr_schedule=_optimizer.CosineDecaySchedule(
480
+ warmup_steps=200,
481
+ peak_lr=2e-5,
482
+ decay_steps=8_000,
483
+ decay_lr=2e-6,
484
+ ),
485
+ num_train_steps=8_000,
486
+ )
487
+ )
488
+
489
+ # Expert fine-tune: from the continuation's final params, additionally unfreeze the full action expert
490
+ # (Gemma-300M weights, the "_1"-suffixed params in the llm). VLM backbone and SigLIP stay frozen.
491
+ _CONFIGS.append(
492
+ dataclasses.replace(
493
+ _CONFIGS[-1],
494
+ name="pi_behavior_sweep_expert",
495
+ exp_name="sweeping_garage_expert",
496
+ weight_loader=weight_loaders.PiBehaviorWeightLoader(
497
+ "/workspace/b1k_winner_ft/outputs/checkpoints/pi_behavior_sweep_lora_cont/sweeping_garage_cont8k/7999/params"
498
+ ),
499
+ freeze_filter=nnx.Any(
500
+ nnx.All(
501
+ nnx_utils.PathRegex(".*llm.*"),
502
+ nnx.Not(nnx_utils.PathRegex(".*lora.*")),
503
+ nnx.Not(nnx_utils.PathRegex(".*llm/.*_1(/.*)?")),
504
+ ),
505
+ nnx_utils.PathRegex(".*img.*"),
506
+ ),
507
+ lr_schedule=_optimizer.CosineDecaySchedule(
508
+ warmup_steps=300,
509
+ peak_lr=2e-5,
510
+ decay_steps=7_500,
511
+ decay_lr=2e-6,
512
+ ),
513
+ num_train_steps=7_500,
514
+ )
515
+ )
516
+
517
+ # Follow-up rounds (driven by rounds.sh): expert-unfrozen fine-tune from any earlier params, with a fresh cosine
518
+ # schedule. B1K_INIT_PARAMS / B1K_STEPS / B1K_PEAK_LR only matter for training; serving ignores them.
519
+ _next_steps = int(os.environ.get("B1K_STEPS", "3500"))
520
+ _CONFIGS.append(
521
+ dataclasses.replace(
522
+ _CONFIGS[-1],
523
+ name="pi_behavior_sweep_next",
524
+ exp_name="sweeping_garage_next",
525
+ weight_loader=weight_loaders.PiBehaviorWeightLoader(
526
+ os.environ.get(
527
+ "B1K_INIT_PARAMS",
528
+ "/workspace/b1k_winner_ft/outputs/checkpoints/pi_behavior_sweep_expert/sweeping_garage_expert/7499/params",
529
+ )
530
+ ),
531
+ lr_schedule=_optimizer.CosineDecaySchedule(
532
+ warmup_steps=200,
533
+ peak_lr=float(os.environ.get("B1K_PEAK_LR", "1e-5")),
534
+ decay_steps=_next_steps,
535
+ decay_lr=1e-6,
536
+ ),
537
+ num_train_steps=_next_steps,
538
+ keep_period=None, # keep only the latest checkpoint (250 GB /workspace limit)
539
+ )
540
+ )
541
+
542
+ if len({config.name for config in _CONFIGS}) != len(_CONFIGS):
543
+ raise ValueError("Config names must be unique.")
544
+ _CONFIGS_DICT = {config.name: config for config in _CONFIGS}
545
+
546
+
547
+ def cli() -> TrainConfig:
548
+ return tyro.extras.overridable_config_cli({k: (k, v) for k, v in _CONFIGS_DICT.items()})
549
+
550
+
551
+ def get_config(config_name: str) -> TrainConfig:
552
+ """Get a config by name."""
553
+ if config_name not in _CONFIGS_DICT:
554
+ closest = difflib.get_close_matches(config_name, _CONFIGS_DICT.keys(), n=1, cutoff=0.0)
555
+ closest_str = f" Did you mean '{closest[0]}'? " if closest else ""
556
+ raise ValueError(f"Config '{config_name}' not found.{closest_str}")
557
+
558
+ return _CONFIGS_DICT[config_name]
559
+
task 57/policy_code/src/b1k/training/data_loader.py ADDED
@@ -0,0 +1,285 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Data loading for BEHAVIOR-1K dataset.
2
+
3
+ Reference: https://github.com/wensi-ai/openpi/tree/behavior
4
+ """
5
+
6
+ import logging
7
+ import time
8
+
9
+ # Import all base data loading from OpenPI
10
+ from openpi.training.data_loader import (
11
+ Dataset,
12
+ IterableDataset,
13
+ DataLoader,
14
+ TransformedDataset,
15
+ IterableTransformedDataset,
16
+ FakeDataset,
17
+ TorchDataLoader,
18
+ RLDSDataLoader,
19
+ create_torch_dataset,
20
+ create_rlds_dataset,
21
+ transform_iterable_dataset,
22
+ create_data_loader,
23
+ create_torch_data_loader,
24
+ create_rlds_data_loader,
25
+ )
26
+
27
+ import openpi.training.config as _config
28
+ import openpi.transforms as _transforms
29
+
30
+ from b1k.models.observation import Observation
31
+ from b1k.transforms_normalize import NormalizeWithPerTimestamp
32
+
33
+
34
+ class DataLoaderImpl(DataLoader):
35
+ """Custom DataLoader using our Observation with fast_tokens."""
36
+
37
+ def __init__(self, data_config: _config.DataConfig, data_loader: TorchDataLoader | RLDSDataLoader):
38
+ self._data_config = data_config
39
+ self._data_loader = data_loader
40
+
41
+ def data_config(self) -> _config.DataConfig:
42
+ return self._data_config
43
+
44
+ def __iter__(self):
45
+ for batch in self._data_loader:
46
+ yield Observation.from_dict(batch), batch["actions"]
47
+
48
+
49
+ def create_behavior_dataset(data_config: _config.DataConfig, action_horizon: int, seed: int | None = None) -> Dataset:
50
+ """Create a BEHAVIOR-1K dataset for training.
51
+
52
+ Uses OmniGibson's BehaviorLeRobotDataset for efficient loading of BEHAVIOR-1K data.
53
+
54
+ Args:
55
+ data_config: Data configuration
56
+ action_horizon: Action horizon for delta timestamps
57
+ seed: Random seed for shuffling. If None, uses random seed based on current time.
58
+
59
+ Returns:
60
+ Dataset instance with BEHAVIOR-1K data
61
+ """
62
+ if data_config.v3_dataset_root is not None:
63
+ return create_lerobot_v3_dataset(data_config, action_horizon)
64
+ from omnigibson.learning.datas.lerobot_dataset import BehaviorLeRobotDataset
65
+
66
+ # Use random seed if not provided
67
+ if seed is None:
68
+ seed = int(time.time() * 1000) % (2**32)
69
+ logging.info(f"Using random seed for BehaviorLeRobotDataset: {seed}")
70
+ tasks = [
71
+ "picking_up_trash", # difficulty: 2
72
+ "putting_away_Halloween_decorations", # difficulty: 3
73
+ "cleaning_up_plates_and_food", # difficulty: 3.5
74
+ "setting_mousetraps", # difficulty: 2
75
+ "hiding_Easter_eggs", # difficulty: 2
76
+ "set_up_a_coffee_station_in_your_kitchen", # difficulty: 3
77
+ "putting_dishes_away_after_cleaning", # difficulty: 3
78
+ "preparing_lunch_box", # difficulty: 3
79
+ "loading_the_car", # difficulty: 3.5
80
+ "carrying_in_groceries", # difficulty: 3.5
81
+ "turning_on_radio", # difficulty: 1
82
+ "picking_up_toys", # difficulty: 3.5
83
+ "can_meat", # difficulty: 3.5
84
+ "rearranging_kitchen_furniture", # difficulty: 3
85
+ "putting_up_Christmas_decorations_inside", # difficulty: 2
86
+ "bringing_in_wood", # difficulty: 1.5
87
+ "moving_boxes_to_storage", # difficulty: 1.5
88
+ "bringing_water", # difficulty: 1.5
89
+ "tidying_bedroom", # difficulty: 2
90
+ "outfit_a_basic_toolbox", # difficulty: 2,
91
+ "sorting_vegetables",
92
+ "collecting_childrens_toys",
93
+ "putting_shoes_on_rack",
94
+ "boxing_books_up_for_storage",
95
+ "storing_food",
96
+ "clearing_food_from_table_into_fridge",
97
+ "assembling_gift_baskets",
98
+ "sorting_household_items",
99
+ "getting_organized_for_work",
100
+ "clean_up_your_desk",
101
+ "setting_the_fire",
102
+ "clean_boxing_gloves",
103
+ "wash_a_baseball_cap",
104
+ "wash_dog_toys",
105
+ "hanging_pictures",
106
+ "attach_a_camera_to_a_tripod",
107
+ "clean_a_patio",
108
+ "clean_a_trumpet",
109
+ "spraying_for_bugs",
110
+ "spraying_fruit_trees",
111
+ "make_microwave_popcorn",
112
+ "cook_cabbage",
113
+ "make_pizza",
114
+ "chop_an_onion",
115
+ "slicing_vegetables",
116
+ "chopping_wood",
117
+ "canning_food",
118
+ "cook_hot_dogs",
119
+ "cook_bacon",
120
+ "freeze_pies",
121
+ ]
122
+
123
+ dataset = BehaviorLeRobotDataset(
124
+ repo_id=data_config.repo_id,
125
+ root=data_config.behavior_dataset_root,
126
+ tasks=tasks,
127
+ modalities=["rgb"],
128
+ local_only=True,
129
+ delta_timestamps={
130
+ key: [t / 30.0 for t in range(action_horizon)] for key in data_config.action_sequence_keys
131
+ },
132
+ episodes=data_config.episodes_index,
133
+ chunk_streaming_using_keyframe=False,
134
+ shuffle=True,
135
+ seed=seed,
136
+ )
137
+
138
+ if data_config.prompt_from_task:
139
+ dataset = TransformedDataset(dataset, [_transforms.PromptFromLeRobotTask(dataset.meta.tasks)])
140
+
141
+ return dataset
142
+
143
+
144
+ def create_lerobot_v3_dataset(data_config: _config.DataConfig, action_horizon: int) -> Dataset:
145
+ """Local 2026-format (LeRobot v3) BEHAVIOR dataset, loaded with openpi's LeRobot compat layer."""
146
+ import openpi.training.lerobot_compat as _lerobot_compat
147
+
148
+ dataset = _lerobot_compat.LeRobotDataset(
149
+ repo_id=data_config.repo_id,
150
+ root=data_config.v3_dataset_root,
151
+ delta_timestamps={key: [t / 30.0 for t in range(action_horizon)] for key in data_config.action_sequence_keys},
152
+ tolerance_s=5e-4,
153
+ episodes=data_config.episodes_index,
154
+ )
155
+ return dataset
156
+
157
+
158
+ def _v3_episode_lengths(root: str) -> dict[int, int]:
159
+ import glob
160
+
161
+ import pandas as pd
162
+
163
+ files = sorted(glob.glob(f"{root}/meta/episodes/*/*.parquet"))
164
+ df = pd.concat([pd.read_parquet(f, columns=["episode_index", "length"]) for f in files])
165
+ return {int(e): int(n) for e, n in zip(df["episode_index"], df["length"])}
166
+
167
+
168
+ def transform_dataset(dataset: Dataset, data_config: _config.DataConfig, *, skip_norm_stats: bool = False) -> Dataset:
169
+ """Transform dataset with B1K-specific per-timestamp normalization support.
170
+
171
+ CRITICAL: This overrides wensi-ai's transform_dataset to pass use_per_timestamp to Normalize.
172
+ wensi-ai's version doesn't support per-timestamp normalization which causes huge action losses!
173
+ """
174
+ norm_stats = {}
175
+ if data_config.repo_id != "fake" and not skip_norm_stats:
176
+ if data_config.norm_stats is None:
177
+ raise ValueError(
178
+ "Normalization stats not found. "
179
+ "Make sure to run `scripts/compute_norm_stats.py --config-name=<your-config>`."
180
+ )
181
+ norm_stats = data_config.norm_stats
182
+
183
+ # Build transform list
184
+ transforms_list = [
185
+ *data_config.repack_transforms.inputs,
186
+ *data_config.data_transforms.inputs,
187
+ # Use custom Normalize with per-timestamp support (wensi-ai's doesn't have it!)
188
+ NormalizeWithPerTimestamp(
189
+ norm_stats,
190
+ use_quantiles=data_config.use_quantile_norm,
191
+ use_per_timestamp=data_config.use_per_timestamp_norm # CRITICAL: Per-timestamp normalization!
192
+ ),
193
+ ]
194
+
195
+ # Add subtask state computation for PI_BEHAVIOR models (needs dataset reference)
196
+ model_transforms = []
197
+ for transform in data_config.model_transforms.inputs:
198
+ # ComputeSubtaskStateFromMeta needs dataset reference to access episode lengths
199
+ if hasattr(transform, '__class__') and transform.__class__.__name__ == 'ComputeSubtaskStateFromMeta':
200
+ # Replace placeholder with dataset-aware version
201
+ from b1k import transforms as b1k_transforms
202
+ if data_config.v3_dataset_root is not None:
203
+ lengths = _v3_episode_lengths(data_config.v3_dataset_root)
204
+ model_transforms.append(b1k_transforms.ComputeSubtaskStateFromLengths(episode_lengths=lengths))
205
+ logging.info(f"Added ComputeSubtaskStateFromLengths ({len(lengths)} episodes)")
206
+ elif hasattr(dataset, 'meta') and hasattr(dataset.meta, 'episodes'):
207
+ model_transforms.append(b1k_transforms.ComputeSubtaskStateFromMeta(dataset=dataset))
208
+ logging.info("Added dataset-aware ComputeSubtaskStateFromMeta transform")
209
+ else:
210
+ logging.warning("Skipping subtask state computation - dataset has no meta.episodes")
211
+ else:
212
+ model_transforms.append(transform)
213
+
214
+ transforms_list.extend(model_transforms)
215
+
216
+ return TransformedDataset(dataset, transforms_list)
217
+
218
+
219
+ def extract_episode_lengths_from_dataset(dataset) -> dict[int, float]:
220
+ """Extract episode lengths from B1K dataset metadata.
221
+
222
+ Args:
223
+ dataset: BehaviorLeRobotDataset instance
224
+
225
+ Returns:
226
+ Dictionary mapping episode_index to episode_length (in frames)
227
+
228
+ Raises:
229
+ ValueError: If dataset doesn't have required metadata
230
+ """
231
+ if not hasattr(dataset, 'episode_data_index'):
232
+ raise ValueError("Dataset must have episode_data_index attribute")
233
+
234
+ episode_data_index = dataset.episode_data_index
235
+ if 'to' not in episode_data_index or 'from' not in episode_data_index:
236
+ raise ValueError("episode_data_index must have 'to' and 'from' keys")
237
+
238
+ episode_to = episode_data_index['to']
239
+ episode_from = episode_data_index['from']
240
+ episodes = dataset.episodes
241
+
242
+ episode_lengths = {}
243
+ for i, episode_index in enumerate(episodes):
244
+ if i < len(episode_to) and i < len(episode_from):
245
+ episode_length = episode_to[i] - episode_from[i]
246
+ episode_lengths[episode_index] = float(episode_length)
247
+
248
+ logging.info(f"Extracted {len(episode_lengths)} episode lengths from dataset")
249
+ return episode_lengths
250
+
251
+
252
+ def create_behavior_data_loader(
253
+ config: _config.TrainConfig,
254
+ *,
255
+ sharding=None,
256
+ shuffle: bool = False,
257
+ num_batches: int | None = None,
258
+ skip_norm_stats: bool = False,
259
+ ) -> DataLoader:
260
+ """Create a data loader for BEHAVIOR-1K training."""
261
+ import jax
262
+ import time
263
+
264
+ data_config = config.data.create(config.assets_dirs, config.model)
265
+
266
+ # Use random seed if not provided
267
+ seed = config.seed
268
+ if seed is None:
269
+ seed = int(time.time() * 1000) % (2**32)
270
+ logging.info(f"Using random seed: {seed}")
271
+
272
+ dataset = create_behavior_dataset(data_config, action_horizon=config.model.action_horizon, seed=seed)
273
+ dataset = transform_dataset(dataset, data_config, skip_norm_stats=skip_norm_stats)
274
+
275
+ data_loader = TorchDataLoader(
276
+ dataset,
277
+ local_batch_size=config.batch_size // jax.process_count(),
278
+ sharding=sharding,
279
+ shuffle=shuffle,
280
+ num_batches=num_batches,
281
+ num_workers=config.num_workers,
282
+ seed=seed,
283
+ )
284
+
285
+ return DataLoaderImpl(data_config, data_loader)
task 57/policy_code/src/b1k/training/weight_loaders.py ADDED
@@ -0,0 +1,87 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Weight loaders for PI_BEHAVIOR model initialization from Pi05 checkpoints.
2
+
3
+ Reference: https://github.com/Physical-Intelligence
4
+ """
5
+
6
+ import dataclasses
7
+ import logging
8
+ import re
9
+
10
+ import flax.traverse_util
11
+ import numpy as np
12
+ import orbax.checkpoint as ocp
13
+
14
+ import openpi.shared.array_typing as at
15
+ import openpi.shared.download as download
16
+
17
+ # Re-export base loaders from OpenPI
18
+ from openpi.training.weight_loaders import (
19
+ WeightLoader,
20
+ NoOpWeightLoader,
21
+ CheckpointWeightLoader,
22
+ _merge_params,
23
+ )
24
+
25
+ logger = logging.getLogger(__name__)
26
+
27
+
28
+ @dataclasses.dataclass(frozen=True)
29
+ class PiBehaviorWeightLoader(WeightLoader):
30
+ """Loads checkpoints for PI_BEHAVIOR model.
31
+
32
+ Automatically detects:
33
+ - Pi05 checkpoint: Loads weights, preserves new PI_BEHAVIOR parameters
34
+ - PI_BEHAVIOR checkpoint: Loads all weights directly
35
+ """
36
+
37
+ params_path: str
38
+
39
+ def load(self, params: at.Params) -> at.Params:
40
+ # Load checkpoint
41
+ params_path = download.maybe_download(self.params_path)
42
+
43
+ # Restore as host numpy arrays; newer orbax refuses a bare restore() without restore args.
44
+ import openpi.models.model as _openpi_model
45
+ restored = _openpi_model.restore_params(params_path, restore_type=np.ndarray)
46
+
47
+ # Handle nested 'params' key (from some checkpoint formats)
48
+ if isinstance(restored, dict) and "params" in restored:
49
+ loaded_params = restored["params"]
50
+ else:
51
+ loaded_params = restored
52
+
53
+ # Remove 'value' suffixes (from nnx.State format)
54
+ flat_params = flax.traverse_util.flatten_dict(loaded_params)
55
+ if all(kp[-1] == "value" for kp in flat_params if len(kp) > 0):
56
+ flat_params = {kp[:-1]: v for kp, v in flat_params.items()}
57
+ loaded_params = flax.traverse_util.unflatten_dict(flat_params)
58
+
59
+ # Detect checkpoint type
60
+ has_task_embeddings = 'task_embeddings' in loaded_params
61
+
62
+ if has_task_embeddings:
63
+ # Loading PI_BEHAVIOR checkpoint - load ALL weights from checkpoint
64
+ logging.info("Loading PI_BEHAVIOR checkpoint (all weights will be loaded)")
65
+ # LoRA adapters (lora_a/lora_b) are not in a full-weight checkpoint; they keep their init
66
+ # (lora_b starts at zero, so the model initially matches the checkpoint exactly).
67
+ return _merge_params(loaded_params, params, missing_regex=".*lora.*")
68
+ else:
69
+ # Loading Pi05 checkpoint - preserve new PI_BEHAVIOR-specific parameters
70
+ logging.info("Loading Pi05 checkpoint (new PI_BEHAVIOR parameters will use random init)")
71
+
72
+ # These parameters are NEW in PI_BEHAVIOR (not in Pi05), so keep them from params (random init)
73
+ missing_regex = (
74
+ ".*task_embeddings.*|"
75
+ ".*stage_pred_from_vlm.*|"
76
+ ".*task_stage_embeddings.*|"
77
+ ".*gate_sincos.*|"
78
+ ".*gate_task_stage.*|"
79
+ ".*gate_task.*|"
80
+ ".*fusion_layer.*|"
81
+ ".*stage_projection.*|"
82
+ ".*task_subtask_fusion.*|"
83
+ ".*fast_token_embedding.*|"
84
+ ".*fast_token_proj.*|"
85
+ ".*kv_transform.*"
86
+ )
87
+ return _merge_params(loaded_params, params, missing_regex=missing_regex)
task 57/policy_code/src/b1k/transforms.py ADDED
@@ -0,0 +1,352 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Data transforms for BEHAVIOR-1K dataset.
2
+
3
+ Standard transforms imported from OpenPI.
4
+ B1K-specific: TaskIndexToTaskId, ComputeSubtaskStateFromMeta, TokenizeFASTActions
5
+
6
+ Reference: https://github.com/wensi-ai/openpi/tree/behavior
7
+ """
8
+
9
+ import dataclasses
10
+ import logging
11
+ import numpy as np
12
+
13
+ # Import all standard transforms from OpenPI
14
+ from openpi.transforms import (
15
+ DataTransformFn,
16
+ DataDict,
17
+ Group,
18
+ CompositeTransform,
19
+ compose,
20
+ RepackTransform,
21
+ Normalize,
22
+ Unnormalize,
23
+ ResizeImages,
24
+ SubsampleActions,
25
+ DeltaActions,
26
+ AbsoluteActions,
27
+ PadStatesAndActions,
28
+ PromptFromLeRobotTask, # Not used for PI_BEHAVIOR but kept for compatibility
29
+ flatten_dict,
30
+ unflatten_dict,
31
+ transform_dict,
32
+ apply_tree,
33
+ pad_to_dim,
34
+ make_bool_mask,
35
+ )
36
+
37
+ from b1k.models.pi_behavior_config import TASK_NUM_STAGES
38
+ from b1k.shared.normalize import NormStats
39
+
40
+
41
+ @dataclasses.dataclass(frozen=True)
42
+ class TaskIndexToTaskId(DataTransformFn):
43
+ """Directly converts task_index to task_id for PI_BEHAVIOR model.
44
+
45
+ PI_BEHAVIOR uses task embeddings instead of text prompts. This transform
46
+ converts the dataset's task_index to task_id and prepares tokenized_prompt
47
+ as [task_id, subtask_state] for the model.
48
+
49
+ Assumes:
50
+ - data["task_index"] exists (from dataset)
51
+ - data["subtask_state"] exists (computed by ComputeSubtaskStateFromMeta)
52
+
53
+ Creates:
54
+ - data["tokenized_prompt"]: np.array([task_id, subtask_state], dtype=int32)
55
+ - data["tokenized_prompt_mask"]: np.array([True, True], dtype=bool)
56
+ """
57
+
58
+ # Optional task remapping (dataset task_index → model task_id)
59
+ # If None, assumes direct mapping (task_index == task_id)
60
+ task_mapping: dict[int, int] | None = None
61
+
62
+ def __call__(self, data: DataDict) -> DataDict:
63
+ # During inference, task_id might be provided directly instead of task_index
64
+ if "task_id" in data:
65
+ # Direct task_id provided (inference mode)
66
+ task_id = int(data["task_id"])
67
+ elif "task_index" in data:
68
+ # task_index provided (training mode)
69
+ task_index = int(data["task_index"])
70
+
71
+ # Apply mapping if provided, otherwise use task_index directly as task_id
72
+ if self.task_mapping is not None:
73
+ if task_index not in self.task_mapping:
74
+ raise ValueError(f"task_index {task_index} not found in mapping")
75
+ task_id = self.task_mapping[task_index]
76
+ else:
77
+ task_id = task_index
78
+ else:
79
+ # During inference, if neither is provided, this transform should be skipped
80
+ # The tokenized_prompt should already be set up correctly
81
+ if "tokenized_prompt" in data:
82
+ return data # Already has tokenized_prompt, skip this transform
83
+ raise ValueError("Either task_index, task_id, or tokenized_prompt is required for PI_BEHAVIOR model")
84
+
85
+ # Pack task_id and subtask_state (if available) into tokenized_prompt
86
+ if "subtask_state" in data:
87
+ subtask_state = int(data["subtask_state"])
88
+ prompt_tokens = np.array([task_id, subtask_state], dtype=np.int32) # [task_id, subtask_state]
89
+ prompt_mask = np.array([True, True], dtype=bool)
90
+ else:
91
+ prompt_tokens = np.array([task_id], dtype=np.int32) # Just [task_id]
92
+ prompt_mask = np.array([True], dtype=bool)
93
+
94
+ return {
95
+ **data,
96
+ "tokenized_prompt": prompt_tokens,
97
+ "tokenized_prompt_mask": prompt_mask
98
+ }
99
+
100
+
101
+ @dataclasses.dataclass(frozen=True)
102
+ class ComputeSubtaskStateFromMeta(DataTransformFn):
103
+ """Computes subtask state from timestamp using dataset.meta.episodes.
104
+
105
+ Divides episode into task-specific number of stages based on episode length.
106
+ Requires dataset reference to access episode metadata.
107
+
108
+ Args:
109
+ dataset: Dataset instance with meta.episodes containing episode_length
110
+
111
+ Assumes:
112
+ - data["episode_index"] exists
113
+ - data["timestamp"] exists (frame index within episode)
114
+ - data["task_index"] exists (for task-specific stage count)
115
+ - dataset.meta.episodes contains episode_length for each episode
116
+
117
+ Creates:
118
+ - data["subtask_state"]: Stage index (0 to num_stages-1)
119
+ """
120
+
121
+ dataset: object | None = None # Will be set by data loader
122
+
123
+ def __call__(self, data: DataDict) -> DataDict:
124
+ if self.dataset is None:
125
+ # During inference or when dataset is not available, use default
126
+ data["subtask_state"] = np.array(0, dtype=np.int32)
127
+ return data
128
+
129
+ if "episode_index" not in data or "timestamp" not in data or "task_index" not in data:
130
+ # Missing required fields, default to stage 0
131
+ data["subtask_state"] = np.array(0, dtype=np.int32)
132
+ return data
133
+
134
+ episode_index = int(data["episode_index"])
135
+ timestamp = float(data["timestamp"])
136
+ task_index = int(data["task_index"])
137
+
138
+ # Validate task_index
139
+ if not (0 <= task_index < 50):
140
+ logging.warning(f"Invalid task_index {task_index}, using stage 0")
141
+ data["subtask_state"] = np.array(0, dtype=np.int32)
142
+ return data
143
+
144
+ # Get number of stages for this task
145
+ num_stages = TASK_NUM_STAGES[task_index]
146
+
147
+ # Get episode length from dataset metadata
148
+ if not hasattr(self.dataset, 'meta') or not hasattr(self.dataset.meta, 'episodes'):
149
+ # No metadata available
150
+ data["subtask_state"] = np.array(0, dtype=np.int32)
151
+ return data
152
+
153
+ meta_episodes = self.dataset.meta.episodes
154
+
155
+ if episode_index not in meta_episodes:
156
+ logging.warning(f"Episode {episode_index} not found in metadata, using stage 0")
157
+ data["subtask_state"] = np.array(0, dtype=np.int32)
158
+ return data
159
+
160
+ episode_info = meta_episodes[episode_index]
161
+
162
+ # Try 'length' first (standard key), then 'episode_length' (alternative)
163
+ episode_length = episode_info.get('length', episode_info.get('episode_length', None))
164
+
165
+ if episode_length is None or episode_length <= 0:
166
+ logging.warning(f"Invalid episode_length for episode {episode_index}, using stage 0")
167
+ data["subtask_state"] = np.array(0, dtype=np.int32)
168
+ return data
169
+
170
+ episode_length = float(episode_length)
171
+
172
+ # CRITICAL: Convert timestamp (in seconds) to frames (30 FPS)
173
+ # Dataset provides timestamp in seconds, episode_length is in frames
174
+ current_step = timestamp * 30.0
175
+
176
+ # Divide episode into num_stages equal parts
177
+ frames_per_stage = episode_length / num_stages
178
+
179
+ # Compute current stage (0-indexed)
180
+ subtask_state = int(current_step / frames_per_stage)
181
+
182
+ # Clamp to valid range [0, num_stages-1]
183
+ subtask_state = max(0, min(subtask_state, num_stages - 1))
184
+
185
+ data["subtask_state"] = np.array(subtask_state, dtype=np.int32)
186
+ return data
187
+
188
+
189
+ @dataclasses.dataclass(frozen=True)
190
+ class TokenizeFASTActions(DataTransformFn):
191
+ """Tokenize actions to FAST discrete tokens for PI_BEHAVIOR auxiliary training.
192
+
193
+ Converts continuous actions to discrete tokens using a trained FAST tokenizer.
194
+ This is used for auxiliary training to improve action quality.
195
+
196
+ Note: Uses GLOBAL normalization even if per-timestamp normalization is enabled
197
+ elsewhere, to preserve temporal smoothness for better DCT compression.
198
+
199
+ Args:
200
+ tokenizer_path: Path to trained FAST tokenizer directory
201
+ encoded_dim_ranges: List of (start, end) tuples for dimensions to encode
202
+ max_fast_tokens: Maximum number of tokens (truncate/pad)
203
+ norm_stats: Normalization stats for denorm/renorm if per-timestamp is used
204
+ use_per_timestamp: Whether per-timestamp normalization is being used
205
+
206
+ Assumes:
207
+ - data["actions"] exists and is normalized
208
+ - data["state"] exists (for delta computation)
209
+
210
+ Creates:
211
+ - data["fast_tokens"]: Discrete token indices [max_fast_tokens]
212
+ - data["fast_token_mask"]: Boolean mask for valid tokens [max_fast_tokens]
213
+ """
214
+
215
+ # Path to FAST tokenizer
216
+ tokenizer_path: str
217
+ # Action dimensions to encode: [(start, end), ...]
218
+ encoded_dim_ranges: list[tuple[int, int]]
219
+ # Max tokens to predict
220
+ max_fast_tokens: int = 180
221
+ # Normalization stats (for denorm/renorm if per-timestamp is used)
222
+ norm_stats: dict[str, NormStats] | None = None
223
+ # Whether per-timestamp normalization is being used
224
+ use_per_timestamp: bool = False
225
+
226
+ def _get_tokenizer(self):
227
+ """Lazy load tokenizer (called in each worker process).
228
+
229
+ transformers>=5 can no longer build the saved PreTrainedTokenizerFast via
230
+ UniversalActionProcessor.from_pretrained, so this re-implements its encode path directly:
231
+ DCT over time -> round(coeff * scale) -> shift by min_token -> chars -> BPE ids.
232
+ """
233
+ if not hasattr(self, '_tokenizer'):
234
+ import json
235
+ from pathlib import Path
236
+
237
+ from scipy.fft import dct
238
+ from tokenizers import Tokenizer
239
+
240
+ tokenizer_dir = Path(self.tokenizer_path)
241
+ cfg = json.loads((tokenizer_dir / "processor_config.json").read_text())
242
+ bpe = Tokenizer.from_file(str(tokenizer_dir / "tokenizer.json"))
243
+ scale, min_token = float(cfg["scale"]), int(cfg["min_token"])
244
+
245
+ def encode(action_chunk: np.ndarray) -> list[list[int]]:
246
+ if action_chunk.ndim == 2:
247
+ action_chunk = action_chunk[None, ...]
248
+ coeff = np.around(dct(action_chunk, axis=1, norm="ortho") * scale)
249
+ return [
250
+ bpe.encode("".join(map(chr, np.maximum(e.flatten() - min_token, 0).astype(int)))).ids
251
+ for e in coeff
252
+ ]
253
+
254
+ object.__setattr__(self, '_tokenizer', encode)
255
+
256
+ return self._tokenizer
257
+
258
+ def __call__(self, data: DataDict) -> DataDict:
259
+ # Only tokenize if actions are present (training time)
260
+ if "actions" not in data:
261
+ return data
262
+
263
+ actions = data["actions"] # [H, D] - may be per-timestamp normalized
264
+
265
+ # Extract encoded dimensions
266
+ encoded_chunks = []
267
+ for start, end in self.encoded_dim_ranges:
268
+ encoded_chunks.append(actions[:, start:end])
269
+ encoded_actions = np.concatenate(encoded_chunks, axis=-1) # [H, D_encoded]
270
+
271
+ # Convert to global QUANTILE normalization for FAST (always, regardless of per_timestamp)
272
+ # This clips outliers and provides consistent [-1, 1] range for DCT compression
273
+ if self.norm_stats is not None:
274
+ action_stats = self.norm_stats['actions']
275
+
276
+ # Extract stats for encoded dimensions only
277
+ # Build dimension indices from ranges
278
+ encoded_dims = []
279
+ for start, end in self.encoded_dim_ranges:
280
+ encoded_dims.extend(range(start, end))
281
+ encoded_dims = np.array(encoded_dims)
282
+
283
+ # Denormalize to get back to raw delta actions
284
+ if self.use_per_timestamp:
285
+ # Denormalize from per-timestamp MEAN/STD normalization
286
+ per_ts_mean = action_stats.per_timestamp_mean[:, encoded_dims] # [H, D_encoded]
287
+ per_ts_std = action_stats.per_timestamp_std[:, encoded_dims] # [H, D_encoded]
288
+ actions_delta = encoded_actions * per_ts_std + per_ts_mean
289
+ else:
290
+ # Denormalize from global MEAN/STD normalization
291
+ global_mean = action_stats.mean[encoded_dims] # [D_encoded]
292
+ global_std = action_stats.std[encoded_dims] # [D_encoded]
293
+ actions_delta = encoded_actions * global_std + global_mean
294
+
295
+ # Renormalize using GLOBAL QUANTILE normalization (for FAST)
296
+ # This clips to [q01, q99] and maps to [-1, 1]
297
+ q01 = action_stats.q01[encoded_dims] # [D_encoded]
298
+ q99 = action_stats.q99[encoded_dims] # [D_encoded]
299
+ actions_delta = np.clip(actions_delta, q01, q99)
300
+ encoded_actions = 2.0 * (actions_delta - q01) / np.maximum(q99 - q01, 1e-6) - 1.0
301
+
302
+ # Tokenize
303
+ tokenizer = self._get_tokenizer()
304
+ tokens = tokenizer(encoded_actions[None])[0]
305
+
306
+ if isinstance(tokens, list):
307
+ tokens = np.array(tokens)
308
+
309
+ # Truncate or pad to max_fast_tokens
310
+ if len(tokens) > self.max_fast_tokens:
311
+ tokens = tokens[:self.max_fast_tokens]
312
+ mask = np.ones(self.max_fast_tokens, dtype=bool)
313
+ else:
314
+ mask = np.concatenate([
315
+ np.ones(len(tokens), dtype=bool),
316
+ np.zeros(self.max_fast_tokens - len(tokens), dtype=bool)
317
+ ])
318
+ tokens = np.pad(tokens, (0, self.max_fast_tokens - len(tokens)), constant_values=0)
319
+
320
+ data["fast_tokens"] = tokens.astype(np.int32)
321
+ data["fast_token_mask"] = mask
322
+
323
+ return data
324
+
325
+
326
+
327
+ @dataclasses.dataclass(frozen=True)
328
+ class OverrideTaskIndex(DataTransformFn):
329
+ """Forces task_index to a fixed model task slot.
330
+
331
+ Used to fine-tune a 50-task PI_BEHAVIOR checkpoint on a task it never saw (e.g. 2026 task 57,
332
+ sweeping_garage) by training it in an existing slot, so all parameter shapes stay unchanged.
333
+ """
334
+
335
+ task_index: int
336
+
337
+ def __call__(self, data: DataDict) -> DataDict:
338
+ return {**data, "task_index": np.array(self.task_index, dtype=np.int64)}
339
+
340
+
341
+ @dataclasses.dataclass(frozen=True)
342
+ class ComputeSubtaskStateFromLengths(DataTransformFn):
343
+ """Same stage split as ComputeSubtaskStateFromMeta, but from an {episode_index: length} dict."""
344
+
345
+ episode_lengths: dict[int, int]
346
+
347
+ def __call__(self, data: DataDict) -> DataDict:
348
+ num_stages = TASK_NUM_STAGES[int(data["task_index"])]
349
+ frames_per_stage = self.episode_lengths[int(data["episode_index"])] / num_stages
350
+ stage = int(float(data["timestamp"]) * 30.0 / frames_per_stage)
351
+ data["subtask_state"] = np.array(max(0, min(stage, num_stages - 1)), dtype=np.int32)
352
+ return data
task 57/policy_code/src/b1k/transforms_normalize.py ADDED
@@ -0,0 +1,124 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Custom Normalize transform with per-timestamp support.
2
+ """
3
+
4
+ import dataclasses
5
+ import numpy as np
6
+
7
+ from openpi.transforms import DataTransformFn, DataDict, apply_tree, pad_to_dim
8
+ from openpi.shared import array_typing as at
9
+ from b1k.shared.normalize import NormStats
10
+
11
+
12
+ @dataclasses.dataclass(frozen=True)
13
+ class NormalizeWithPerTimestamp(DataTransformFn):
14
+ norm_stats: at.PyTree[NormStats] | None
15
+ use_quantiles: bool = False
16
+ strict: bool = False
17
+ use_per_timestamp: bool = False
18
+
19
+ def __post_init__(self):
20
+ if self.norm_stats is not None and self.use_quantiles:
21
+ for stats in at.tree_leaves(self.norm_stats):
22
+ if isinstance(stats, NormStats) and (stats.q01 is None or stats.q99 is None):
23
+ raise ValueError("Quantile normalization requires q01 and q99 in norm_stats")
24
+
25
+ def __call__(self, data: DataDict) -> DataDict:
26
+ if self.norm_stats is None:
27
+ return data
28
+
29
+ return apply_tree(
30
+ data,
31
+ self.norm_stats,
32
+ self._normalize_quantile if self.use_quantiles else self._normalize,
33
+ strict=self.strict,
34
+ )
35
+
36
+ def _normalize(self, x, stats: NormStats):
37
+ # Check if this is actions and we should use per-timestamp normalization
38
+ if self.use_per_timestamp and stats.per_timestamp_mean is not None and x.ndim >= 2:
39
+ # x has shape [..., action_horizon, action_dim] for actions
40
+ # stats.per_timestamp_mean has shape [action_horizon, action_dim]
41
+ mean = stats.per_timestamp_mean[..., : x.shape[-2], : x.shape[-1]]
42
+ std = stats.per_timestamp_std[..., : x.shape[-2], : x.shape[-1]]
43
+ return (x - mean) / (std + 1e-6)
44
+ else:
45
+ # Regular normalization
46
+ mean, std = stats.mean[..., : x.shape[-1]], stats.std[..., : x.shape[-1]]
47
+ return (x - mean) / (std + 1e-6)
48
+
49
+ def _normalize_quantile(self, x, stats: NormStats):
50
+ assert stats.q01 is not None
51
+ assert stats.q99 is not None
52
+
53
+ # Check if this is actions and we should use per-timestamp normalization
54
+ if self.use_per_timestamp and stats.per_timestamp_q01 is not None and x.ndim >= 2:
55
+ # x has shape [..., action_horizon, action_dim] for actions
56
+ # stats.per_timestamp_q01 has shape [action_horizon, action_dim]
57
+ q01 = stats.per_timestamp_q01[: x.shape[-2], : x.shape[-1]]
58
+ q99 = stats.per_timestamp_q99[: x.shape[-2], : x.shape[-1]]
59
+ return (x - q01) / (q99 - q01 + 1e-6) * 2.0 - 1.0
60
+ else:
61
+ # Regular quantile normalization
62
+ q01, q99 = stats.q01[..., : x.shape[-1]], stats.q99[..., : x.shape[-1]]
63
+ return (x - q01) / (q99 - q01 + 1e-6) * 2.0 - 1.0
64
+
65
+
66
+ @dataclasses.dataclass(frozen=True)
67
+ class UnnormalizeWithPerTimestamp(DataTransformFn):
68
+ norm_stats: at.PyTree[NormStats] | None
69
+ use_quantiles: bool = False
70
+ use_per_timestamp: bool = False
71
+
72
+ def __post_init__(self):
73
+ if self.norm_stats is not None and self.use_quantiles:
74
+ for stats in at.tree_leaves(self.norm_stats):
75
+ if isinstance(stats, NormStats) and (stats.q01 is None or stats.q99 is None):
76
+ raise ValueError("Quantile normalization requires q01 and q99 in norm_stats")
77
+
78
+ def __call__(self, data: DataDict) -> DataDict:
79
+ if self.norm_stats is None:
80
+ return data
81
+
82
+ return apply_tree(
83
+ data,
84
+ self.norm_stats,
85
+ self._unnormalize_quantile if self.use_quantiles else self._unnormalize,
86
+ strict=True,
87
+ )
88
+
89
+ def _unnormalize(self, x, stats: NormStats):
90
+ # Check if this is actions and we should use per-timestamp normalization
91
+ if self.use_per_timestamp and stats.per_timestamp_mean is not None and x.ndim >= 2:
92
+ # x has shape [..., action_horizon, action_dim] for actions
93
+ # stats.per_timestamp_mean has shape [action_horizon, action_dim]
94
+ mean = pad_to_dim(stats.per_timestamp_mean, x.shape[-1], axis=-1, value=0.0)
95
+ std = pad_to_dim(stats.per_timestamp_std, x.shape[-1], axis=-1, value=1.0)
96
+ # Ensure we only use the appropriate timesteps
97
+ mean = mean[: x.shape[-2], :]
98
+ std = std[: x.shape[-2], :]
99
+ return x * (std + 1e-6) + mean
100
+ else:
101
+ # Regular unnormalization
102
+ mean = pad_to_dim(stats.mean, x.shape[-1], axis=-1, value=0.0)
103
+ std = pad_to_dim(stats.std, x.shape[-1], axis=-1, value=1.0)
104
+ return x * (std + 1e-6) + mean
105
+
106
+ def _unnormalize_quantile(self, x, stats: NormStats):
107
+ assert stats.q01 is not None
108
+ assert stats.q99 is not None
109
+
110
+ # Check if this is actions and we should use per-timestamp normalization
111
+ if self.use_per_timestamp and stats.per_timestamp_q01 is not None and x.ndim >= 2:
112
+ # x has shape [..., action_horizon, action_dim] for actions
113
+ # stats.per_timestamp_q01 has shape [action_horizon, action_dim]
114
+ q01 = stats.per_timestamp_q01[: x.shape[-2], : x.shape[-1]]
115
+ q99 = stats.per_timestamp_q99[: x.shape[-2], : x.shape[-1]]
116
+ return (x + 1.0) / 2.0 * (q99 - q01 + 1e-6) + q01
117
+ else:
118
+ # Regular quantile unnormalization
119
+ q01, q99 = stats.q01, stats.q99
120
+ if (dim := q01.shape[-1]) < x.shape[-1]:
121
+ return np.concatenate([(x[..., :dim] + 1.0) / 2.0 * (q99 - q01 + 1e-6) + q01, x[..., dim:]], axis=-1)
122
+ return (x + 1.0) / 2.0 * (q99 - q01 + 1e-6) + q01
123
+
124
+