petkopetkov commited on
Commit
7dbf03c
·
verified ·
1 Parent(s): 7e0ec2b

backup: sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5 (eval outputs excluded due to 48k-file commit limit)

Browse files
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/checkpoints/model-checkpoint-ema-030000.pt ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:4905588d719d632deead433446f4760e26b9dd4960e496dfdbd7b1eb57bb5d57
3
+ size 12952714490
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/common_pizero_fm_paligemma.py ADDED
@@ -0,0 +1,571 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from functools import cached_property
2
+ from typing import Any, Callable, Dict, List, Optional, Sequence, Tuple, Type
3
+
4
+ import torch
5
+ import torch.distributed.fsdp
6
+ import torch.distributed.tensor
7
+ import torch.nn.attention.flex_attention
8
+ import transformers
9
+ from backports.strenum import StrEnum
10
+ from databib.dataclasses import Dataclass, dataclass
11
+ from databib.dataclasses.dataclass import DataclassT
12
+ from databib.utils.classproperty import classproperty
13
+
14
+
15
+ class ReferenceFrame(StrEnum):
16
+ """
17
+ Indicates the frame frame w.r.t. which translation or rotation is expressed.
18
+ Note that each of translation and rotation has its own (possibly different) ReferenceFrame value.
19
+
20
+ WORLD: Only for completeness, not yet used. Will become relevant when navigation is introduced.
21
+ ROBOT_BASE: Translation/rotation expressed in absolute robot base frame
22
+ ROBOT_BASE_DELTA:
23
+ - Translation expressed as delta value w.r.t. the previous EEF translation pose
24
+ The delta value is defined in the robot base frame (rather than in the current EEF frame)
25
+ - Rotation expressed as w.r.t. the previous rotation pose
26
+ The axis of rotation is defined in the robot base frame (rather than in the current EEF frame)
27
+ ROBOT_BASE_RELATIVE: Same as ROBOT_BASE_DELTA, but the sequence is expressed w.r.t.the 0-th element
28
+ instead of the previous element
29
+ EEF: Translation/rotation expressed in the current end-effector frame
30
+ EEF_DELTA:
31
+ - Translation expressed as delta value w.r.t. the previous EEF translation pose
32
+ The delta value is defined in the current EEF frame (rather than in the robot base frame)
33
+ - Rotation expressed as w.r.t. the previous rotation pose
34
+ The axis of rotation is defined in the current EEF frame (rather than in the robot base frame)
35
+ """
36
+
37
+ ROBOT_BASE = 'robot_base'
38
+ ROBOT_BASE_DELTA = 'robot_base_delta'
39
+ ROBOT_BASE_RELATIVE = 'robot_base_relative'
40
+ EEF_RELATIVE = EEF = 'eef_relative'
41
+ EEF_DELTA = 'eef_delta'
42
+ CAMERA = 'camera'
43
+ UNKNOWN = 'unknown'
44
+
45
+ @classproperty
46
+ def robot_frames(cls) -> set['ReferenceFrame']:
47
+ return {
48
+ ReferenceFrame.ROBOT_BASE,
49
+ ReferenceFrame.ROBOT_BASE_DELTA,
50
+ ReferenceFrame.ROBOT_BASE_RELATIVE,
51
+ }
52
+
53
+ @classproperty
54
+ def eef_frames(cls) -> set['ReferenceFrame']:
55
+ return {ReferenceFrame.EEF, ReferenceFrame.EEF_RELATIVE, ReferenceFrame.EEF_DELTA}
56
+
57
+ @classproperty
58
+ def delta_frames(cls) -> set['ReferenceFrame']:
59
+ return {ReferenceFrame.ROBOT_BASE_DELTA, ReferenceFrame.EEF_DELTA}
60
+
61
+ @classproperty
62
+ def relative_frames(cls) -> set['ReferenceFrame']:
63
+ return {ReferenceFrame.ROBOT_BASE_RELATIVE, ReferenceFrame.EEF_RELATIVE}
64
+
65
+ @classproperty
66
+ def core_frames(cls) -> set['ReferenceFrame']:
67
+ return {ReferenceFrame.ROBOT_BASE, ReferenceFrame.EEF}
68
+
69
+ def to_relative(self) -> 'ReferenceFrame':
70
+ if self in self.robot_frames:
71
+ return self.ROBOT_BASE_RELATIVE
72
+ if self in self.eef_frames:
73
+ return self.EEF_RELATIVE
74
+ raise ValueError(f'Cannot convert frame {self} to relative frame')
75
+
76
+ def to_delta(self) -> 'ReferenceFrame':
77
+ if self in self.robot_frames:
78
+ return self.ROBOT_BASE_DELTA
79
+ if self in self.eef_frames:
80
+ return self.EEF_DELTA
81
+ raise ValueError(f'Cannot convert frame {self} to delta frame')
82
+
83
+ def to_core(self) -> 'ReferenceFrame':
84
+ if self in self.robot_frames:
85
+ return self.ROBOT_BASE
86
+ if self in self.eef_frames:
87
+ return self.EEF
88
+ raise ValueError(f'Cannot convert frame {self} to relative frame')
89
+
90
+
91
+ class RotationFormat(StrEnum):
92
+ """Determines how rotations will be encoded in the loaded batch"""
93
+
94
+ EULER = 'euler'
95
+ QUATERNION = 'quaternion'
96
+ ROTMAT = 'rotmat'
97
+
98
+
99
+ class ResizeMode(StrEnum):
100
+ """
101
+ Different modes for resizing images.
102
+ """
103
+
104
+ MATCH_WIDTH = 'match_width'
105
+ MATCH_HEIGHT = 'match_height'
106
+ MATCH_MAX = 'match_max'
107
+ NAIVE = 'naive'
108
+ SMART = 'smart'
109
+ PAD = 'pad'
110
+ CROP = 'crop'
111
+
112
+
113
+ def expand_dims(tensor: torch.Tensor, ndim: int, order: Sequence[int]) -> torch.Tensor:
114
+ """
115
+ Expand the dimensions of `tensor` to `ndim` such that all new dimensions have size of 1
116
+ Args:
117
+ tensor: torch.Tensor of any shape
118
+ ndim: Number of output dimensions. Must be >= `tensor.ndim`
119
+ order: Sequence of size `tensor.ndim + 1`. Contains only values of 1 and a single value of -1,
120
+ indicating where the new `ndim - tensor.ndim` dimensions will be inserted
121
+ Returns:
122
+ torch.Tensor with dimensions `ndim`, a view of `tensor`
123
+
124
+ Ex:
125
+ expand_dims(torch.ones([2, 3, 4]), ndim=5, order=[1, -1, 1, 1]).shape -> [2, 1, 1, 3, 4]
126
+ expand_dims(torch.ones([2, 3, 4]), ndim=5, order=[-1, 1, 1, 1]).shape -> [1, 1, 2, 3, 4]
127
+ expand_dims(torch.ones([2, 3, 4]), ndim=5, order=[1, 1, 1, -1]).shape -> [2, 3, 4, 1, 1]
128
+ """
129
+ assert tensor.ndim <= ndim, f'{tensor.ndim} > {ndim}; shape={tensor.shape}'
130
+ assert len(order) == tensor.ndim + 1, f'{len(order)} != {tensor.ndim + 1}; shape={tensor.shape}'
131
+ order = list(order)
132
+ assert order.count(-1) == 1, 'Order must have exactly one value of -1'
133
+ assert order.count(1) == len(order) - 1, 'Order must have exactly len(order) - 1 values of 1'
134
+ if tensor.ndim == ndim:
135
+ return tensor
136
+ insert_index = order.index(-1)
137
+ view = list(tensor.shape[:insert_index]) + [1] * (ndim - tensor.ndim) + list(tensor.shape[insert_index:])
138
+ tensor = tensor.view(view)
139
+ return tensor
140
+
141
+
142
+ def compare_dicts(dict_0: Dict[str, Any], dict_1: Dict[str, Any], comparison_function: Callable) -> bool:
143
+ if set(dict_0.keys()) != set(dict_1.keys()):
144
+ return False
145
+ for key, _ in dict_0.items():
146
+ if type(dict_0[key]) != type(dict_1[key]):
147
+ return False
148
+ if isinstance(dict_0[key], dict):
149
+ result = compare_dicts(dict_0[key], dict_1[key], comparison_function)
150
+ else:
151
+ result = comparison_function(dict_0[key], dict_1[key])
152
+ if isinstance(result, torch.Tensor):
153
+ result = bool(result.all())
154
+ if not result:
155
+ return False
156
+ return True
157
+
158
+
159
+ def tensor_size_bytes(tensor: Optional[torch.Tensor]) -> int:
160
+ if tensor is None:
161
+ return 0
162
+ if not isinstance(tensor, torch.Tensor):
163
+ raise RuntimeError('Provided data is not a torch.Tensor: ', tensor)
164
+ bytes_per_element = tensor.element_size()
165
+ return bytes_per_element * tensor.numel()
166
+
167
+
168
+ def tensor_dataclass(cls: Type[DataclassT], **kwargs) -> Type[DataclassT]:
169
+ cls = dataclass(cls, eq=False, **kwargs)
170
+ return cls
171
+
172
+
173
+ @tensor_dataclass
174
+ class TensorDataclass(Dataclass):
175
+ """
176
+ Extends Dataclass with common torch.Tensor utilities.
177
+ - Can contain non-tensor fields, but some member functions might ignore these fields
178
+ or explicitly raise errors.
179
+ - Useful for packing batches, input and output data for ML models
180
+ - When using for input / output data for ML models, it's recommended to keep only torch.Tensor
181
+ fields to allow for supporting functionality such as torch.jit.script
182
+ """
183
+
184
+ def __eq__(self, other) -> bool:
185
+ if type(other) is not type(self):
186
+ return False
187
+ return compare_dicts(self.as_json(), other.as_json(), lambda x, y: x == y)
188
+
189
+ def __ne__(self, other) -> bool:
190
+ return not self == other
191
+
192
+ def __hash__(self):
193
+ raise ValueError(f'Hash function not implemented for {self.__class__.__name__}.')
194
+
195
+ def calc_size_bytes(self) -> int:
196
+ return sum(
197
+ (
198
+ tensor_size_bytes(value)
199
+ for (_, value) in self.items(recursive=True)
200
+ if isinstance(value, torch.Tensor)
201
+ )
202
+ )
203
+
204
+ def calc_size_megabytes(self) -> float:
205
+ return self.calc_size_bytes() / 2**20
206
+
207
+ def cpu(self) -> 'TensorDataclass':
208
+ return self.to(device='cpu')
209
+
210
+ def to(self, *, device=None, dtype=None, copy=False, non_blocking=False) -> 'TensorDataclass':
211
+ assert device is not None or dtype is not None
212
+ return self.apply(
213
+ lambda value: value.to(device=device, dtype=dtype, copy=copy, non_blocking=non_blocking)
214
+ if isinstance(value, torch.Tensor)
215
+ else value
216
+ )
217
+
218
+ def float32(self) -> 'TensorDataclass':
219
+ return self.apply(
220
+ lambda value: value.to(dtype=torch.float32)
221
+ if isinstance(value, torch.Tensor) and value.dtype.is_floating_point
222
+ else value
223
+ )
224
+
225
+ def detach(self) -> 'TensorDataclass':
226
+ return self.apply(lambda value: value.detach() if isinstance(value, torch.Tensor) else value)
227
+
228
+ def __getitem__(self, index) -> 'TensorDataclass':
229
+ def extract(obj):
230
+ if obj is None:
231
+ return None
232
+ if isinstance(obj, torch.Tensor):
233
+ return obj[index]
234
+ raise ValueError(f'Cannot slice {obj.__class__.__name__} object')
235
+
236
+ return self.apply(extract)
237
+
238
+ @property
239
+ def device(self) -> Optional[torch.device]:
240
+ """
241
+ Returns the device on which tensors in this dataclass reside. If tensors are on
242
+ different devices, raises RuntimeError. If no tensors in the class, returns None
243
+ """
244
+ devices = [
245
+ value.device
246
+ for (key, value) in self.items()
247
+ if isinstance(value, (TensorDataclass, torch.Tensor))
248
+ ]
249
+ devices = [d for d in devices if d is not None]
250
+ if len(devices) == 0:
251
+ return None
252
+ if len(set(devices)) == 1:
253
+ return devices[0]
254
+ (key, device) = (None, None)
255
+ for k, value in self.items():
256
+ if value is None:
257
+ continue
258
+ if device is None:
259
+ device = value.device
260
+ key = k
261
+ elif device != value.device:
262
+ raise RuntimeError(
263
+ f'Inconsistent device for instance of {self.__class__.__name__}. Device of field {key} is {device}, while device of field {k} is {value.device}'
264
+ )
265
+ raise RuntimeError
266
+
267
+ def to_shared_memory(self) -> 'TensorDataclass':
268
+ """Move all tensors in the dataclass to shared memory"""
269
+ return self.apply(lambda value: value.share_memory_() if isinstance(value, torch.Tensor) else value)
270
+
271
+ def pin_memory(self) -> 'TensorDataclass':
272
+ """Used for pinning memory during dataloading. Do not modify the name of the function"""
273
+ return self.apply(lambda value: value.pin_memory() if isinstance(value, torch.Tensor) else value)
274
+
275
+
276
+ @tensor_dataclass
277
+ class ModelTarget(TensorDataclass):
278
+ """
279
+ Only relevant for supervised learning.
280
+ Packs regression / classification target values that we input in the loss
281
+ """
282
+
283
+
284
+ @tensor_dataclass
285
+ class RoboticsTarget(ModelTarget):
286
+ control_tokens_ids: Optional[torch.Tensor]
287
+ text_tokens_ids: Optional[torch.Tensor]
288
+ translation: torch.Tensor
289
+ rotation: torch.Tensor
290
+ gripper: torch.Tensor
291
+ valid_mask: torch.Tensor
292
+
293
+
294
+ @tensor_dataclass
295
+ class PolicyControlPlan(TensorDataclass):
296
+ """
297
+ Abstraction class relevant for control tasks. Note that `ModelOutput` might not contain the actual
298
+ controls we want to use on the robot in the environment. Examples:
299
+ - `ModelOutput` contains logits, since computing losses on logits is more numerically stable.
300
+ We need to convert these logits to actual controls for the actual robot
301
+ - `ModelOutput` contains an entire costmap from which we need to extract waypoints
302
+ - `ModelOutput` contains unnormalized quaternion or rotation matrix that need to be normalized
303
+ - `ModelOutput` contains 2D/3D positions from which we need to extract speed and steering
304
+ `PolicyControlPlan`
305
+ - Extracts actual physical representation from `ModelOutput` that we can use to dervie the controls
306
+ - Doesn't necessarily contain the controls themselves, but they can be derived from this data
307
+ - **Interpretable control plan which we can visualize, interpret and compare to the real data**
308
+ - Ex: Controls might be in speed and steering, but we likely want to compare 2D/3D positions
309
+ instead of controls for metrics and visualizations
310
+ - Ex: Robot control is usually a single timestep, while `PolicyControlPlan` contains
311
+ controls over multiple timesteps
312
+ - Can have different abstractions, e.g.
313
+ - End effector 3D translation and rotation (positional control)
314
+ - Speed and steering for a vehicle (actuator control)
315
+ - 3D waypoints for a path to be followed
316
+ - Usually **unnormalized** values into physical units (vs normalized `ModelOutput`)
317
+ Main purpose: (Human) Interpretable control plans and metadata that can be used for visualization,
318
+ metrics and debugging
319
+ """
320
+
321
+
322
+ @tensor_dataclass
323
+ class RoboticsControlPlan(PolicyControlPlan):
324
+ translation_m: torch.Tensor
325
+ rotmat: torch.Tensor
326
+ gripper_prob: torch.Tensor
327
+ valid_mask: torch.Tensor
328
+
329
+ def __post_init__(self):
330
+ super().__post_init__()
331
+ assert self.translation_m.ndim == 3, self.translation_m.shape
332
+ assert self.rotmat.ndim == 3, self.rotmat.shape
333
+ assert self.gripper_prob.ndim == 3, self.gripper_prob.shape
334
+
335
+
336
+ @tensor_dataclass
337
+ class ModelOutput(TensorDataclass):
338
+ """
339
+ Packs data which an NN model outputs. Note this can contain a lot of metadata
340
+ such as intermediate outputs, probabilities, visualizations, etc
341
+ In the case of robot control, the action class is not guaranteed to be part of this
342
+ class, but we must be able to derive an action from the data in this class
343
+ """
344
+
345
+
346
+ @tensor_dataclass
347
+ class RoboticsInput(TensorDataclass):
348
+ images: Dict[str, torch.Tensor]
349
+ input_ids: torch.Tensor
350
+ attn_mask: torch.Tensor
351
+ ee_pose_translation: torch.Tensor
352
+ ee_pose_rotation: torch.Tensor
353
+ gripper: torch.Tensor
354
+ joints: torch.Tensor
355
+ control_tokens_ids: Optional[torch.Tensor]
356
+
357
+ @property
358
+ def inputs_embeds(self) -> Optional[torch.Tensor]:
359
+ return None
360
+
361
+ @property
362
+ def past_key_values(self) -> Optional[List[torch.Tensor]]:
363
+ return None
364
+
365
+ @cached_property
366
+ def multimodal_indices(self) -> torch.Tensor:
367
+ """
368
+ Returns a torch.Tensor containing only the indices of the batch examples which are multimodal.
369
+ Return shape is [B]
370
+ """
371
+ return torch.arange(self.input_ids.shape[0], dtype=torch.int64, device=self.input_ids.device)
372
+
373
+ @cached_property
374
+ def unimodal_indices(self) -> torch.Tensor:
375
+ """
376
+ Returns a torch.Tensor containing only the indices of the batch examples which are unimodal.
377
+ Return shape is [B]
378
+ """
379
+ return torch.tensor([], dtype=torch.int64, device=self.input_ids.device)
380
+
381
+
382
+ @tensor_dataclass
383
+ class FlowInput(TensorDataclass):
384
+ timestep: torch.Tensor
385
+ translation_t: torch.Tensor
386
+ rotation_t: torch.Tensor
387
+ gripper_t: torch.Tensor
388
+ translation_t0: torch.Tensor
389
+ rotation_t0: torch.Tensor
390
+ gripper_t0: torch.Tensor
391
+
392
+
393
+ @tensor_dataclass
394
+ class RoboticsFlowInput(RoboticsInput):
395
+ """Input to the entire Robotics VLM"""
396
+
397
+ flow_input: FlowInput
398
+
399
+
400
+ @tensor_dataclass
401
+ class DiffusionInput(TensorDataclass):
402
+ timestep: torch.Tensor
403
+ noised_translation: torch.Tensor
404
+ noised_rotation: torch.Tensor
405
+ noised_gripper: torch.Tensor
406
+
407
+
408
+ @tensor_dataclass
409
+ class LLMOutput(TensorDataclass):
410
+ """Fork of transformers.modeling_outputs.CausalLMOutputWithPast"""
411
+
412
+ input_ids: torch.Tensor
413
+ logits: Optional[torch.Tensor]
414
+ output_ids: Optional[torch.Tensor]
415
+ loss: Optional[torch.Tensor]
416
+ past_key_values: List[Tuple[torch.Tensor, torch.Tensor]]
417
+ hidden_states: List[torch.Tensor]
418
+ text_mask: torch.Tensor
419
+ image_mask: torch.Tensor
420
+
421
+ @classmethod
422
+ def from_transformers(
423
+ cls,
424
+ input_ids: torch.Tensor,
425
+ llm_output: transformers.modeling_outputs.CausalLMOutputWithPast,
426
+ text_mask: torch.Tensor,
427
+ image_mask: torch.Tensor,
428
+ ) -> 'LLMOutput':
429
+ return LLMOutput(
430
+ input_ids=input_ids,
431
+ logits=getattr(llm_output, 'logits', None),
432
+ output_ids=None,
433
+ loss=getattr(llm_output, 'loss', None),
434
+ past_key_values=list(llm_output.past_key_values)
435
+ if llm_output.past_key_values is not None
436
+ else [],
437
+ hidden_states=list(llm_output.hidden_states) if llm_output.hidden_states is not None else [],
438
+ text_mask=text_mask,
439
+ image_mask=image_mask,
440
+ )
441
+
442
+ def compress(self, ignore_index: int = -100) -> 'LLMOutput':
443
+ """
444
+ Compress the data contained in the class so it can be moved between CPU and GPU or concatenated
445
+ much faster:
446
+ - hidden_states - huge tensors; take a lot of CPU time to move across devices or concat
447
+ - past_key_values - huge tensors; take a lot of CPU time to move across devices or concat
448
+ - logits - huge last dimension; takes a lot of CPU time to move across devices or concat
449
+ """
450
+ replace: Dict[str, Any] = {'hidden_states': [], 'past_key_values': [], 'loss': None}
451
+ if self.logits is not None:
452
+ replace['logits'] = None
453
+ if self.output_ids is None:
454
+ assert (
455
+ self.text_mask is not None
456
+ ), 'text_mask is required to compute output_ids when output_ids is None'
457
+ assert (
458
+ self.logits.shape[:2] == self.text_mask.shape
459
+ ), 'logits and text_mask batch and sequence dimensions must match to compute output_ids'
460
+ predicted_ids = self.logits.argmax(dim=-1)
461
+ output_ids = torch.where(self.text_mask, predicted_ids, ignore_index)
462
+ replace['output_ids'] = output_ids
463
+ return self.replace(**replace)
464
+
465
+
466
+ @tensor_dataclass
467
+ class RoboticsOutput(ModelOutput):
468
+ translation: Optional[torch.Tensor]
469
+ rotation: Optional[torch.Tensor]
470
+ gripper: Optional[torch.Tensor]
471
+ token_logits: Optional[torch.Tensor]
472
+ token_ids: Optional[torch.Tensor]
473
+ llm_output: LLMOutput
474
+
475
+ def compress(self, ignore_index: int = -100) -> 'RoboticsOutput':
476
+ """
477
+ Compress output and drop unnecessary components to speed up transfer GPU <-> CPU.
478
+ Note that LLM logits can be extremely expensive since their size is [B, S, vocab_size], which
479
+ can reach millions or billions of values for large vocab_size
480
+ """
481
+ replace: Dict[str, Any] = {
482
+ 'llm_output': self.llm_output.compress(ignore_index=ignore_index),
483
+ 'token_logits': None,
484
+ }
485
+ if self.token_logits is not None and self.token_ids is None:
486
+ replace['token_ids'] = torch.argmax(self.token_logits, dim=-1)
487
+ return self.replace(**replace)
488
+
489
+
490
+ @tensor_dataclass
491
+ class VLMOutput(TensorDataclass):
492
+ llm_output: LLMOutput
493
+ vit_tokens: Optional[torch.Tensor]
494
+ attn_mask: torch.Tensor
495
+
496
+ def compress(self, ignore_index: int = -100) -> 'VLMOutput':
497
+ """
498
+ Compress output and drop unnecessary components to speed up transfer GPU <-> CPU.
499
+ Note that LLM logits can be extremely expensive since their size is [B, S, vocab_size], which
500
+ can reach millions or billions of values for large vocab_size
501
+ """
502
+ return self.replace(llm_output=self.llm_output.compress(ignore_index=ignore_index))
503
+
504
+
505
+ def is_quaternion(quaternion: torch.Tensor) -> bool:
506
+ return quaternion.shape[-1] == 4
507
+
508
+
509
+ def quaternion_half_cover(quaternion: torch.Tensor) -> torch.Tensor:
510
+ """
511
+ Flip quaternions so they cover only a half the space. If the q_w is negative, flip the quaternion.
512
+ If q_w is 0, then choose such that the first non-zero component is positive. Note that geometrically,
513
+ this doesn't correspond to a single hemisphere of the unit sphere. Follows
514
+ https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.as_quat.html#scipy.spatial.transform.Rotation.as_quat
515
+ """
516
+ assert is_quaternion(quaternion), quaternion.shape
517
+ with torch.no_grad():
518
+ is_zero = quaternion == 0
519
+ flip_condition = (
520
+ (quaternion[..., -1:] < 0)
521
+ | is_zero[..., -1:] & (quaternion[..., 0:1] < 0)
522
+ | is_zero[..., -1:] & is_zero[..., 0:1] & (quaternion[..., 1:2] < 0)
523
+ | is_zero[..., -1:] & is_zero[..., 0:1] & is_zero[..., 1:2] & (quaternion[..., 2:3] < 0)
524
+ )
525
+ quaternion = torch.where(flip_condition, -quaternion, quaternion)
526
+ return quaternion
527
+
528
+
529
+ def is_rotmat_3x3(rotmat: torch.Tensor) -> bool:
530
+ return rotmat.shape[-2:] == torch.Size([3, 3])
531
+
532
+
533
+ def is_rotmat_9(rotmat: torch.Tensor) -> bool:
534
+ return rotmat.shape[-1] == 9
535
+
536
+
537
+ def rotmat_as_9(rotmat: torch.Tensor) -> torch.Tensor:
538
+ """Convert any rotmat input to [..., 9] shape"""
539
+ if is_rotmat_9(rotmat):
540
+ return rotmat
541
+ if is_rotmat_3x3(rotmat):
542
+ return rotmat.reshape(*rotmat.shape[:-2], 9)
543
+ raise ValueError(f"Can't convert tensor of shape {rotmat.shape} to a 3x3 rotation matrix")
544
+
545
+
546
+ def is_rotmat(rotmat: torch.Tensor) -> bool:
547
+ """
548
+ Checks if the tensor shape matches that of a rotmat. However, it's not guaranteed the data is a
549
+ valid rotmat. `is_orthonormal_rotmat` performs this additional check.
550
+ NOTE: This might incorrectly return True if the underlying data is euler angles and accidentally
551
+ `rotmat.shape[-2:] == [3, 3]`. This would happen very rarely, but use with caution
552
+ """
553
+ return is_rotmat_3x3(rotmat) or is_rotmat_9(rotmat)
554
+
555
+
556
+ def rotmat_as_3x3(rotmat: torch.Tensor) -> torch.Tensor:
557
+ """Convert any rotmat input to [..., 3, 3] shape"""
558
+ if rotmat.shape[-1] == 9:
559
+ return rotmat.reshape(*rotmat.shape[:-1], 3, 3)
560
+ if rotmat.shape[-2:] == torch.Size([3, 3]):
561
+ return rotmat
562
+ raise ValueError(f"Can't convert tensor of shape {rotmat.shape} to a 3x3 rotation matrix")
563
+
564
+
565
+ def rotmat_inverse(rotation: torch.Tensor) -> torch.Tensor:
566
+ assert is_rotmat(rotation), f'Expected a rotation matrix, but got shape {rotation.shape}'
567
+ rotmat = rotmat_as_3x3(rotation)
568
+ rotmat = rotmat.transpose(-1, -2)
569
+ if is_rotmat_9(rotation):
570
+ rotmat = rotmat_as_9(rotmat)
571
+ return rotmat
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/configuration_pizero_fm_paligemma.py ADDED
@@ -0,0 +1,366 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from typing import Any, Dict, List, Optional
2
+
3
+ from databib.config import Config
4
+
5
+ from .common_pizero_fm_paligemma import ReferenceFrame, ResizeMode, RotationFormat
6
+
7
+
8
+ class ConfigurableModuleConfig(Config):
9
+ @property
10
+ def pretrained(self) -> bool:
11
+ return not self.pretrain_config.empty
12
+
13
+
14
+ class FourierFeaturesProjectorConfig(ConfigurableModuleConfig):
15
+ in_features: int
16
+ num_features: int = 256
17
+ layers: List[int] = [256, 512, 256]
18
+ activation: str = 'GELU'
19
+ norm: Optional[str] = None
20
+
21
+
22
+ class RotaryPositionalEncodingConfig(ConfigurableModuleConfig):
23
+ num_embeddings: int
24
+ embedding_dim: int
25
+ base: int = 10000
26
+ cached: bool = True
27
+
28
+
29
+ class PiZeroFlowMatchingDecoderBlockConfig(ConfigurableModuleConfig):
30
+ feature_size: int
31
+ head_dim: int = 128
32
+ num_heads: int = 32
33
+ num_kv_heads: int = 1
34
+ hidden_size: int
35
+ activation: str = 'GELU'
36
+ activation_kwargs: Dict[str, Any] = {}
37
+ norm: str = 'RMSNorm'
38
+ dropout: float = 0.0
39
+ attn_implementation: str = 'sdpa'
40
+ position_embed_config: RotaryPositionalEncodingConfig
41
+
42
+
43
+ class PiZeroFlowMatchingDecoderConfig(ConfigurableModuleConfig):
44
+ num_blocks: int
45
+ block_config: PiZeroFlowMatchingDecoderBlockConfig
46
+
47
+
48
+ class RobotStateProjectorConfig(ConfigurableModuleConfig):
49
+ layers: List[int] = []
50
+ mode: str
51
+ activation: str = 'GELU'
52
+ fourier: bool = False
53
+
54
+ def __post_init__(self):
55
+ super().__post_init__()
56
+ assert self.mode in [
57
+ 'ee_pose',
58
+ 'ee_pose_gripper',
59
+ 'ee_pose_joints',
60
+ 'joints',
61
+ 'all',
62
+ 'none',
63
+ ], self.mode
64
+
65
+
66
+ class FourierFeaturesConfig(ConfigurableModuleConfig):
67
+ num_features: int = 256
68
+ learnable_features: bool = False
69
+ max_period: float = 10000.0
70
+ layers: List[int] = [256, 512, 256]
71
+ activation: str = 'SiLU'
72
+ norm: Optional[str] = None
73
+
74
+
75
+ class NoisedControlProjectorConfig(ConfigurableModuleConfig):
76
+ time_embed: FourierFeaturesConfig
77
+ layers: List[int] = []
78
+ activation: str = 'SiLU'
79
+ norm: Optional[str] = None
80
+
81
+
82
+ class PiZeroFlowMatchingModuleConfig(ConfigurableModuleConfig):
83
+ token_size: int = 1024
84
+ noised_control_proj_config: NoisedControlProjectorConfig
85
+ robot_state_proj_config: RobotStateProjectorConfig
86
+ control_decoder_config: PiZeroFlowMatchingDecoderConfig
87
+ rotation_components: int = 3
88
+
89
+
90
+ class VLMConfig(ConfigurableModuleConfig):
91
+ pass
92
+
93
+
94
+ class InputSequencingConfig(Config):
95
+ """
96
+ past_frames_sequence_length: number of past images needed in a single robot state
97
+ past_scalars_sequence_length: number of past scalar state data, e.g. actions, poses, etc,
98
+ needed in a single robot state
99
+ past_frames_stride_sec: sampling rate, determines how far apart in time each point in the sequence
100
+ is. If None, ignored and takes the default data collection frequency from the dataset
101
+ past_scalars_stride_sec: similar to past_frames_stride_sec
102
+
103
+ sequence_frames: number of temporally-sequential points in a single example in the batch
104
+ sequence_frames_stride_sec: sampling rate
105
+
106
+ Understanding sequence_frames:
107
+ TODO: sequences are possibly useful in some rare cases, maybe sequence modeling problems,
108
+ but yet to be confirmed. Keeping for now, but could be removed if proved unnecessary
109
+
110
+ - past_scalars_sequence_length, past_frames_sequence_length, future_controls_sequence_length,
111
+ future_frames_sequence_length are hyperparameters refering to a SINGLE dataset example / 'state'.
112
+ It is assumed that `past_scalars_sequence_length` and `past_frames_sequence_length` are the min
113
+ number of observations that comprise a single 'state'
114
+ - sequence_frames is a hyperparameter refering to the entire learning process. It controls the size
115
+ of the sequence dimension in the batch. It's treated similarly to the batch dimension, with the
116
+ difference that points in the sequence dimensions are temporally aligned. Unlike `past_*`
117
+ attributes, in supervised learning a label is loaded for every point in the sequence dimension
118
+ and the loss usually computed over the entire sequence dimension.
119
+ """
120
+
121
+ past_scalars_sequence_length: int = 1
122
+ past_frames_sequence_length: int = 1
123
+ past_scalars_stride_sec: Optional[float] = None
124
+ past_frames_stride_sec: Optional[float] = None
125
+ sequence_frames: int = 1
126
+ sequence_frames_stride_sec: Optional[float] = None
127
+
128
+ def __post_init__(self):
129
+ super().__post_init__()
130
+ assert self.past_scalars_sequence_length >= 1, self.past_scalars_sequence_length
131
+ assert self.past_frames_sequence_length >= 1, self.past_frames_sequence_length
132
+ assert self.sequence_frames >= 1, self.sequence_frames
133
+ if self.past_frames_stride_sec is not None:
134
+ assert self.past_frames_stride_sec >= 0.0, self.past_frames_stride_sec
135
+ if self.past_scalars_stride_sec is not None:
136
+ assert self.past_scalars_stride_sec >= 0.0, self.past_scalars_stride_sec
137
+ if self.sequence_frames_stride_sec is not None:
138
+ assert self.sequence_frames_stride_sec >= 0.0, self.sequence_frames_stride_sec
139
+
140
+ def assert_same_past(self) -> None:
141
+ assert (
142
+ self.past_frames_stride_sec == self.past_scalars_stride_sec
143
+ ), f'{self.past_frames_stride_sec} != {self.past_scalars_stride_sec}'
144
+ assert (
145
+ self.past_frames_sequence_length == self.past_scalars_sequence_length
146
+ ), f'{self.past_frames_sequence_length} != {self.past_scalars_sequence_length}'
147
+
148
+
149
+ class OutputSequencingConfig(Config):
150
+ """
151
+ future_controls_sequence_length: number of control steps in the future the model predicts
152
+ future_frames_sequence_length: number of future frames the model predicts
153
+ (only relevant for neural networks that learn some sort of a world model)
154
+
155
+ future_controls_sequence_stride_sec / future_frames_sequence_stride_sec: sampling rate
156
+ that determines how far apart in time each point in the sequence is. If None,
157
+ ignored and takes the default data collection frequency from the dataset
158
+
159
+ future_control_offset_sec: time interval between the last observation and the first
160
+ point at which control is predicted. Serves as a 'causality hyperparameter', allowing
161
+ for predicting controls slightly further into the future in environments with dynamics
162
+ where the observed effects of an action appear slightly later
163
+ """
164
+
165
+ future_controls_sequence_length: int = 1
166
+ future_controls_sequence_stride_sec: Optional[float] = None
167
+ future_frames_sequence_length: int = 1
168
+ future_frames_sequence_stride_sec: Optional[float] = None
169
+ future_control_offset_sec: float = 0.0
170
+
171
+ def __post_init__(self):
172
+ super().__post_init__()
173
+ assert self.future_controls_sequence_length >= 1, self.future_controls_sequence_length
174
+ assert self.future_frames_sequence_length >= 1, self.future_frames_sequence_length
175
+ assert self.future_control_offset_sec >= 0.0, self.future_control_offset_sec
176
+ if self.future_controls_sequence_stride_sec is not None:
177
+ assert self.future_controls_sequence_stride_sec >= 0.0, self.future_controls_sequence_stride_sec
178
+ if self.future_frames_sequence_stride_sec is not None:
179
+ assert self.future_frames_sequence_stride_sec >= 0.0, self.future_frames_sequence_stride_sec
180
+
181
+
182
+ class ControlDataIOConfig(InputSequencingConfig, OutputSequencingConfig):
183
+ pass
184
+
185
+
186
+ class NormalizerConfig(Config):
187
+ pass
188
+
189
+
190
+ class RotationStereomapNormalizerConfig(NormalizerConfig):
191
+ factor: float
192
+
193
+
194
+ class IdentityNormalizerConfig(NormalizerConfig):
195
+ pass
196
+
197
+
198
+ class DatasetStatsNormalizerConfig(NormalizerConfig):
199
+ stats_filepath: str
200
+ stats_key: str = ''
201
+ component_name: str
202
+ mode: str
203
+
204
+ def __post_init__(self):
205
+ super().__post_init__()
206
+ assert self.mode in {'mean', 'bounds', 'bounds_q99'}, self.mode
207
+
208
+
209
+ class BoundsNormalizerConfig(NormalizerConfig):
210
+ low: List[float]
211
+ high: List[float]
212
+
213
+ def __post_init__(self):
214
+ super().__post_init__()
215
+ if len(self.low) != len(self.high):
216
+ raise ValueError(
217
+ f'Low and high bounds must have the same length, but got {self.low} and {self.high}'
218
+ )
219
+ for low, high in zip(self.low, self.high, strict=True):
220
+ assert low < high, f'Low bound {low} must be less than high bound {high}'
221
+
222
+
223
+ class ControlTokenizerConfig(Config):
224
+ pass
225
+
226
+
227
+ class EmptyTokenizerConfig(ControlTokenizerConfig):
228
+ pass
229
+
230
+
231
+ class VLAMProcessorConfig(Config):
232
+ control_io_config: ControlDataIOConfig
233
+ joints_obs_norm: BoundsNormalizerConfig
234
+ translation_obs_norm: DatasetStatsNormalizerConfig
235
+ rotation_obs_norm: IdentityNormalizerConfig
236
+ translation_control_norm: BoundsNormalizerConfig
237
+ rotation_control_norm: RotationStereomapNormalizerConfig
238
+ translation_obs_frame: ReferenceFrame = ReferenceFrame.ROBOT_BASE
239
+ rotation_obs_frame: ReferenceFrame = ReferenceFrame.ROBOT_BASE
240
+ translation_control_frame: ReferenceFrame = ReferenceFrame.ROBOT_BASE_DELTA
241
+ rotation_control_frame: ReferenceFrame = ReferenceFrame.EEF_DELTA
242
+ rotation_format: RotationFormat
243
+ image_resize: ResizeMode = ResizeMode.SMART
244
+ control_tokenizer_config: EmptyTokenizerConfig
245
+
246
+ def __post_init__(self):
247
+ super().__post_init__()
248
+ if (
249
+ self.rotation_obs_frame != ReferenceFrame.ROBOT_BASE
250
+ or self.translation_obs_frame != ReferenceFrame.ROBOT_BASE
251
+ ):
252
+ raise NotImplementedError()
253
+
254
+ @property
255
+ def delta_controls(self) -> bool:
256
+ translation_is_delta = self.translation_control_frame in (
257
+ ReferenceFrame.ROBOT_BASE_DELTA,
258
+ ReferenceFrame.EEF_DELTA,
259
+ )
260
+ rotation_is_delta = self.rotation_control_frame in (
261
+ ReferenceFrame.ROBOT_BASE_DELTA,
262
+ ReferenceFrame.EEF_DELTA,
263
+ )
264
+ if translation_is_delta != rotation_is_delta:
265
+ raise NotImplementedError(
266
+ 'Delta controls for only one of translation or rotation not yet supported'
267
+ )
268
+ return translation_is_delta
269
+
270
+
271
+ class RegressionProcessorConfig(VLAMProcessorConfig):
272
+ pass
273
+
274
+
275
+ class PiZeroFlowProcessorConfig(RegressionProcessorConfig):
276
+ num_inference_steps: int
277
+ r0_distribution: str = 'uniform'
278
+ timestep_distribution: str
279
+ distribution_hyperparams: Dict[str, Any] = {}
280
+ sig_min: float = 0.001
281
+
282
+ def __post_init__(self):
283
+ super().__post_init__()
284
+ assert self.r0_distribution in ['normal', 'uniform']
285
+ if self.rotation_obs_frame != ReferenceFrame.ROBOT_BASE:
286
+ raise NotImplementedError()
287
+
288
+
289
+ class VLMProcessorConfig(Config):
290
+ pass
291
+
292
+
293
+ class ImageSizeConfig(Config):
294
+ width: int
295
+ height: int
296
+
297
+
298
+ class PaliGemmaProcessorConfig(VLMProcessorConfig):
299
+ image_token: str = '<image>'
300
+ image_sizes: Dict[str, ImageSizeConfig] = {'main': ImageSizeConfig(width=224, height=224)}
301
+ max_language_tokens: int = -1
302
+
303
+ def __post_init__(self):
304
+ super().__post_init__()
305
+ for camera_name, camera_image_size in self.image_sizes.items():
306
+ assert camera_image_size.height % 14 == 0, f'{camera_name}: {camera_image_size}'
307
+ assert camera_image_size.width % 14 == 0, f'{camera_name}: {camera_image_size}'
308
+
309
+ @property
310
+ def num_image_tokens(self) -> Dict[str, int]:
311
+ return {
312
+ camera_name: camera_image_size.height // 14 * (camera_image_size.width // 14)
313
+ for (camera_name, camera_image_size) in self.image_sizes.items()
314
+ }
315
+
316
+ @property
317
+ def is_single_image_size(self) -> bool:
318
+ return (
319
+ len(self.image_sizes) == 1
320
+ or len(set(((image_size.height, image_size.width) for image_size in self.image_sizes.values())))
321
+ == 1
322
+ )
323
+
324
+ @property
325
+ def camera_names(self) -> List[str]:
326
+ return list(self.image_sizes.keys())
327
+
328
+
329
+ class PaliGemmaVLMConfig(VLMConfig):
330
+ model_id: str = 'google/paligemma-3b-mix-224'
331
+ attn_implementation: str = 'flash_attention_2'
332
+ processor_config: PaliGemmaProcessorConfig
333
+ lm_head: bool = False
334
+ paligemma_3d_config: Dict[str, Any] = {}
335
+ depth_tokens: int = 0
336
+ train_only_depth_tokens: bool = False
337
+ mean_resizing: bool = False
338
+
339
+ def __post_init__(self):
340
+ super().__post_init__()
341
+ if self.train_only_depth_tokens:
342
+ assert self.depth_tokens > 0, self.depth_tokens
343
+ if self.paligemma_3d_config.get('mask_prob', 0.0) != 0.0:
344
+ raise NotImplementedError(
345
+ f"Masking is deprecated, but got mask_prob={self.paligemma_3d_config['mask_prob']}"
346
+ )
347
+
348
+ @property
349
+ def paligemma_3d_config_dict(self) -> Dict[str, Any]:
350
+ config = dict(self.paligemma_3d_config)
351
+ config['depth_config'] = dict(config['depth_config'])
352
+ config['depth_config']['image_sizes'] = dict(self.processor_config.image_sizes.as_json())
353
+ return config
354
+
355
+ @property
356
+ def with_depth(self) -> bool:
357
+ return len(self.paligemma_3d_config) > 0
358
+
359
+
360
+ class VLAMConfig(ConfigurableModuleConfig):
361
+ processor_config: PiZeroFlowProcessorConfig
362
+ vlm_config: PaliGemmaVLMConfig
363
+ control_module_config: PiZeroFlowMatchingModuleConfig
364
+
365
+
366
+ MainModelConfig = VLAMConfig
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/format.log ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:95693ae71baf81e85fe7159e01807da0e2419be9be3a8f1dc16f24c945842c98
3
+ size 5524
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/model_config.yaml ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:4f892712e82673a0b76048f0d0fb4a492813c36df8ecbcc54265c66d697dac86
3
+ size 2819
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/modeling_pizero_fm_paligemma.py ADDED
The diff for this file is too large to render. See raw diff
 
sess_2026_04_02_00_03_54_msp3-4_petko_petkov_bridge_full_tread_8b_k5/hf_export/dancing-black-jellyfish/src/processing_pizero_fm_paligemma.py ADDED
@@ -0,0 +1,1913 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import warnings
2
+ from abc import abstractmethod
3
+ from functools import cached_property
4
+ from typing import Dict, List, Optional, Tuple, TypeVar
5
+
6
+ import numpy as np
7
+ import PIL.Image
8
+ import roma
9
+ import torch
10
+ import torchvision.transforms.v2
11
+ import transformers
12
+ from databib.config import Configurable
13
+ from databib.template import Template
14
+
15
+ from .common_pizero_fm_paligemma import (
16
+ FlowInput,
17
+ ReferenceFrame,
18
+ ResizeMode,
19
+ RoboticsControlPlan,
20
+ RoboticsFlowInput,
21
+ RoboticsInput,
22
+ RoboticsOutput,
23
+ RoboticsTarget,
24
+ RotationFormat,
25
+ expand_dims,
26
+ is_quaternion,
27
+ is_rotmat,
28
+ is_rotmat_3x3,
29
+ is_rotmat_9,
30
+ quaternion_half_cover,
31
+ rotmat_as_3x3,
32
+ rotmat_as_9,
33
+ rotmat_inverse,
34
+ )
35
+ from .configuration_pizero_fm_paligemma import (
36
+ BoundsNormalizerConfig,
37
+ ControlDataIOConfig,
38
+ ControlTokenizerConfig,
39
+ DatasetStatsNormalizerConfig,
40
+ EmptyTokenizerConfig,
41
+ IdentityNormalizerConfig,
42
+ ImageSizeConfig,
43
+ NormalizerConfig,
44
+ PiZeroFlowProcessorConfig,
45
+ RegressionProcessorConfig,
46
+ RotationStereomapNormalizerConfig,
47
+ VLAMProcessorConfig,
48
+ VLMProcessorConfig,
49
+ )
50
+
51
+ ControlTokenizerConfigT = TypeVar('ControlTokenizerConfigT', bound=ControlTokenizerConfig)
52
+
53
+
54
+ class ControlTokenizer(Configurable[ControlTokenizerConfigT], Template[ControlTokenizerConfigT]):
55
+ @abstractmethod
56
+ def __call__(self, *args, **kwargs) -> str:
57
+ """Given GT actions and possibly other information, output text control. Gets appened to the prompt"""
58
+
59
+
60
+ class EmptyTokenizer(ControlTokenizer[EmptyTokenizerConfig]):
61
+ """
62
+ Takes the LLM hidden states from `llm_layer_indices` and concatenates them to produce the
63
+ desired result. Includes the hidden states for the image tokens.
64
+ """
65
+
66
+ def __init__(self, config, tokenizer: transformers.PreTrainedTokenizerBase) -> None:
67
+ super().__init__(config)
68
+ self.tokenizer = tokenizer
69
+
70
+ def __call__(self, *_) -> str:
71
+ return ''
72
+
73
+
74
+ NormalizerConfigT = TypeVar('NormalizerConfigT', bound=NormalizerConfig)
75
+
76
+
77
+ class Normalizer(Configurable[NormalizerConfigT], Template[NormalizerConfigT]):
78
+ @abstractmethod
79
+ def normalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
80
+ """
81
+ Normalize the input value.
82
+
83
+ Args:
84
+ value: Tensor to be normalized
85
+ **kwargs: Implmentation-specific arguments for normalization
86
+ Returns:
87
+ Normalized tensor of the same shape as input
88
+ """
89
+
90
+ @abstractmethod
91
+ def unnormalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
92
+ """
93
+ Unnormalize the input value.
94
+
95
+ Args:
96
+ value: Tensor to be normalized
97
+ **kwargs: Implmentation-specific arguments for normalization
98
+ Returns:
99
+ Unnormalized tensor of the same shape as input
100
+ """
101
+
102
+
103
+ class IdentityNormalizer(Normalizer[IdentityNormalizerConfig]):
104
+ def normalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
105
+ del kwargs
106
+ return value
107
+
108
+ def unnormalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
109
+ del kwargs
110
+ return value
111
+
112
+
113
+ def np_unique(data: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
114
+ """
115
+ Compute unique elements in data and corresponding indices.
116
+
117
+ np.unique returns the values in a sorted order, even if the source is not sorted. Thus, if you simply
118
+ run np.unique on unsorted data, the indices you will get will be invalid.
119
+
120
+ """
121
+ (_, indices, inverse) = np.unique(data, return_index=True, return_inverse=True)
122
+ (_, indices_of_first_occurence, inverse_indices, counts) = np.unique(
123
+ indices[inverse], return_index=True, return_inverse=True, return_counts=True
124
+ )
125
+ unique_ids = data[indices_of_first_occurence]
126
+ return unique_ids, indices_of_first_occurence, inverse_indices, counts
127
+
128
+
129
+ def _broadcast_shapes(
130
+ value: torch.Tensor, low: torch.Tensor, high: torch.Tensor
131
+ ) -> Tuple[torch.Tensor, torch.Tensor]:
132
+ """
133
+ Broadcast shapes for normalization:
134
+ Args:
135
+ value: torch.Tensor of shape [..., num_components]. The entire shape might be:
136
+ - [num_components]: `value` has no batch dimension
137
+ - [num_datasets, num_components]: `value` contains entries *aligned* with the dataset bounds
138
+ contained in `low` and `high`
139
+ - [num_datasets, ..., num_components]: `value` contains entries *aligned* with the dataset bounds
140
+ contained in `low` and `high`
141
+ - [..., num_components]: `value` contains multiple dimensions. In this case, `low` and `high`
142
+ must be for a single dataset, i.e. `num_datasets = 1`
143
+
144
+ low: torch.Tensor, shape [num_datasets, num_components], where `num_datasets` can be 1 when `low`
145
+ contains normalization bounds for a single dataset
146
+ high: torch.Tensor, shape [num_datasets, num_components], where `num_datasets` can be 1 when `high`
147
+ contains normalization bounds for a single dataset
148
+ Returns:
149
+ Tuple of torch.Tensors (low, high), where `low` and `high` have the same number of dimensions as `value`
150
+ """
151
+ assert low.ndim == high.ndim == 2, f'{low.shape} != {high.shape} or ndim != 2'
152
+ assert value.shape[-1] == low.shape[-1] == high.shape[-1], f'{value.shape} != {low.shape} / {high.shape}'
153
+ if value.ndim == low.ndim == high.ndim:
154
+ return low, high
155
+ if value.ndim < low.ndim:
156
+ assert low.ndim == high.ndim == 2, f'{low.shape}, {high.shape}'
157
+ assert low.shape[0] == high.shape[0] == 1, f'{low.shape}, {high.shape}'
158
+ (low, high) = (low.view(-1), high.view(-1))
159
+ return low, high
160
+ if low.shape[0] == high.shape[0] == 1:
161
+ low = expand_dims(low.view(-1), ndim=value.ndim, order=[-1, 1])
162
+ high = expand_dims(high.view(-1), ndim=value.ndim, order=[-1, 1])
163
+ else:
164
+ assert value.shape[0] == low.shape[0] == high.shape[0], f'{value.shape} != {low.shape} / {high.shape}'
165
+ low = expand_dims(low, ndim=value.ndim, order=[1, -1, 1])
166
+ high = expand_dims(high, ndim=value.ndim, order=[1, -1, 1])
167
+ return low, high
168
+
169
+
170
+ def normalize_gripper_by_bounds(
171
+ value: torch.Tensor, low: torch.Tensor, high: torch.Tensor, binary: bool = True
172
+ ) -> torch.Tensor:
173
+ """
174
+ If binary, normalize to [0, 1], otherwise normalize to [-1, 1]
175
+ """
176
+ (low, high) = _broadcast_shapes(value, low, high)
177
+ (low, high) = (low.to(device=value.device), high.to(device=value.device))
178
+ if binary:
179
+ return torch.clamp((value - low) / torch.clamp(high - low, min=1e-08), min=0.0, max=1.0)
180
+ return torch.clamp(2 * (value - low) / torch.clamp(high - low, min=1e-08) - 1, min=-1.0, max=1.0)
181
+
182
+
183
+ def unnormalize_by_moments(value: torch.Tensor, mean: torch.Tensor, std: torch.Tensor) -> torch.Tensor:
184
+ (mean, std) = _broadcast_shapes(value, mean, std)
185
+ (mean, std) = (mean.to(device=value.device), std.to(device=value.device))
186
+ return value * (std + 1e-08) + mean
187
+
188
+
189
+ def normalize_by_moments(value: torch.Tensor, mean: torch.Tensor, std: torch.Tensor) -> torch.Tensor:
190
+ (mean, std) = _broadcast_shapes(value, mean, std)
191
+ (mean, std) = (mean.to(device=value.device), std.to(device=value.device))
192
+ return (value - mean) / (std + 1e-08)
193
+
194
+
195
+ def unnormalize_by_bounds(value: torch.Tensor, low: torch.Tensor, high: torch.Tensor) -> torch.Tensor:
196
+ (low, high) = _broadcast_shapes(value, low, high)
197
+ (low, high) = (low.to(device=value.device), high.to(device=value.device))
198
+ return 0.5 * (value + 1) * (high - low) + low
199
+
200
+
201
+ def normalize_by_bounds(value: torch.Tensor, low: torch.Tensor, high: torch.Tensor) -> torch.Tensor:
202
+ (low, high) = _broadcast_shapes(value, low, high)
203
+ (low, high) = (low.to(device=value.device), high.to(device=value.device))
204
+ return torch.clamp(2 * (value - low) / torch.clamp(high - low, min=1e-08) - 1, min=-1.0, max=1.0)
205
+
206
+
207
+ class DatasetStatsNormalizer(Normalizer[DatasetStatsNormalizerConfig]):
208
+ def __init__(self, config: DatasetStatsNormalizerConfig):
209
+ super().__init__(config)
210
+ self._norm_stats = self._load_norm_stats()
211
+
212
+ def _load_norm_stats(self) -> Dict[str, Dict[str, Dict[str, torch.Tensor]]]:
213
+ norm_stats = {
214
+ 'austin_buds_dataset': {
215
+ 'low': [0.3499317765235901, -0.2854413390159607, 0.010516085661947727],
216
+ 'high': [0.7243335843086243, 0.20652863383293152, 0.3218296766281128],
217
+ },
218
+ 'austin_sailor_dataset': {
219
+ 'low': [0.387094110250473, -0.3164229393005371, 0.024492919445037842],
220
+ 'high': [0.6869593262672424, 0.2086469978094101, 0.2551962733268738],
221
+ },
222
+ 'austin_sirius_dataset': {
223
+ 'low': [0.0, -0.11814527958631516, 0.0],
224
+ 'high': [0.532875120639801, 0.26084619760513306, 0.27225059270858765],
225
+ },
226
+ 'bc_z': {
227
+ 'low': [-0.3956047296524048, -0.11924505233764648, 0.601338267326355],
228
+ 'high': [0.332028865814209, 0.3088575601577759, 0.98329097032547],
229
+ },
230
+ 'berkeley_autolab_ur5': {
231
+ 'low': [0.3020566999912262, -0.21297279000282288, -0.18836002051830292],
232
+ 'high': [0.6132073998451233, 0.30656182765960693, 0.12212439626455307],
233
+ },
234
+ 'berkeley_cable_routing': {
235
+ 'low': [0.4641263782978058, -0.2806571424007416, 0.030183622613549232],
236
+ 'high': [0.6452807784080505, 0.28204888105392456, 0.1557157188653946],
237
+ },
238
+ 'berkeley_fanuc_manipulation': {
239
+ 'low': [0.3718133866786957, -0.4071895182132721, 0.01847645826637745],
240
+ 'high': [0.7200658321380615, 0.3128541111946106, 0.5413243770599365],
241
+ },
242
+ 'bridge': {
243
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
244
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
245
+ },
246
+ 'bridge_32b': {
247
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
248
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
249
+ },
250
+ 'bridge_coarse_max3': {
251
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
252
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
253
+ },
254
+ 'bridge_full_tread_8b_k5': {
255
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
256
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
257
+ },
258
+ 'bridge_hindsight': {
259
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
260
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
261
+ },
262
+ 'bridge_orig': {
263
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
264
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
265
+ },
266
+ 'bridge_paraphrase_k10': {
267
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
268
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
269
+ },
270
+ 'bridge_paraphrase_k5': {
271
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
272
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
273
+ },
274
+ 'bridge_steering': {
275
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
276
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
277
+ },
278
+ 'bridge_tread': {
279
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
280
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
281
+ },
282
+ 'bridge_tread_full': {
283
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
284
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
285
+ },
286
+ 'bridge_tread_k10': {
287
+ 'low': [0.1711955964565277, -0.15639324486255646, -0.048255354166030884],
288
+ 'high': [0.4604376256465912, 0.24112474918365479, 0.18886254727840424],
289
+ },
290
+ 'cmu_stretch': {
291
+ 'low': [0.017430847510695457, 0.0, 0.46050605177879333],
292
+ 'high': [0.33094948530197144, 0.0, 1.0952961444854736],
293
+ },
294
+ 'dlr_edan_shared_control': {
295
+ 'low': [-0.729511022567749, 0.077408567070961, 0.2658006250858307],
296
+ 'high': [-0.13719859719276428, 0.5719971060752869, 0.7898909449577332],
297
+ },
298
+ 'droid': {
299
+ 'low': [0.26669958233833313, -0.43774399161338806, -0.048167888075113297],
300
+ 'high': [0.7774086594581604, 0.42832574248313904, 0.7760910391807556],
301
+ },
302
+ 'fmb': {
303
+ 'low': [0.3655048608779907, -0.28729698061943054, 0.033201027661561966],
304
+ 'high': [0.6782684326171875, 0.209969624876976, 0.3331448435783386],
305
+ },
306
+ 'fractal20220817_data': {
307
+ 'low': [0.3249714970588684, -0.2818704843521118, 0.1410011649131775],
308
+ 'high': [0.8754204511642456, 0.21279653906822205, 1.071526288986206],
309
+ },
310
+ 'furniture_bench_dataset': {
311
+ 'low': [0.36915361881256104, -0.180975541472435, 0.0058300793170928955],
312
+ 'high': [0.6652880311012268, 0.1772783100605011, 0.18316447734832764],
313
+ },
314
+ 'iamlab_cmu_pickup_insert': {
315
+ 'low': [0.31449857354164124, -0.20315787196159363, 0.06785127520561218],
316
+ 'high': [0.6472027897834778, 0.20840713381767273, 0.3700340986251831],
317
+ },
318
+ 'jaco_play': {
319
+ 'low': [-0.3789186179637909, -0.6194459795951843, 0.16865813732147217],
320
+ 'high': [0.21203258633613586, -0.26914602518081665, 0.38958534598350525],
321
+ },
322
+ 'kuka': {
323
+ 'low': [0.4765772819519043, -0.14815208315849304, 0.06674224138259888],
324
+ 'high': [0.6515637040138245, 0.2447487711906433, 0.28018367290496826],
325
+ },
326
+ 'language_table': {
327
+ 'low': [0.19237099587917328, -0.2962527573108673, 0.0],
328
+ 'high': [0.6171894669532776, 0.30645298957824707, 0.0],
329
+ },
330
+ 'nyu_franka_play_dataset': {
331
+ 'low': [0.13936959207057953, 0.07645522058010101, 0.19364508986473083],
332
+ 'high': [0.5920727252960205, 0.6584802269935608, 0.8056891560554504],
333
+ },
334
+ 'roboset': {
335
+ 'low': [0.18437016010284424, -0.25699371099472046, 0.15134164690971375],
336
+ 'high': [0.543661892414093, 0.29646238684654236, 0.6682320833206177],
337
+ },
338
+ 'roboturk': {
339
+ 'low': [0.28454264998435974, -0.3288349509239197, -0.09349551796913147],
340
+ 'high': [0.8773894309997559, 0.2857522964477539, 0.32863926887512207],
341
+ },
342
+ 'stanford_hydra_dataset': {
343
+ 'low': [0.23737286031246185, -0.26521679759025574, 0.09069013595581055],
344
+ 'high': [0.7124238014221191, 0.25299057364463806, 0.49505406618118286],
345
+ },
346
+ 'taco_play': {
347
+ 'low': [0.1368357390165329, -0.4297449290752411, 0.20516259968280792],
348
+ 'high': [0.6700438857078552, 0.5943909883499146, 0.5966404676437378],
349
+ },
350
+ 'toto': {
351
+ 'low': [-0.09177927672863007, -0.3571659028530121, 0.2196546494960785],
352
+ 'high': [0.6757593750953674, 0.2889021635055542, 0.5011094212532043],
353
+ },
354
+ 'ucsd_kitchen_dataset': {
355
+ 'low': [0.18739914894104004, -0.18234309554100037, 0.04897069185972214],
356
+ 'high': [0.6410437822341919, 0.20632223784923553, 0.5983893275260925],
357
+ },
358
+ 'utaustin_mutex': {
359
+ 'low': [0.3217194080352783, -0.4733337163925171, 0.014122226275503635],
360
+ 'high': [0.5321439504623413, 0.3733823001384735, 0.5785381197929382],
361
+ },
362
+ 'viola': {
363
+ 'low': [0.40061360597610474, -0.25196850299835205, 0.010269512422382832],
364
+ 'high': [0.6458418369293213, 0.17776551842689514, 0.4456312954425812],
365
+ },
366
+ }
367
+ return {
368
+ dataset_name: {
369
+ key: torch.tensor(value, dtype=torch.float32) for (key, value) in dataset_stats.items()
370
+ }
371
+ for (dataset_name, dataset_stats) in norm_stats.items()
372
+ }
373
+
374
+ def _broadcast_norm_stats_to_dataset_name(
375
+ self, dataset_name: np.ndarray
376
+ ) -> Tuple[torch.Tensor, torch.Tensor]:
377
+ """
378
+ Create an array of normalization bounds corresponding to dataset names
379
+ Args:
380
+ dataset_name: Array of shape [B] of dataset names for which to fetch normalization stats.
381
+ Note the values can be repeated
382
+ Returns:
383
+ Tuple of (low, high) or (norm, std) stats, each of shape [B, -1]
384
+ """
385
+ if self.config.mode == 'mean':
386
+ (stats_key_1, stats_key_2) = ('mean', 'std')
387
+ else:
388
+ (stats_key_1, stats_key_2) = ('low', 'high')
389
+ (unique_names, _, inverse_indices, _) = np_unique(dataset_name)
390
+ stats_1 = np.zeros([len(unique_names), self._component_size], dtype=np.float32)
391
+ stats_2 = np.zeros([len(unique_names), self._component_size], dtype=np.float32)
392
+ for i, ds_name in enumerate(unique_names):
393
+ stats_1[i] = self._norm_stats[ds_name][stats_key_1].numpy()
394
+ stats_2[i] = self._norm_stats[ds_name][stats_key_2].numpy()
395
+ stats_1 = stats_1[inverse_indices]
396
+ stats_2 = stats_2[inverse_indices]
397
+ return torch.from_numpy(stats_1), torch.from_numpy(stats_2)
398
+
399
+ @property
400
+ def _component_size(self) -> int:
401
+ return list(list(self._norm_stats.values())[0].values())[0].shape[-1]
402
+
403
+ def normalize(self, value: torch.Tensor, dataset_name: np.ndarray, **kwargs) -> torch.Tensor:
404
+ del kwargs
405
+ if self.config.mode == 'mean':
406
+ (mean, std) = self._broadcast_norm_stats_to_dataset_name(dataset_name)
407
+ output = normalize_by_moments(value, mean=mean, std=std)
408
+ else:
409
+ (low, high) = self._broadcast_norm_stats_to_dataset_name(dataset_name)
410
+ output = normalize_by_bounds(value, low=low, high=high)
411
+ return output
412
+
413
+ def unnormalize(self, value: torch.Tensor, dataset_name: np.ndarray, **kwargs) -> torch.Tensor:
414
+ del kwargs
415
+ if self.config.mode == 'mean':
416
+ (mean, std) = self._broadcast_norm_stats_to_dataset_name(dataset_name)
417
+ output = unnormalize_by_moments(value, mean=mean, std=std)
418
+ else:
419
+ (low, high) = self._broadcast_norm_stats_to_dataset_name(dataset_name)
420
+ output = unnormalize_by_bounds(value, low=low, high=high)
421
+ return output
422
+
423
+
424
+ class BoundsNormalizer(Normalizer[BoundsNormalizerConfig]):
425
+ def __init__(self, config: BoundsNormalizerConfig):
426
+ super().__init__(config)
427
+ self.low = torch.tensor(self.config.low, dtype=torch.float32).view(1, -1)
428
+ self.high = torch.tensor(self.config.high, dtype=torch.float32).view(1, -1)
429
+
430
+ def normalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
431
+ del kwargs
432
+ return normalize_by_bounds(value, low=self.low, high=self.high)
433
+
434
+ def unnormalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
435
+ del kwargs
436
+ return unnormalize_by_bounds(value, low=self.low, high=self.high)
437
+
438
+
439
+ def euler_to_rotmat(angles: torch.Tensor) -> torch.Tensor:
440
+ """
441
+ Args:
442
+ angles: Euler angles in radians in the format 'xyz', shape [..., 3]
443
+ Returns:
444
+ torch.Tensor of shape [..., 3, 3] containing rotation matrices
445
+ """
446
+ return roma.euler_to_rotmat(convention='xyz', angles=angles, degrees=False)
447
+
448
+
449
+ def euler_to_unit_quaternion(angles: torch.Tensor) -> torch.Tensor:
450
+ """
451
+ Args:
452
+ angles: Euler angles in radians in the format 'xyz', shape [..., 3]
453
+ Returns:
454
+ torch.Tensor of shape [..., 4] containing unit quaternions
455
+ """
456
+ return roma.euler_to_unitquat(convention='xyz', angles=angles, degrees=False, normalize=True)
457
+
458
+
459
+ def normalize_quaternion(quaternion: torch.Tensor, eps: float = 1e-08) -> torch.Tensor:
460
+ """
461
+ Args:
462
+ quaternion: Unnormalized quaternion, torch.Tensor of shape [..., 4]
463
+ eps: Small constant to prevent division by zero
464
+ Returns:
465
+ torch.Tensor of shape [..., 4] of unit quaternions
466
+ """
467
+ return quaternion / (quaternion.norm(dim=-1, keepdim=True).detach() + eps)
468
+
469
+
470
+ def quaternion_to_euler(quaternion: torch.Tensor) -> torch.Tensor:
471
+ """
472
+ Args:
473
+ quaternion: torch.Tensor of shape [..., 4]; Can be non-normalized
474
+ Returns:
475
+ torch.Tensor of shape [..., 3, 3] containing rotation matrices in SO(3)
476
+ """
477
+ unit_quat = normalize_quaternion(quaternion)
478
+ rotmat = roma.unitquat_to_euler(convention='xyz', quat=unit_quat, as_tuple=False, degrees=False)
479
+ return rotmat
480
+
481
+
482
+ def quaternion_to_rotmat(quaternion: torch.Tensor) -> torch.Tensor:
483
+ """
484
+ Args:
485
+ quaternion: torch.Tensor of shape [..., 4]; Can be non-normalized
486
+ Returns:
487
+ torch.Tensor of shape [..., 3, 3] containing rotation matrices in SO(3)
488
+ """
489
+ unit_quat = normalize_quaternion(quaternion)
490
+ rotmat = roma.unitquat_to_rotmat(unit_quat)
491
+ return rotmat
492
+
493
+
494
+ def rotmat_to_unit_quaternion(rotmat: torch.Tensor) -> torch.Tensor:
495
+ """
496
+ Args:
497
+ rotmat: Batch of rotation matrices, shape [..., 3, 3]
498
+ Returns:
499
+ Batch of unit quaternions, shape [..., 4]
500
+ """
501
+ rotmat = rotmat_as_3x3(rotmat)
502
+ return roma.rotmat_to_unitquat(rotmat)
503
+
504
+
505
+ def rotmat_to_euler(rotmat: torch.Tensor) -> torch.Tensor:
506
+ """
507
+ Args:
508
+ rotmat: Batch of rotation matrices, shape [..., 3, 3]
509
+ Returns:
510
+ Batch of Euler angles in radiant, shape [..., 3]
511
+ """
512
+ rotmat = rotmat_as_3x3(rotmat)
513
+ return roma.rotmat_to_euler(convention='xyz', rotmat=rotmat, as_tuple=False, degrees=False)
514
+
515
+
516
+ def symmetric_orthogonalization(x: torch.Tensor) -> torch.Tensor:
517
+ """
518
+ Maps 9D input vectors onto SO(3) via symmetric orthogonalization.
519
+ - Let SVD(M) = U \Sigma V^T
520
+ - Returned value is SVD+(M) = U diag(1, 1, det(UV^T)) V^T
521
+ - det(UV^T) ensures that det(SVD+(M)) = 1
522
+ - The return value is a rotation matrix (ortonormal) with the least-squares distance to M
523
+
524
+ Args:
525
+ x: Input matrices, not necessarily orthonormal, shape [..., 9] or [..., 3, 3]
526
+ Returns:
527
+ torch.Tensor with the same shape as x, where each inner 3x3 matrix is in SO(3)
528
+ """
529
+ with warnings.catch_warnings():
530
+ warnings.filterwarnings(
531
+ 'ignore', message='In CPU autocast, but the target dtype is not supported. Disabling autocast.'
532
+ )
533
+ with torch.autocast(device_type=x.device.type, dtype=torch.float32):
534
+ matrices = x.view(-1, 3, 3)
535
+ matrices = matrices.to(dtype=torch.float32)
536
+ (u, s, v) = torch.svd(matrices)
537
+ vt = torch.transpose(v, 1, 2)
538
+ det = torch.det(torch.matmul(u, vt)).view(-1, 1, 1)
539
+ diag_vt = torch.cat((vt[:, :2, :], vt[:, -1:, :] * det), dim=1)
540
+ result = torch.matmul(u, diag_vt)
541
+ result = result.view(*x.shape)
542
+ result = result.to(dtype=x.dtype)
543
+ return result
544
+
545
+
546
+ def is_rotmat_orthonormal(
547
+ rotmat: torch.Tensor, epsilon: float = 1e-06, reduction: str = 'none'
548
+ ) -> torch.Tensor | bool:
549
+ """
550
+ Check if a rotation matrix is orthonormal or not.
551
+ Args:
552
+ rotmat: torch.Tensor of shape [..., 3, 3] or [..., 9]
553
+ epsilon: Tolerance for numerical comparisons. Bigger values allow for more freedom. Generally,
554
+ anything smaller than 1e-6 might incorrectly detect some otrhonormal matrices as not
555
+ reduction:
556
+ 'none' - returns torch.Tensor of bools with the same batch shape
557
+ 'all' - returns a bool, True is ALL matrices in the batch are orthonormal
558
+ Returns:
559
+ torch.Tensor with the same batch shape or bool
560
+ """
561
+ assert is_rotmat(rotmat)
562
+ rotmat = rotmat_as_3x3(rotmat.to(dtype=torch.float32))
563
+ is_orthonormal = roma.is_orthonormal_matrix(rotmat, epsilon=epsilon)
564
+ if reduction == 'none':
565
+ return is_orthonormal
566
+ if reduction == 'all':
567
+ return bool(torch.all(is_orthonormal).item())
568
+ raise ValueError(f'Unknown reduction mode {reduction}')
569
+
570
+
571
+ def is_orthonormal_rotmat(rotmat: torch.Tensor, epsilon=0.01, reduction='none') -> bool:
572
+ """
573
+ Checks if the tensor shape matches that of a rotmat. If the last dimensions of shape are 3x3,
574
+ also checks if the data is a valid rotmat. This is to avoid a possible clash with euler angles
575
+ when accidentally `rotmat.shape[-2:] == [3, 3]`
576
+ """
577
+ return (
578
+ is_rotmat_9(rotmat)
579
+ or is_rotmat_3x3(rotmat)
580
+ and is_rotmat_orthonormal(rotmat, epsilon=epsilon, reduction=reduction)
581
+ )
582
+
583
+
584
+ def is_euler(euler: torch.Tensor) -> bool:
585
+ return euler.shape[-1] == 3 and not is_orthonormal_rotmat(euler, reduction='all')
586
+
587
+
588
+ def normalize_rotation(rotation: torch.Tensor) -> torch.Tensor:
589
+ if is_quaternion(rotation):
590
+ return normalize_quaternion(rotation)
591
+ if is_euler(rotation):
592
+ return rotation
593
+ if is_rotmat(rotation):
594
+ is_flat = is_rotmat_9(rotation)
595
+ rotation = rotmat_as_3x3(rotation) if is_flat else rotation
596
+ rotmat = roma.special_gramschmidt(rotation)
597
+ rotmat = rotmat_as_9(rotmat) if is_flat else rotmat
598
+ return rotmat
599
+ raise ValueError(f'Unknown rotation format: {rotation.shape}')
600
+
601
+
602
+ def rotation_format_from_tensor(rotation) -> RotationFormat:
603
+ if is_quaternion(rotation):
604
+ return RotationFormat.QUATERNION
605
+ if is_orthonormal_rotmat(rotation, reduction='all'):
606
+ return RotationFormat.ROTMAT
607
+ if is_euler(rotation):
608
+ return RotationFormat.EULER
609
+ raise ValueError(f'Tensor shape {rotation.shape} is not a valid rotation format')
610
+
611
+
612
+ def is_unit_quaternion(
613
+ quaternion: torch.Tensor, epsilon: float = 1e-08, reduction: str = 'none'
614
+ ) -> torch.Tensor | bool:
615
+ """
616
+ Check if a quternion is normalized or not.
617
+ Args:
618
+ quaternion: torch.Tensor of shape [..., 4]
619
+ tolerance: Tolerance for numerical comparisons
620
+ reduction:
621
+ 'none' - returns torch.Tensor of bools with the same batch shape
622
+ 'all' - returns a bool, True if ALL quaternions in the batch are normalized
623
+ Returns:
624
+ torch.Tensor with the same batch shape or bool
625
+ """
626
+ if not is_quaternion(quaternion):
627
+ return False
628
+ is_norm = torch.isclose(
629
+ quaternion.norm(dim=-1, keepdim=True),
630
+ torch.tensor(1.0, dtype=quaternion.dtype, device=quaternion.device),
631
+ atol=epsilon,
632
+ )
633
+ if reduction == 'none':
634
+ return is_norm
635
+ if reduction == 'all':
636
+ return bool(torch.all(is_norm).item())
637
+ raise ValueError(f'Unknown reduction mode {reduction}')
638
+
639
+
640
+ def convert_rotation(
641
+ rotation: torch.Tensor | np.ndarray,
642
+ output_format: RotationFormat,
643
+ autonorm: bool = True,
644
+ half_cover: bool = True,
645
+ ) -> torch.Tensor | np.ndarray:
646
+ is_np = isinstance(rotation, np.ndarray)
647
+ if is_np:
648
+ rotation = torch.from_numpy(rotation)
649
+ if is_quaternion(rotation):
650
+ if autonorm and not is_unit_quaternion(rotation, reduction='all'):
651
+ rotation = normalize_quaternion(rotation)
652
+ if output_format == RotationFormat.QUATERNION:
653
+ output = rotation
654
+ elif output_format == RotationFormat.ROTMAT:
655
+ output = rotmat_as_9(quaternion_to_rotmat(rotation))
656
+ elif output_format == RotationFormat.EULER:
657
+ output = quaternion_to_euler(rotation)
658
+ else:
659
+ raise NotImplementedError(f'Unsupported rotation format: {output_format}')
660
+ elif is_orthonormal_rotmat(rotation, reduction='all'):
661
+ if autonorm and not is_rotmat_orthonormal(rotation, epsilon=0.01, reduction='all'):
662
+ rotation = symmetric_orthogonalization(rotation)
663
+ if output_format == RotationFormat.QUATERNION:
664
+ output = rotmat_to_unit_quaternion(rotation)
665
+ elif output_format == RotationFormat.ROTMAT:
666
+ output = rotmat_as_9(rotation)
667
+ elif output_format == RotationFormat.EULER:
668
+ output = rotmat_to_euler(rotation)
669
+ else:
670
+ raise NotImplementedError(f'Unsupported rotation format: {output_format}')
671
+ elif is_euler(rotation):
672
+ if output_format == RotationFormat.QUATERNION:
673
+ output = euler_to_unit_quaternion(rotation)
674
+ elif output_format == RotationFormat.ROTMAT:
675
+ output = rotmat_as_9(euler_to_rotmat(rotation))
676
+ elif output_format == RotationFormat.EULER:
677
+ output = rotation
678
+ else:
679
+ raise NotImplementedError(f'Unsupported rotation format: {output_format}')
680
+ else:
681
+ raise ValueError(f'Unknown rotation encoding with shape {rotation.shape}')
682
+ if output_format == RotationFormat.QUATERNION and half_cover:
683
+ output = quaternion_half_cover(output)
684
+ if is_np:
685
+ output = output.numpy()
686
+ return output
687
+
688
+
689
+ def apply_rotation(rotation: torch.Tensor, value: torch.Tensor) -> torch.Tensor:
690
+ """
691
+ Rotate `value` by `rotation`
692
+ Args:
693
+ rotation: torch.Tensor, euler, quaternion or rotmat. Any batch shape that can be expanded
694
+ such that it broadcasts to `value`
695
+ value: torch.Tensor. Supported shapes:
696
+ - Rotmat: [B, ..., 3, 3] or [B, ..., 9]
697
+ - Quaternion: [B, ..., 4]
698
+ - 3D vector: [B, ..., 3]
699
+ Returns:
700
+ torch.Tensor of the same shape as `value`
701
+ """
702
+ rotation = rotmat_as_3x3(convert_rotation(rotation, RotationFormat.ROTMAT))
703
+ quaternion = is_quaternion(value)
704
+ if quaternion:
705
+ value = convert_rotation(value, RotationFormat.ROTMAT)
706
+ if is_orthonormal_rotmat(value, reduction='all'):
707
+ if is_rotmat_9(value):
708
+ assert rotation.ndim <= value.ndim + 1, f'{rotation.shape}, {value.shape}'
709
+ if rotation.ndim > 2:
710
+ rotation = expand_dims(
711
+ rotation, ndim=value.ndim + 1, order=[1, -1] + [1] * (rotation.ndim - 3) + [1, 1]
712
+ )
713
+ value = rotmat_as_9(torch.matmul(rotation, rotmat_as_3x3(value)))
714
+ else:
715
+ assert rotation.ndim <= value.ndim, f'{rotation.shape}, {value.shape}'
716
+ if rotation.ndim > 2:
717
+ rotation = expand_dims(
718
+ rotation, ndim=value.ndim, order=[1, -1] + [1] * (rotation.ndim - 3) + [1, 1]
719
+ )
720
+ value = torch.matmul(rotation, value)
721
+ else:
722
+ assert value.shape[-1] == 3, f'Expected a 3-dim vector in last dim, but got shape: {value.shape}'
723
+ assert rotation.ndim <= value.ndim + 1, f'{rotation.shape}, {value.shape}'
724
+ if rotation.ndim > 2:
725
+ rotation = expand_dims(
726
+ rotation, ndim=value.ndim + 1, order=[1, -1] + [1] * (rotation.ndim - 3) + [1, 1]
727
+ )
728
+ value = torch.matmul(rotation, value.unsqueeze(-1)).squeeze(-1)
729
+ if quaternion:
730
+ value = convert_rotation(value, RotationFormat.QUATERNION)
731
+ return value
732
+
733
+
734
+ def relative_to_delta_rotations(
735
+ rotation_sequence: torch.Tensor, encoding_frame: ReferenceFrame
736
+ ) -> torch.Tensor:
737
+ """
738
+ Transform a sequence of rotation representations encoded w.r.t. the same reference frame to delta
739
+ rotations where each element is encoded w.r.t. the PREVIOUS rotation frame in the sequence.
740
+ The first element in the sequence remains the same.
741
+
742
+ Ex:
743
+ Sequence of points (rotations): R_1, R_2, R_3, R_4
744
+ `rotation_sequence` contains the rotations: R_01, R_02, R_03, R_04, where 0 is the reference frame
745
+ and R_01 is the pose of R1 frame in the reference frame 0, i.e. R_10 converts from reference
746
+ frame to R1 frame
747
+ Output: R_01, R_12, R_23, R_34, i.e. the rotation poses of R_1 in 0 frame, of R_2 in R1 frame, etc
748
+
749
+ Args:
750
+ rotation_sequence: torch.Tensor of shape [..., S, 9], [..., S, 3, 3] or [..., S, 4], containing
751
+ either rotation matrices (R_01, R_12, R_23, R_34, ...) or quaternions, where S corresponds
752
+ to the sequence dimension
753
+ encoding_frame: Indicates the frame w.r.t. which the input rotations are expressed.
754
+ - EEF: Input rotations are fully expressed w.r.t. 0-th reference frame,
755
+ (i.e. the axis of rotation is defined in 0-th reference frame)
756
+ R_12 = R_01^-1 @ R_02
757
+ R_23 = R_12^-1 @ R_03
758
+ - ROBOT_BASE: Input rotations are still relative, but the
759
+ axis of rotation is defined in robot base frame
760
+ R_12 = R_01^-1 @ R_02
761
+ R_23 = R_12^-1 @ R_03
762
+ - All other EEF or ROBOT_BASE frames treated accordingly
763
+ Returns:
764
+ torch.Tensor of the same shape as rotation_sequence, containing delta rotations
765
+ """
766
+ assert rotation_sequence.ndim >= 3, rotation_sequence.shape
767
+ rotation_format: RotationFormat = rotation_format_from_tensor(rotation_sequence)
768
+ rotation_sequence = convert_rotation(rotation_sequence, RotationFormat.QUATERNION)
769
+ reference_sequence = torch.roll(rotation_sequence, 1, dims=-2).clone()
770
+ reference_sequence[..., 0, :] = roma.identity_quat()
771
+ reference_sequence = roma.quat_inverse(reference_sequence)
772
+ if encoding_frame in ReferenceFrame.eef_frames:
773
+ delta_rotations = roma.quat_product(reference_sequence, rotation_sequence)
774
+ elif encoding_frame in ReferenceFrame.robot_frames:
775
+ delta_rotations = roma.quat_product(rotation_sequence, reference_sequence)
776
+ else:
777
+ raise NotImplementedError(f'Encoding frame {encoding_frame} not implemented')
778
+ delta_rotations = convert_rotation(delta_rotations, rotation_format)
779
+ return delta_rotations
780
+
781
+
782
+ def delta_to_relative_rotations(
783
+ rotation_sequence: torch.Tensor, encoding_frame: ReferenceFrame
784
+ ) -> torch.Tensor:
785
+ """
786
+ Transform a sequence of rotation representations encoded w.r.t. the PREVIOUS rotation frame in the
787
+ sequence to the 0-th element preceding the sequence
788
+
789
+ Ex:
790
+ `rotation_sequence` contains the rotations: R_01, R_12, R_23, R_34, where R0 is the base frame,
791
+ implicitly encoded in R_01 and R_10 converts from R0 frame to R1 frame
792
+ Output: R_01, R_02, R_03, R_04
793
+
794
+ Args:
795
+ rotation_sequence: torch.Tensor of shape [..., S, 9], [..., S, 3, 3] or [..., S, 4], containing
796
+ either rotation matrices (R_01, R_12, R_23, R_34, ...) or quaternions
797
+ Returns:
798
+ torch.Tensor of shape [..., S, 9], [..., S, 3, 3] or [..., S, 4] containing transformed rotations
799
+ (R_01, R_02, R_03, R_04, ...)
800
+ """
801
+ assert rotation_sequence.ndim >= 3, rotation_sequence.shape
802
+ rotation_format: RotationFormat = rotation_format_from_tensor(rotation_sequence)
803
+ rotation_sequence = convert_rotation(rotation_sequence, RotationFormat.QUATERNION)
804
+ rotation_sequence = rotation_sequence.clone()
805
+ cumulative = rotation_sequence[..., :1, :]
806
+ delta_rotations = [cumulative]
807
+ for i in range(2, rotation_sequence.shape[-2] + 1):
808
+ if encoding_frame in ReferenceFrame.eef_frames:
809
+ cumulative = roma.quat_product(cumulative, rotation_sequence[..., i - 1 : i, :])
810
+ elif encoding_frame in ReferenceFrame.robot_frames:
811
+ cumulative = roma.quat_product(rotation_sequence[..., i - 1 : i, :], cumulative)
812
+ else:
813
+ raise NotImplementedError(f'Encoding frame {encoding_frame} not implemented')
814
+ delta_rotations.append(cumulative)
815
+ delta_rotations = torch.cat(delta_rotations, dim=-2)
816
+ delta_rotations = convert_rotation(delta_rotations, rotation_format)
817
+ return delta_rotations
818
+
819
+
820
+ def world_to_relative_rotations(
821
+ rotation_sequence: torch.Tensor, reference_rotation: torch.Tensor, encoding_frame: ReferenceFrame
822
+ ) -> torch.Tensor:
823
+ """
824
+ Transform a sequence of rotations expressed w.r.t. WORLD frame to relative rotations w.r.t.
825
+ `reference_rotation`, where `reference_rotation` is provided w.r.t. WORLD frame.
826
+
827
+ Ex:
828
+ Sequence of points (rotations): R_0, R_1, R_2, R_3, R_4
829
+ `rotation_sequence` contains the rotations: R_W1, R_W2, R_W3, R_W4, where W is the world frame
830
+ and R_W1 is the pose of R1 frame in world frame, i.e. R_1W converts from world frame to R1 frame
831
+ `reference_rotation`: R_W0
832
+ Output: R_01, R_02, R_03, R_04 -> the rotation poses of R_1, R_2, R_3, R_4 expressed in R_0 frame
833
+
834
+ Args:
835
+ rotation_sequence: torch.Tensor of shape [..., S, 9], [..., S, 3, 3] or [..., S, 4], containing
836
+ either rotation matrices (R_W1, R_W2, R_W3, R_W4, ...) or quaternions
837
+ reference_rotation: torch.Tensor, shape [..., 9], [..., 3, 3] or [..., 4] and the SAME number of BATCH
838
+ dims as `rotation_sequence`. The new reference frame, provided w.r.t. WORLD coordinate frame R_W0
839
+ encoding_frame: Indicates the frame w.r.t. which the output rotations would be encoded - the fixed
840
+ world frame (ROBOT_BASE) or the local reference_frame (EEF)
841
+ - EEF: Output rotations are fully expressed w.r.t. reference_rotation
842
+ (i.e. the axis of rotation is defined in reference frame)
843
+ R_W1 = R_W0 @ R_01 <=> R_01 = R_0W @ R_W1
844
+ - ROBOT_BASE: Output rotations are still relative, but
845
+ the axis of rotation is defined in robot base frame
846
+ R_W1 = R_01 @ R_W0 <=> R_01 = R_W1 @ R_0W
847
+ - All other EEF or ROBOT_BASE frames treated accordingly
848
+ Returns:
849
+ torch.Tensor of shape [..., S, 9], [..., S, 3, 3] or [..., S, 4] containing transformed rotations
850
+ (R_01, R_02, R_03, R_04, ...)
851
+ """
852
+ assert rotation_sequence.ndim >= 3, rotation_sequence.shape
853
+ rotation_format: RotationFormat = rotation_format_from_tensor(rotation_sequence)
854
+ reference_rotation = rotmat_as_3x3(convert_rotation(reference_rotation, RotationFormat.ROTMAT))
855
+ rotation_sequence = rotmat_as_3x3(convert_rotation(rotation_sequence, RotationFormat.ROTMAT))
856
+ if reference_rotation.ndim != rotation_sequence.ndim:
857
+ raise ValueError(
858
+ f'Cannot broadcast reference_rotation of shape {reference_rotation.shape} to rotation_sequence of shape {rotation_sequence.shape}. Provide tensors with the same number of batch dimensions'
859
+ )
860
+ R_0W = rotmat_as_3x3(rotmat_inverse(reference_rotation))
861
+ if encoding_frame in ReferenceFrame.eef_frames:
862
+ relative_rotations = torch.matmul(R_0W, rotation_sequence)
863
+ elif encoding_frame in ReferenceFrame.robot_frames:
864
+ relative_rotations = torch.matmul(rotation_sequence, R_0W)
865
+ else:
866
+ raise NotImplementedError(f'Encoding frame {encoding_frame} not implemented')
867
+ relative_rotations = convert_rotation(relative_rotations, rotation_format)
868
+ return relative_rotations
869
+
870
+
871
+ def rotation_to_target_frame(
872
+ rotation: torch.Tensor,
873
+ source_frame: ReferenceFrame,
874
+ target_frame: ReferenceFrame,
875
+ ee_pose_rotation: Optional[torch.Tensor] = None,
876
+ ) -> torch.Tensor:
877
+ """
878
+ Convert rotation sequence from source_frame to target_frame
879
+ Args:
880
+ rotation: torch.Tensor of shape [..., S, 9 | 4 | 3 x 3], containing
881
+ the rotations, where S corresponds to the sequence dimension
882
+ source_frame: indicates the frame w.r.t. which `rotation` is expressed
883
+ target_frame: indicates the frame w.r.t. which the output rotation should be expressed
884
+ ee_pose_rotation: torch.Tensor of shape [..., 9 | 4 | 3 x 3], containing the rotation of the
885
+ current end-effector pose w.r.t. ROBOT_BASE frame. Required only when source_frame and
886
+ target_frame have different core reference frames.
887
+ Returns:
888
+ torch.Tensor of the same shape as rotation, containing the converted rotations
889
+ """
890
+ if source_frame == target_frame:
891
+ return rotation
892
+ assert source_frame in ReferenceFrame.robot_frames | ReferenceFrame.eef_frames, source_frame
893
+ assert target_frame in ReferenceFrame.robot_frames | ReferenceFrame.eef_frames, target_frame
894
+ if ee_pose_rotation is not None:
895
+ ee_pose_rotation = rotmat_as_3x3(convert_rotation(ee_pose_rotation, RotationFormat.ROTMAT))
896
+ if source_frame.to_core() != target_frame.to_core():
897
+ assert ee_pose_rotation is not None, f'{source_frame}, {target_frame}'
898
+ if source_frame in ReferenceFrame.delta_frames:
899
+ rotation = delta_to_relative_rotations(rotation, encoding_frame=source_frame)
900
+ source_frame = source_frame.to_relative()
901
+ if target_frame in ReferenceFrame.robot_frames:
902
+ assert source_frame == ReferenceFrame.EEF_RELATIVE, source_frame
903
+ rotation = world_to_relative_rotations(
904
+ rotation, reference_rotation=rotmat_inverse(ee_pose_rotation), encoding_frame=source_frame
905
+ )
906
+ source_frame = ReferenceFrame.ROBOT_BASE
907
+ elif target_frame in ReferenceFrame.eef_frames:
908
+ assert source_frame in ReferenceFrame.robot_frames, source_frame
909
+ if source_frame == ReferenceFrame.ROBOT_BASE_RELATIVE:
910
+ rotation = world_to_relative_rotations(
911
+ rotation, reference_rotation=rotmat_inverse(ee_pose_rotation), encoding_frame=source_frame
912
+ )
913
+ source_frame = ReferenceFrame.ROBOT_BASE
914
+ rotation = world_to_relative_rotations(
915
+ rotation, reference_rotation=ee_pose_rotation, encoding_frame=target_frame
916
+ )
917
+ source_frame = target_frame.to_relative()
918
+ assert source_frame.to_core() == target_frame.to_core(), f'{source_frame}, {target_frame}'
919
+ if source_frame == target_frame:
920
+ return rotation
921
+ if (
922
+ source_frame in ReferenceFrame.delta_frames
923
+ and target_frame in ReferenceFrame.relative_frames | ReferenceFrame.core_frames
924
+ ):
925
+ rotation = delta_to_relative_rotations(rotation, encoding_frame=source_frame)
926
+ source_frame = source_frame.to_relative()
927
+ elif source_frame == ReferenceFrame.ROBOT_BASE:
928
+ assert ee_pose_rotation is not None
929
+ rotation = world_to_relative_rotations(
930
+ rotation, reference_rotation=ee_pose_rotation, encoding_frame=source_frame
931
+ )
932
+ source_frame = ReferenceFrame.ROBOT_BASE_RELATIVE
933
+ assert source_frame in ReferenceFrame.relative_frames, source_frame
934
+ if target_frame in ReferenceFrame.delta_frames:
935
+ rotation = relative_to_delta_rotations(rotation, encoding_frame=source_frame)
936
+ source_frame = source_frame.to_delta()
937
+ elif target_frame == ReferenceFrame.ROBOT_BASE:
938
+ rotation = world_to_relative_rotations(
939
+ rotation, reference_rotation=rotmat_inverse(ee_pose_rotation), encoding_frame=source_frame
940
+ )
941
+ source_frame = ReferenceFrame.ROBOT_BASE
942
+ assert source_frame == target_frame, f'{source_frame}, {target_frame}'
943
+ return rotation
944
+
945
+
946
+ def stereographic_map_quaternion(
947
+ quaternion: torch.Tensor, k: float, inverse: bool, eps: float = 1e-08
948
+ ) -> torch.Tensor:
949
+ """
950
+ Forward or inverse 1-1 quaternion remapping on S^3 using stereographic linear map.
951
+ Forward map:
952
+ theta' = 4 * arctan(k * tan(theta / 4)) where q = [cos(theta), sin(theta)*axis]
953
+ Inverse map:
954
+ theta = 4 * arctan( 1/k * tan(theta' / 4))
955
+
956
+ Args:
957
+ quaternion: torch.Tensor of shape [..., 4], input quaternion
958
+ k: positive scalar stretch factor
959
+ eps: numerical stability constant.
960
+
961
+ Returns:
962
+ torh.Tensor of shape [..., 4], mapped quaternion
963
+ """
964
+ assert k > 0, f'Stretch factor k must be positive, but got {k}'
965
+ assert is_quaternion(quaternion), f'{quaternion.shape} not a quaternion'
966
+ rotvec = roma.unitquat_to_rotvec(quaternion)
967
+ theta = torch.norm(rotvec, dim=-1, keepdim=True)
968
+ k_eff = k if not inverse else 1.0 / k
969
+ theta_prime = 4.0 * torch.atan(k_eff * torch.tan(torch.clamp(theta / 4.0, min=0, max=torch.pi / 2 - eps)))
970
+ rotvec = rotvec / torch.max(theta, torch.tensor(eps)) * theta_prime
971
+ quaternion_output = roma.rotvec_to_unitquat(rotvec)
972
+ return quaternion_output
973
+
974
+
975
+ def stereographic_map_rotation(
976
+ rotation: torch.Tensor, factor: float, inverse: bool, eps=1e-08
977
+ ) -> torch.Tensor:
978
+ if factor == 1.0:
979
+ return rotation
980
+ rotation_format = rotation_format_from_tensor(rotation)
981
+ is_3x3 = is_rotmat_3x3(rotation)
982
+ rotation = convert_rotation(rotation, RotationFormat.QUATERNION, autonorm=False, half_cover=True)
983
+ rotation = stereographic_map_quaternion(rotation, factor, inverse=inverse, eps=eps)
984
+ rotation = convert_rotation(rotation, rotation_format, autonorm=False, half_cover=True)
985
+ if is_3x3:
986
+ rotation = rotmat_as_3x3(rotation)
987
+ return rotation
988
+
989
+
990
+ class RotationStereomapNormalizer(Normalizer[RotationStereomapNormalizerConfig]):
991
+ def normalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
992
+ del kwargs
993
+ return stereographic_map_rotation(value, factor=self.config.factor, inverse=False)
994
+
995
+ def unnormalize(self, value: torch.Tensor, **kwargs) -> torch.Tensor:
996
+ del kwargs
997
+ return stereographic_map_rotation(value, factor=self.config.factor, inverse=True)
998
+
999
+
1000
+ def assert_np_hwc_or_hw_image(image: np.ndarray | PIL.Image.Image) -> np.ndarray:
1001
+ """Make sure image is of type np.ndarray and HWC format"""
1002
+ if isinstance(image, PIL.Image.Image):
1003
+ image = np.asarray(image)
1004
+ assert isinstance(image, np.ndarray), type(image)
1005
+ assert image.ndim in [2, 3], image.shape
1006
+ if image.ndim == 3:
1007
+ assert image.shape[-1] <= 4, image.shape
1008
+ return image
1009
+
1010
+
1011
+ def hw_from_image(image: PIL.Image.Image | np.ndarray) -> tuple[int, int]:
1012
+ if isinstance(image, np.ndarray):
1013
+ (height, width) = image.shape[:2]
1014
+ else:
1015
+ (width, height) = image.size
1016
+ return height, width
1017
+
1018
+
1019
+ def pad_image(
1020
+ image: PIL.Image.Image | np.ndarray,
1021
+ target_size: dict[str, int],
1022
+ pad_value: tuple[int, int, int] | tuple[float, float, float] | int | float = 0,
1023
+ ) -> PIL.Image.Image | np.ndarray:
1024
+ """Pad image adding a symmetric border around the height/width."""
1025
+ assert isinstance(image, (PIL.Image.Image, np.ndarray)), type(image)
1026
+ (height, width) = hw_from_image(image)
1027
+ (target_width, target_height) = (target_size['width'], target_size['height'])
1028
+ if width == target_width and height == target_height:
1029
+ return image
1030
+ assert target_width >= width, f"Can't pad image of width {width} to {target_width}"
1031
+ assert target_height >= height, f"Can't pad image of height {height} to {target_height}"
1032
+ (horizontal_pad, vertical_pad) = (int((target_width - width) / 2), int((target_height - height) / 2))
1033
+ if isinstance(image, np.ndarray):
1034
+ padding = ((vertical_pad, vertical_pad), (horizontal_pad, horizontal_pad)) + ((0, 0),) * (
1035
+ image.ndim - 2
1036
+ )
1037
+ image = np.pad(image, padding, mode='constant', constant_values=pad_value)
1038
+ else:
1039
+ padding = (horizontal_pad, vertical_pad, horizontal_pad, vertical_pad)
1040
+ image = torchvision.transforms.v2.functional.pad(
1041
+ image, padding=padding, fill=pad_value, padding_mode='constant'
1042
+ )
1043
+ return image
1044
+
1045
+
1046
+ def pad_image_to_ratio(
1047
+ image: PIL.Image.Image | np.ndarray,
1048
+ target_wh_ratio: float,
1049
+ pad_value: tuple[int, int, int] | tuple[float, float, float] | int | float = 0,
1050
+ ) -> PIL.Image.Image | np.ndarray:
1051
+ """Pad image to a target aspect ratio."""
1052
+ (height, width) = hw_from_image(image)
1053
+ wh_ratio = width / height
1054
+ if target_wh_ratio >= wh_ratio:
1055
+ pad_size = {'width': round(height * target_wh_ratio), 'height': height}
1056
+ else:
1057
+ pad_size = {'width': width, 'height': round(width / target_wh_ratio)}
1058
+ image = pad_image(image, target_size=pad_size, pad_value=pad_value)
1059
+ return image
1060
+
1061
+
1062
+ def crop_image(
1063
+ image: np.ndarray | PIL.Image.Image,
1064
+ start_height: int,
1065
+ start_width: int,
1066
+ target_height: int,
1067
+ target_width: int,
1068
+ ) -> np.ndarray | PIL.Image.Image:
1069
+ np_image = assert_np_hwc_or_hw_image(image)
1070
+ (height, width) = hw_from_image(image)
1071
+ assert target_width <= width, f"Can't crop image of width {width} to {target_width}"
1072
+ assert target_height <= height, f"Can't crop image of width {height} to {target_height}"
1073
+ (start_height, start_width) = (round(start_height), round(start_width))
1074
+ (target_height, target_width) = (round(target_height), round(target_width))
1075
+ np_image = np_image[
1076
+ start_height : start_height + target_height, start_width : start_width + target_width, ...
1077
+ ]
1078
+ image = PIL.Image.fromarray(np_image) if isinstance(image, PIL.Image.Image) else np_image
1079
+ return image
1080
+
1081
+
1082
+ def crop_image_center(
1083
+ image: np.ndarray | PIL.Image.Image, target_size: dict[str, int]
1084
+ ) -> np.ndarray | PIL.Image.Image:
1085
+ np_image = assert_np_hwc_or_hw_image(image)
1086
+ (height, width) = np_image.shape[:2]
1087
+ (target_height, target_width) = (target_size['height'], target_size['width'])
1088
+ assert target_width <= width, f"Can't crop image of width {width} to {target_width}"
1089
+ assert target_height <= height, f"Can't crop image of width {height} to {target_height}"
1090
+ top = (height - target_height) // 2
1091
+ left = (width - target_width) // 2
1092
+ np_image = crop_image(np_image, top, left, target_height, target_width)
1093
+ image = PIL.Image.fromarray(np_image) if isinstance(image, PIL.Image.Image) else np_image
1094
+ return image
1095
+
1096
+
1097
+ def crop_image_to_ratio(
1098
+ image: PIL.Image.Image | np.ndarray, target_wh_ratio: float
1099
+ ) -> PIL.Image.Image | np.ndarray:
1100
+ """Pad image to a target aspect ratio."""
1101
+ (height, width) = hw_from_image(image)
1102
+ wh_ratio = width / height
1103
+ if target_wh_ratio >= wh_ratio:
1104
+ crop_size = {'width': width, 'height': round(width / target_wh_ratio)}
1105
+ else:
1106
+ crop_size = {'width': round(height * target_wh_ratio), 'height': height}
1107
+ image = crop_image_center(image, target_size=crop_size)
1108
+ return image
1109
+
1110
+
1111
+ def crop_and_pad_image_to_ratio(
1112
+ image: PIL.Image.Image | np.ndarray,
1113
+ target_wh_ratio: float,
1114
+ mode: ResizeMode | str,
1115
+ pad_value: tuple[int, int, int] | tuple[float, float, float] | int | float = 0,
1116
+ ) -> PIL.Image.Image | np.ndarray:
1117
+ """
1118
+ Crop and pad an image to a target size depending on the mode.
1119
+ It's expected that the source image and target size have different aspect ratios.
1120
+
1121
+ Args:
1122
+ image: The image to crop and pad.
1123
+ target_size: The target size to crop and pad the image to.
1124
+ mode: The mode to use for cropping and padding.
1125
+ """
1126
+ (height, width) = hw_from_image(image)
1127
+ wh_ratio = width / height
1128
+ if np.isclose(wh_ratio, target_wh_ratio, rtol=0.01, atol=0.0001):
1129
+ return image
1130
+ if mode == ResizeMode.SMART:
1131
+ aspect_ratio = max(width, height) / min(width, height)
1132
+ target_ratio = max(target_wh_ratio, 1 / target_wh_ratio)
1133
+ if aspect_ratio == 1:
1134
+ if target_ratio >= 4 / 3 - 0.01:
1135
+ crop_wh_ratio = 4 / 3 if target_wh_ratio >= 1.0 else 3 / 4
1136
+ image = crop_image_to_ratio(image, crop_wh_ratio)
1137
+ else:
1138
+ pass
1139
+ elif aspect_ratio <= 4 / 3 + 0.01:
1140
+ if wh_ratio >= 1.0 != (target_wh_ratio >= 1.0):
1141
+ image = crop_image_to_ratio(image, 1.0)
1142
+ elif wh_ratio >= 1.0 != (target_wh_ratio >= 1.0):
1143
+ image = crop_image_to_ratio(image, 1.0)
1144
+ elif target_ratio >= 4 / 3 + 0.01:
1145
+ pass
1146
+ else:
1147
+ crop_wh_ratio = 4 / 3 if target_wh_ratio >= 1.0 else 3 / 4
1148
+ image = crop_image_to_ratio(image, crop_wh_ratio)
1149
+ image = pad_image_to_ratio(image, target_wh_ratio, pad_value=pad_value)
1150
+ elif mode == ResizeMode.PAD:
1151
+ image = pad_image_to_ratio(image, target_wh_ratio, pad_value=pad_value)
1152
+ elif mode == ResizeMode.CROP:
1153
+ image = crop_image_to_ratio(image, target_wh_ratio)
1154
+ else:
1155
+ raise ValueError(f'Mode {mode} not supported')
1156
+ return image
1157
+
1158
+
1159
+ def is_single_channel_image(image: np.ndarray | PIL.Image.Image) -> bool:
1160
+ if isinstance(image, PIL.Image.Image):
1161
+ return image.mode in ['1', 'L', 'LA', 'La', 'P', 'PA', 'F', 'I', 'I;16', 'I;16L', 'I;16B', 'I;16N']
1162
+ if isinstance(image, np.ndarray):
1163
+ return image.ndim == 2 or image.ndim == 3 and image.shape[2] == 1
1164
+ raise ValueError(f'Unsupported image type: {type(image)}')
1165
+
1166
+
1167
+ def is_binary_mask(image: np.ndarray | PIL.Image.Image) -> bool:
1168
+ image = np.asarray(image)
1169
+ return image.dtype in [np.uint8, np.bool_] and np.max(image) == 1
1170
+
1171
+
1172
+ def resize_image(
1173
+ image: PIL.Image.Image | np.ndarray,
1174
+ target_size: dict[str, int],
1175
+ mode: ResizeMode | str,
1176
+ resample: PIL.Image.Resampling | str = 'auto',
1177
+ pad_value: tuple[int, int, int] | tuple[float, float, float] | int | float = 0,
1178
+ ) -> PIL.Image.Image | np.ndarray:
1179
+ (target_width, target_height) = (target_size['width'], target_size['height'])
1180
+ (height, width) = hw_from_image(image)
1181
+ if height == target_height and width == target_width:
1182
+ return image
1183
+ if resample == 'auto':
1184
+ if is_single_channel_image(image):
1185
+ resample = PIL.Image.Resampling.BILINEAR
1186
+ else:
1187
+ resample = PIL.Image.Resampling.LANCZOS
1188
+ else:
1189
+ assert isinstance(resample, PIL.Image.Resampling), resample
1190
+ if is_single_channel_image(image) and resample not in [
1191
+ PIL.Image.Resampling.BILINEAR,
1192
+ PIL.Image.Resampling.BICUBIC,
1193
+ ]:
1194
+ raise ValueError(
1195
+ f'Single channel images must be resized with bilinear or bicubic, but got {resample}'
1196
+ )
1197
+ if is_bin_mask := is_binary_mask(image):
1198
+ image = np.asarray(image).astype(np.uint8) * 255
1199
+ if mode == ResizeMode.SMART:
1200
+ image = crop_and_pad_image_to_ratio(
1201
+ image, target_wh_ratio=target_width / target_height, mode=mode, pad_value=pad_value
1202
+ )
1203
+ pil_image = PIL.Image.fromarray(image) if isinstance(image, np.ndarray) else image
1204
+ if mode in [ResizeMode.NAIVE, ResizeMode.SMART]:
1205
+ pil_image = pil_image.resize((target_width, target_height), resample=resample)
1206
+ else:
1207
+ raise NotImplementedError(f'Mode {mode} not supported')
1208
+ image = np.asarray(pil_image) if isinstance(image, np.ndarray) else pil_image
1209
+ if is_bin_mask:
1210
+ image = image.astype(np.uint8) > 127
1211
+ return image
1212
+
1213
+
1214
+ def invert_gripper(gripper: np.ndarray, low: float, high: float) -> np.ndarray:
1215
+ if low < 0.0:
1216
+ return np.clip(-gripper, low, high)
1217
+ return high - np.clip(gripper, low, high)
1218
+
1219
+
1220
+ GRIPPER_BOUNDS = {
1221
+ 'austin_buds_dataset': (0.0, 0.08),
1222
+ 'austin_sailor_dataset': (0.0, 0.08),
1223
+ 'austin_sirius_dataset': (0.0, 0.08),
1224
+ 'bc_z': (0.0, 1.0),
1225
+ 'berkeley_autolab_ur5': (0.0, 1.0),
1226
+ 'berkeley_cable_routing': (0.0, 1.0),
1227
+ 'berkeley_fanuc_manipulation': (0.0, 1.0),
1228
+ 'bridge': (0.0, 1.0),
1229
+ 'bridge_steering': (0.0, 1.0),
1230
+ 'bridge_tread': (0.0, 1.0),
1231
+ 'bridge_paraphrase_k5': (0.0, 1.0),
1232
+ 'bridge_paraphrase_k10': (0.0, 1.0),
1233
+ 'bridge_full_tread_8b_k5': (0.0, 1.0),
1234
+ 'bridge_tread_full': (0.0, 1.0),
1235
+ 'bridge_coarse_max3': (0.0, 1.0),
1236
+ 'bridge_hindsight': (0.0, 1.0),
1237
+ 'bridge_32b': (0.0, 1.0),
1238
+ 'bridge_tread_k10': (0.0, 1.0),
1239
+ 'bridge_orig': (0.0, 1.0),
1240
+ 'cmu_stretch': (-3.0, 3.0),
1241
+ 'dlr_edan_shared_control': (0.0, 1.0),
1242
+ 'droid': (0.0, 1.0),
1243
+ 'fmb': (0.0, 1.0),
1244
+ 'fractal20220817_data': (0.0, 1.0),
1245
+ 'furniture_bench_dataset': (0.0, 0.08),
1246
+ 'iamlab_cmu_pickup_insert': (0.0, 1.0),
1247
+ 'jaco_play': (0.0, 1.4),
1248
+ 'kuka': (0.0, 1.0),
1249
+ 'language_table': (0.0, 1.0),
1250
+ 'nyu_franka_play_dataset': (0.0, 1.0),
1251
+ 'roboset': (0.0, 1.0),
1252
+ 'roboturk': (0.0, 1.0),
1253
+ 'stanford_hydra_dataset': (0.0, 0.08),
1254
+ 'taco_play': (0.0, 0.08),
1255
+ 'toto': (0.0, 1.0),
1256
+ 'ucsd_kitchen_dataset': (0.0, 1.0),
1257
+ 'utaustin_mutex': (0.0, 0.08),
1258
+ 'viola': (0.0, 0.08),
1259
+ }
1260
+
1261
+
1262
+ def preprocess_gripper_observation(
1263
+ gripper: np.ndarray, dataset_name: str | np.ndarray, binary: bool = True
1264
+ ) -> np.ndarray:
1265
+ """
1266
+ Preprocess gripper observation depending on dataset. Input is the raw gripper observation from the dataset
1267
+ or from the robot and output is normalized continuous value.
1268
+ - if `binary`, output is in [0, 1], with 0 = closed and 1 = open.
1269
+ - otherwise, output is in [-1, 1], with -1 = closed and 1 = open.
1270
+
1271
+ Dataset-specific gripper observations:
1272
+ austin_buds_dataset: continuous; ~[0=closed; 0.08=open] (franka gripper)
1273
+ austin_sailor_dataset: continuous; ~[0=closed; 0.08=open] (franka gripper)
1274
+ austin_sirius_dataset: continuous; ~[0=closed; 0.08=open] (franka gripper)
1275
+ bc_z: continuous; [0=open; 1=closed]
1276
+ berkeley_autolab_ur5: binary; [0=open; 1=closed]
1277
+ berkeley_cable_routing: constant (closed)
1278
+ berkeley_fanuc_manipulation: binary; [0=open; 1=closed]
1279
+ bridge: continuous; ~[0=closed; 1=open]
1280
+ bridge_orig: continuous; ~[0=closed; 1=open]
1281
+ cmu_stretch: continuous; [-3=closed; 3=open]
1282
+ dlr_edan_shared_control: missing
1283
+ droid: continuous; [0=open, 1=closed]
1284
+ fmb: binary; [0=open; 1=closed]
1285
+ fractal20220817_data: continuous; [0=open; 1=closed]
1286
+ furniture_bench_dataset: continuous; ~[0=closed; 0.08=open] (franka gripper)
1287
+ iamlab_cmu_pickup_insert: binary; [0=closed; 1=open]
1288
+ jaco_play: continuous; [0=open; 1.4=closed]
1289
+ kuka: binary; [0=open; 1=closed]
1290
+ language_table: constant (no gripper)
1291
+ nyu_franka_play_dataset: missing
1292
+ roboset: continuous; [0=open, 1=closed]
1293
+ roboturk: continuous; [0=closed, 0.04=open]
1294
+ stanford_hydra_dataset: continuous; ~[0=closed; 0.08=open] (franka gripper)
1295
+ taco_play: continuous; ~[0=closed; 0.08=open] (franka gripper)
1296
+ toto: constant (closed)
1297
+ ucsd_kitchen_dataset: missing
1298
+ utaustin_mutex: continuous; ~[0=closed; 0.08=open] (franka gripper)
1299
+ viola: continuous; ~[0=closed; 0.08=open] (franka gripper)
1300
+
1301
+ """
1302
+ if isinstance(dataset_name, np.ndarray):
1303
+ assert np.unique(dataset_name).size == 1, dataset_name
1304
+ dataset_name = str(dataset_name[0])
1305
+ if dataset_name in [
1306
+ 'berkeley_cable_routing',
1307
+ 'dlr_edan_shared_control',
1308
+ 'language_table',
1309
+ 'nyu_franka_play_dataset',
1310
+ 'toto',
1311
+ 'ucsd_kitchen_dataset',
1312
+ ]:
1313
+ gripper = normalize_gripper_by_bounds(
1314
+ torch.from_numpy(gripper),
1315
+ low=torch.full(gripper.shape, GRIPPER_BOUNDS[dataset_name][0], dtype=torch.float32),
1316
+ high=torch.full(gripper.shape, GRIPPER_BOUNDS[dataset_name][1], dtype=torch.float32),
1317
+ binary=binary,
1318
+ ).numpy()
1319
+ elif dataset_name in [
1320
+ 'bc_z',
1321
+ 'berkeley_autolab_ur5',
1322
+ 'berkeley_fanuc_manipulation',
1323
+ 'droid',
1324
+ 'fmb',
1325
+ 'fractal20220817_data',
1326
+ 'jaco_play',
1327
+ 'kuka',
1328
+ 'roboset',
1329
+ ]:
1330
+ (low, high) = GRIPPER_BOUNDS[dataset_name]
1331
+ gripper = normalize_gripper_by_bounds(
1332
+ torch.from_numpy(invert_gripper(gripper, low=low, high=high)),
1333
+ low=torch.full(gripper.shape, GRIPPER_BOUNDS[dataset_name][0], dtype=torch.float32),
1334
+ high=torch.full(gripper.shape, GRIPPER_BOUNDS[dataset_name][1], dtype=torch.float32),
1335
+ binary=binary,
1336
+ ).numpy()
1337
+ elif dataset_name in [
1338
+ 'austin_buds_dataset',
1339
+ 'austin_sailor_dataset',
1340
+ 'austin_sirius_dataset',
1341
+ 'bridge',
1342
+ 'bridge_steering',
1343
+ 'bridge_tread',
1344
+ 'bridge_paraphrase_k5',
1345
+ 'bridge_paraphrase_k10',
1346
+ 'bridge_full_tread_8b_k5',
1347
+ 'bridge_tread_full',
1348
+ 'bridge_coarse_max3',
1349
+ 'bridge_hindsight',
1350
+ 'bridge_32b',
1351
+ 'bridge_tread_k10',
1352
+ 'bridge_orig',
1353
+ 'cmu_stretch',
1354
+ 'furniture_bench_dataset',
1355
+ 'iamlab_cmu_pickup_insert',
1356
+ 'roboturk',
1357
+ 'stanford_hydra_dataset',
1358
+ 'taco_play',
1359
+ 'utaustin_mutex',
1360
+ 'viola',
1361
+ ]:
1362
+ (low, high) = GRIPPER_BOUNDS[dataset_name]
1363
+ gripper = normalize_gripper_by_bounds(
1364
+ torch.from_numpy(gripper),
1365
+ low=torch.full(gripper.shape, low, dtype=torch.float32),
1366
+ high=torch.full(gripper.shape, high, dtype=torch.float32),
1367
+ binary=binary,
1368
+ ).numpy()
1369
+ else:
1370
+ raise NotImplementedError(f'Unknown dataset: {dataset_name}')
1371
+ return gripper
1372
+
1373
+
1374
+ VLMProcessorConfigT = TypeVar('VLMProcessorConfigT', bound=VLMProcessorConfig)
1375
+
1376
+
1377
+ class VLMProcessor(Configurable[VLMProcessorConfigT], Template[VLMProcessorConfigT]):
1378
+ @abstractmethod
1379
+ def preprocess_inputs(
1380
+ self, chat: List[str], images: Dict[str, List[PIL.Image.Image]]
1381
+ ) -> Dict[str, torch.Tensor | Dict[str, torch.Tensor]]:
1382
+ ...
1383
+
1384
+ @property
1385
+ @abstractmethod
1386
+ def tokenizer(self) -> transformers.PreTrainedTokenizerBase:
1387
+ pass
1388
+
1389
+ @property
1390
+ @abstractmethod
1391
+ def image_sizes(self) -> Dict[str, ImageSizeConfig]:
1392
+ pass
1393
+
1394
+ @property
1395
+ @abstractmethod
1396
+ def ignore_index(self) -> int:
1397
+ pass
1398
+
1399
+
1400
+ VLAMProcessorConfigT = TypeVar('VLAMProcessorConfigT', bound=VLAMProcessorConfig)
1401
+
1402
+
1403
+ class VLAMProcessor(Configurable[VLAMProcessorConfigT], Template[VLAMProcessorConfigT]):
1404
+ def __init__(self, config: VLAMProcessorConfigT, vlm_processor: VLMProcessor):
1405
+ super().__init__(config)
1406
+ self.vlm_processor = vlm_processor
1407
+ self.control_tokenizer = EmptyTokenizer(
1408
+ config=self.config.control_tokenizer_config, tokenizer=self.tokenizer
1409
+ )
1410
+ self.translation_obs_norm = DatasetStatsNormalizer(self.config.translation_obs_norm)
1411
+ self.rotation_obs_norm = IdentityNormalizer(self.config.rotation_obs_norm)
1412
+ self.translation_control_norm = BoundsNormalizer(self.config.translation_control_norm)
1413
+ self.rotation_control_norm = RotationStereomapNormalizer(self.config.rotation_control_norm)
1414
+ self.joints_obs_norm = BoundsNormalizer(self.config.joints_obs_norm)
1415
+
1416
+ @property
1417
+ def tokenizer(self) -> transformers.PreTrainedTokenizerBase:
1418
+ return self.vlm_processor.tokenizer
1419
+
1420
+ @property
1421
+ def image_sizes(self) -> Dict[str, ImageSizeConfig]:
1422
+ return self.vlm_processor.image_sizes
1423
+
1424
+ @property
1425
+ def camera_names(self) -> List[str]:
1426
+ return list(self.vlm_processor.image_sizes.keys())
1427
+
1428
+ @property
1429
+ def ignore_index(self) -> int:
1430
+ return self.vlm_processor.ignore_index
1431
+
1432
+ @property
1433
+ def control_io_config(self) -> ControlDataIOConfig:
1434
+ return self.config.control_io_config
1435
+
1436
+ @cached_property
1437
+ def rotation_components(self) -> int:
1438
+ if self.config.rotation_format == RotationFormat.EULER:
1439
+ return 3
1440
+ if self.config.rotation_format == RotationFormat.QUATERNION:
1441
+ return 4
1442
+ if self.config.rotation_format == RotationFormat.ROTMAT:
1443
+ return 9
1444
+ raise NotImplementedError(self.config.rotation_format)
1445
+
1446
+ @abstractmethod
1447
+ def policy_control_plan_from_model_target(
1448
+ self, target: RoboticsTarget, dataset_name: np.ndarray
1449
+ ) -> RoboticsControlPlan:
1450
+ """
1451
+ Produce a RoboticsControlPlan from `model_output`. Unnormalizes the outputs, runs any
1452
+ model-specific postprocessing and converts to the desired target reference frame.
1453
+ See `policy_control_plan_from_model_output` for details on arguments.
1454
+ """
1455
+
1456
+ @abstractmethod
1457
+ def policy_control_plan_from_model_output(
1458
+ self, model_output: RoboticsOutput, dataset_name: np.ndarray, valid_mask: torch.Tensor
1459
+ ) -> RoboticsControlPlan:
1460
+ """
1461
+ Produce a RoboticsControlPlan from `model_output`. Unnormalizes the outputs and runs any
1462
+ model-specific postprocessing. Translation and rotation outputs are always in a RELATIVE
1463
+ frame w.r.t. the currrent end-effector pose, where the reference frame used during learning
1464
+ (ROBOT_BASE vs EEF) is preserved for each component. In other words, if translation_control_frame
1465
+ is ROBOT_BASE_DELTA, and rotation_control_frame is EEF_DELTA, then the output translation will be
1466
+ in ROBOT_BASE_RELATIVE frame and rotation in EEF_RELATIVE frame.
1467
+
1468
+ We explicitly avoid any conversions which require the EE pose. The EE pose needs to be in
1469
+ ROBOT_BASE frame, but there are many easy sources of error. For example, it's easy to mistakenly
1470
+ provide the EE pose, which was input to the model and is not guaranteed to be in ROBOT_BASE.
1471
+ It's also easy to provide normalized EE pose, which also leads to incorrect results. Instead,
1472
+ if further conversions are required, it's recommended to call translation_to_target_frame and
1473
+ rotation_to_target_frame outside this function, where the user has full control over.
1474
+
1475
+ Args:
1476
+ model_output: RoboticsOutput from the model of shape [B, num_timesteps, ...]
1477
+ dataset_name: np.ndarray of shape [B] with dataset names for each batch example
1478
+ valid_mask: torch.Tensor of shape [B, num_timesteps] indicating valid control steps
1479
+ Returns:
1480
+ RoboticsControlPlan with **UNNORMALIZED** controls in the desired target frame
1481
+ """
1482
+
1483
+ def resize_image(
1484
+ self, camera_name: str, image: PIL.Image.Image | np.ndarray
1485
+ ) -> PIL.Image.Image | np.ndarray:
1486
+ return resize_image(
1487
+ image,
1488
+ target_size={
1489
+ 'width': self.image_sizes[camera_name].width,
1490
+ 'height': self.image_sizes[camera_name].height,
1491
+ },
1492
+ mode=self.config.image_resize,
1493
+ resample=PIL.Image.Resampling.LANCZOS,
1494
+ )
1495
+
1496
+ def preprocess_inputs(
1497
+ self,
1498
+ chat: List[str],
1499
+ images: Dict[str, PIL.Image.Image | List[PIL.Image.Image]],
1500
+ ee_pose_translation: np.ndarray,
1501
+ ee_pose_rotation: np.ndarray,
1502
+ gripper: np.ndarray,
1503
+ joints: np.ndarray,
1504
+ dataset_name: np.ndarray,
1505
+ inference_mode: bool,
1506
+ control_target: Optional[RoboticsTarget] = None,
1507
+ ) -> Dict[str, torch.Tensor | Dict[str, torch.Tensor]]:
1508
+ """
1509
+ Preprocess the inputs for a single example
1510
+ Args:
1511
+ instruction: Language instruction
1512
+ images: History of input images with increasing timestamps
1513
+ ee_pose_translation: np.ndarray, shape [..., num_past_scalars, 3]
1514
+ ee_pose_rotation: np.ndarray, shape [..., num_past_scalars, 3 | 4 | 9]
1515
+ joints: np.ndarray, shape [..., num_past_scalars, <= 7]
1516
+ dataset_name: 1D np.ndarray
1517
+ inference_mode: If True, prepare the input for inference (e.g. don't include target
1518
+ any tokens in the input if relevant). If control_target is available, it should
1519
+ still be preprocessed for test dataset comparison
1520
+ control_target: RoboticsTarget, each component of shape
1521
+ [..., num_control_steps, num_control_components]. Provided only when available, usually
1522
+ during training and dataset test
1523
+ Returns:
1524
+ Dict containing torch.Tensor with inputs
1525
+ """
1526
+ del control_target, inference_mode
1527
+ inputs = self.vlm_processor.preprocess_inputs(chat=chat, images=images)
1528
+ images: Dict[str, torch.Tensor] = inputs['images']
1529
+ input_ids: torch.Tensor = inputs['input_ids'][..., : self.tokenizer.model_max_length]
1530
+ target_text_tokens_ids: torch.Tensor = inputs['target_ids'][..., : self.tokenizer.model_max_length]
1531
+ attn_mask = torch.ones(input_ids.shape, dtype=torch.bool)
1532
+ ee_pose_translation = torch.tensor(ee_pose_translation, dtype=torch.float32)
1533
+ ee_pose_rotation = torch.tensor(ee_pose_rotation, dtype=torch.float32)
1534
+ ee_pose_rotation = convert_rotation(ee_pose_rotation, self.config.rotation_format, autonorm=True)
1535
+ gripper = preprocess_gripper_observation(gripper, dataset_name)
1536
+ gripper = torch.tensor(gripper, dtype=torch.float32)
1537
+ ee_pose_translation = self.normalize(
1538
+ ee_pose_translation, dataset_name=dataset_name, key='translation_obs'
1539
+ )
1540
+ ee_pose_rotation = self.normalize(ee_pose_rotation, dataset_name=dataset_name, key='rotation_obs')
1541
+ joints = torch.tensor(joints, dtype=torch.float32)
1542
+ if joints.shape[-1] < 7:
1543
+ missing_size = 7 - joints.shape[-1]
1544
+ joints = torch.cat([joints, torch.zeros([*joints.shape[:-1], missing_size])], dim=-1)
1545
+ joints = self.normalize(joints, dataset_name=dataset_name, key='joints_obs')
1546
+ outputs = {
1547
+ 'images': images,
1548
+ 'input_ids': input_ids,
1549
+ 'target_text_tokens_ids': target_text_tokens_ids,
1550
+ 'attn_mask': attn_mask,
1551
+ 'ee_pose_translation': ee_pose_translation,
1552
+ 'ee_pose_rotation': ee_pose_rotation,
1553
+ 'gripper': gripper,
1554
+ 'joints': joints,
1555
+ 'control_tokens_ids': None,
1556
+ 'target_control_tokens_ids': None,
1557
+ }
1558
+ return outputs
1559
+
1560
+ def create_input(
1561
+ self,
1562
+ chat: List[str],
1563
+ images: Dict[str, List[PIL.Image.Image]],
1564
+ ee_pose_translation: np.ndarray,
1565
+ ee_pose_rotation: np.ndarray,
1566
+ gripper: np.ndarray,
1567
+ joints: np.ndarray,
1568
+ dataset_name: np.ndarray,
1569
+ inference_mode: bool,
1570
+ control_target: Optional[RoboticsTarget] = None,
1571
+ ) -> RoboticsInput:
1572
+ inputs = self.preprocess_inputs(
1573
+ chat=chat,
1574
+ images=images,
1575
+ ee_pose_translation=ee_pose_translation,
1576
+ ee_pose_rotation=ee_pose_rotation,
1577
+ gripper=gripper,
1578
+ joints=joints,
1579
+ dataset_name=dataset_name,
1580
+ inference_mode=inference_mode,
1581
+ control_target=control_target,
1582
+ )
1583
+ inputs.pop('target_text_tokens_ids')
1584
+ inputs.pop('target_control_tokens_ids')
1585
+ return RoboticsInput(**inputs)
1586
+
1587
+ def normalize(self, value: torch.Tensor, dataset_name: np.ndarray, key: str) -> torch.Tensor:
1588
+ normalizer = getattr(self, f'{key}_norm')
1589
+ return normalizer.normalize(value, dataset_name=dataset_name)
1590
+
1591
+ def unnormalize(self, value: torch.Tensor, dataset_name: np.ndarray, key: str) -> torch.Tensor:
1592
+ normalizer = getattr(self, f'{key}_norm')
1593
+ return normalizer.unnormalize(value, dataset_name=dataset_name)
1594
+
1595
+ @property
1596
+ def _stats_horizon_key(self) -> str:
1597
+ if self.config.delta_controls:
1598
+ if self.control_io_config.future_controls_sequence_stride_sec is None:
1599
+ horizon = 0.0
1600
+ else:
1601
+ horizon = self.control_io_config.future_controls_sequence_stride_sec
1602
+ elif self.control_io_config.future_controls_sequence_stride_sec is None:
1603
+ if self.control_io_config.future_controls_sequence_length == 1:
1604
+ horizon = 0.0
1605
+ else:
1606
+ raise NotImplementedError()
1607
+ else:
1608
+ horizon = (
1609
+ self.control_io_config.future_controls_sequence_length
1610
+ * self.control_io_config.future_controls_sequence_stride_sec
1611
+ )
1612
+ key = f'horizon_{round(horizon, 2)}s'
1613
+ return key
1614
+
1615
+
1616
+ def world_to_relative_translations(
1617
+ translation_sequence: torch.Tensor, reference_frame: torch.Tensor
1618
+ ) -> torch.Tensor:
1619
+ """
1620
+ Transform a sequence of translation vectors encoded w.r.t. WORLD frame to encoding w.r.t.
1621
+ `reference_frame`, where `reference_frame` is provided w.r.t. WORLD frame
1622
+ Ex:
1623
+ Sequence of points: T1, T2, T3, T4
1624
+ `translation_sequence` contains the vectors: WT1, WT2, WT3, WT4, where W is the world frame
1625
+ Output: T0T1, T0T2, T0T3, T0T4, where T0 is the reference frame
1626
+
1627
+ Args:
1628
+ translation_sequence: torch.Tensor of shape [..., S, 3], containing the translation vectors, where S
1629
+ corresponds to the sequence dimension
1630
+ reference_frame: torch.Tensor, shape [..., 1, 3] and the SAME number of BATCH dims as
1631
+ `translation_sequence`. The new reference frame, provided w.r.t. WORLD coordinate frame
1632
+ Returns:
1633
+ torch.Tensor of the same shape as translation_sequence, containing delta translations
1634
+ """
1635
+ assert translation_sequence.ndim >= 3, translation_sequence.shape
1636
+ if reference_frame.ndim != translation_sequence.ndim:
1637
+ raise ValueError(
1638
+ f'Cannot broadcast reference_frame of shape {reference_frame.shape} to translation_sequence of shape {translation_sequence.shape}. Provide tensors with the same number of batch dimensions'
1639
+ )
1640
+ delta_translations = translation_sequence - reference_frame
1641
+ return delta_translations
1642
+
1643
+
1644
+ def delta_to_relative_translations(translation_sequence: torch.Tensor) -> torch.Tensor:
1645
+ """
1646
+ Transform a sequence of translation vectors encoded w.r.t. PREVIOUS frame in the sequence to encoding
1647
+ w.r.t. the 0-th element preceding the sequence
1648
+ Ex:
1649
+ Sequence of points: T1, T2, T3, T4
1650
+ `translation_sequence` contains the vectors: T0T1, T1T2, T2T3, T3T4, where T0 is the base frame,
1651
+ implicitly encoded in T0T1
1652
+ Output: T0T1, T0T2, T0T3, T0T4
1653
+
1654
+ Args:
1655
+ translation_sequence: torch.Tensor of shape [..., S, 3], containing the translation vectors, where S
1656
+ corresponds to the sequence dimension
1657
+ Returns:
1658
+ torch.Tensor of the same shape as translation_sequence, containing delta translations
1659
+ """
1660
+ assert translation_sequence.ndim >= 3, translation_sequence.shape
1661
+ delta_translations = torch.cumsum(translation_sequence, dim=-2)
1662
+ return delta_translations
1663
+
1664
+
1665
+ def relative_to_delta_translations(translation_sequence: torch.Tensor) -> torch.Tensor:
1666
+ """
1667
+ Transform a sequence of translation vectors encoded w.r.t. the same reference frame to delta translation
1668
+ vectors where each value is encoded w.r.t. the PREVIOUS frame in the sequence. The first element in
1669
+ the sequence remains the same.
1670
+ Ex:
1671
+ Sequence of points: T1, T2, T3, T4
1672
+ `translation_sequence` contains the vectors: RT1, RT2, RT3, RT4, where R is the reference frame
1673
+ Output: RT1, T1T2, T2T3, T3T4
1674
+
1675
+ Args:
1676
+ translation_sequence: torch.Tensor of shape [..., S, 3], containing the translation vectors, where S
1677
+ corresponds to the sequence dimension
1678
+ Returns:
1679
+ torch.Tensor of the same shape as translation_sequence, containing delta translations
1680
+ """
1681
+ assert translation_sequence.ndim >= 3, translation_sequence.shape
1682
+ reference_frames = torch.roll(translation_sequence, 1, dims=-2).clone()
1683
+ reference_frames[..., 0, :] = 0
1684
+ delta_translations = translation_sequence - reference_frames
1685
+ return delta_translations
1686
+
1687
+
1688
+ def translation_to_target_frame(
1689
+ translation: torch.Tensor,
1690
+ source_frame: ReferenceFrame,
1691
+ target_frame: ReferenceFrame,
1692
+ ee_pose_translation: Optional[torch.Tensor] = None,
1693
+ ee_pose_rotation: Optional[torch.Tensor] = None,
1694
+ ) -> torch.Tensor:
1695
+ """
1696
+ Convert translation sequence from source_frame to target_frame
1697
+ Args:
1698
+ translation: torch.Tensor of shape [..., S, 3], containing the translation vectors, where S
1699
+ corresponds to the sequence dimension
1700
+ source_frame: indicates the frame w.r.t. which `translation` is expressed
1701
+ target_frame: indicates the frame w.r.t. which the output translation should be expressed
1702
+ ee_pose_translation: torch.Tensor of shape [B, ..., 3], containing the translation of the current
1703
+ end-effector pose. Required only if target_frame is ROBOT_BASE and source_frame isn't.
1704
+ ee_pose_rotation: torch.Tensor of shape [..., 9 | 4 | 3 x 3], containing the rotation of the
1705
+ current end-effector pose w.r.t. ROBOT_BASE frame. Required only when source_frame and
1706
+ target_frame have different core reference frames.
1707
+ Returns:
1708
+ torch.Tensor of the same shape as translation, containing the converted translations
1709
+ """
1710
+ if source_frame == target_frame:
1711
+ return translation
1712
+ assert source_frame in ReferenceFrame.robot_frames | ReferenceFrame.eef_frames, source_frame
1713
+ assert target_frame in ReferenceFrame.robot_frames | ReferenceFrame.eef_frames, target_frame
1714
+ if ee_pose_rotation is not None:
1715
+ ee_pose_rotation = rotmat_as_3x3(convert_rotation(ee_pose_rotation, RotationFormat.ROTMAT))
1716
+ if source_frame.to_core() != target_frame.to_core():
1717
+ assert ee_pose_rotation is not None, f'{source_frame}, {target_frame}'
1718
+ if source_frame in ReferenceFrame.delta_frames:
1719
+ translation = delta_to_relative_translations(translation)
1720
+ source_frame = source_frame.to_relative()
1721
+ if target_frame in ReferenceFrame.robot_frames:
1722
+ assert source_frame == ReferenceFrame.EEF_RELATIVE, source_frame
1723
+ translation = apply_rotation(rotation=ee_pose_rotation, value=translation)
1724
+ source_frame = ReferenceFrame.ROBOT_BASE_RELATIVE
1725
+ elif target_frame in ReferenceFrame.eef_frames:
1726
+ assert source_frame in ReferenceFrame.robot_frames, source_frame
1727
+ if source_frame == ReferenceFrame.ROBOT_BASE:
1728
+ assert ee_pose_translation is not None
1729
+ translation = world_to_relative_translations(translation, reference_frame=ee_pose_translation)
1730
+ source_frame = ReferenceFrame.ROBOT_BASE_RELATIVE
1731
+ assert source_frame in ReferenceFrame.relative_frames, source_frame
1732
+ translation = apply_rotation(rotation=rotmat_inverse(ee_pose_rotation), value=translation)
1733
+ source_frame = ReferenceFrame.EEF_RELATIVE
1734
+ assert source_frame.to_core() == target_frame.to_core(), f'{source_frame}, {target_frame}'
1735
+ if source_frame == target_frame:
1736
+ return translation
1737
+ if (
1738
+ source_frame in ReferenceFrame.delta_frames
1739
+ and target_frame in ReferenceFrame.relative_frames | ReferenceFrame.core_frames
1740
+ ):
1741
+ translation = delta_to_relative_translations(translation)
1742
+ source_frame = source_frame.to_relative()
1743
+ elif source_frame == ReferenceFrame.ROBOT_BASE:
1744
+ assert ee_pose_translation is not None
1745
+ translation = world_to_relative_translations(translation, reference_frame=ee_pose_translation)
1746
+ source_frame = ReferenceFrame.ROBOT_BASE_RELATIVE
1747
+ assert source_frame in ReferenceFrame.relative_frames, source_frame
1748
+ if target_frame in ReferenceFrame.delta_frames:
1749
+ translation = relative_to_delta_translations(translation)
1750
+ source_frame = source_frame.to_delta()
1751
+ elif target_frame == ReferenceFrame.ROBOT_BASE:
1752
+ translation = world_to_relative_translations(translation, reference_frame=-ee_pose_translation)
1753
+ source_frame = ReferenceFrame.ROBOT_BASE
1754
+ assert source_frame == target_frame, f'{source_frame}, {target_frame}'
1755
+ return translation
1756
+
1757
+
1758
+ class RegressionProcessor(VLAMProcessor[RegressionProcessorConfig]):
1759
+ def policy_control_plan_from_model_target(
1760
+ self, target: RoboticsTarget, dataset_name: np.ndarray
1761
+ ) -> RoboticsControlPlan:
1762
+ """See VLAMProcessor.policy_control_plan_from_model_target for arguments"""
1763
+ translation_m = self.unnormalize(
1764
+ target.translation, dataset_name=dataset_name, key='translation_control'
1765
+ )
1766
+ rotation = self.unnormalize(target.rotation, dataset_name=dataset_name, key='rotation_control')
1767
+ rotmat = convert_rotation(rotation, RotationFormat.ROTMAT)
1768
+ gripper_prob = target.gripper
1769
+ if self.config.translation_control_frame != ReferenceFrame.ROBOT_BASE:
1770
+ translation_m = translation_to_target_frame(
1771
+ translation_m,
1772
+ source_frame=self.config.translation_control_frame,
1773
+ target_frame=self.config.translation_control_frame.to_relative(),
1774
+ )
1775
+ if self.config.rotation_control_frame != ReferenceFrame.ROBOT_BASE:
1776
+ rotmat = rotation_to_target_frame(
1777
+ rotmat,
1778
+ source_frame=self.config.rotation_control_frame,
1779
+ target_frame=self.config.rotation_control_frame.to_relative(),
1780
+ )
1781
+ return RoboticsControlPlan(
1782
+ translation_m=translation_m,
1783
+ rotmat=rotmat,
1784
+ gripper_prob=gripper_prob,
1785
+ valid_mask=target.valid_mask,
1786
+ )
1787
+
1788
+ def policy_control_plan_from_model_output(
1789
+ self, model_output: RoboticsOutput, dataset_name: np.ndarray, valid_mask: torch.Tensor
1790
+ ) -> RoboticsControlPlan:
1791
+ """
1792
+ Called during inference to create control plan from model output
1793
+ See VLAMProcessor.policy_control_plan_from_model_output for arguments
1794
+ """
1795
+ translation_m = self.unnormalize(
1796
+ model_output.translation, dataset_name=dataset_name, key='translation_control'
1797
+ )
1798
+ rotation = self.unnormalize(model_output.rotation, dataset_name=dataset_name, key='rotation_control')
1799
+ rotmat = convert_rotation(rotation, RotationFormat.ROTMAT, autonorm=True)
1800
+ gripper_prob = torch.sigmoid(model_output.gripper)
1801
+ if self.config.translation_control_frame != ReferenceFrame.ROBOT_BASE:
1802
+ translation_m = translation_to_target_frame(
1803
+ translation_m,
1804
+ source_frame=self.config.translation_control_frame,
1805
+ target_frame=self.config.translation_control_frame.to_relative(),
1806
+ )
1807
+ if self.config.rotation_control_frame != ReferenceFrame.ROBOT_BASE:
1808
+ rotmat = rotation_to_target_frame(
1809
+ rotmat,
1810
+ source_frame=self.config.rotation_control_frame,
1811
+ target_frame=self.config.rotation_control_frame.to_relative(),
1812
+ )
1813
+ return RoboticsControlPlan(
1814
+ translation_m=translation_m, rotmat=rotmat, gripper_prob=gripper_prob, valid_mask=valid_mask
1815
+ )
1816
+
1817
+
1818
+ class PiZeroFlowMatchingProcessor(Configurable[PiZeroFlowProcessorConfig], RegressionProcessor):
1819
+ def __init__(self, **kwargs):
1820
+ super().__init__(**kwargs)
1821
+ self.generator: torch.Generator = torch.Generator()
1822
+
1823
+ @cached_property
1824
+ def beta_distribution(self) -> torch.distributions.Beta:
1825
+ return torch.distributions.Beta(
1826
+ self.config.distribution_hyperparams.get('alpha', 1.5),
1827
+ self.config.distribution_hyperparams.get('beta', 1.0),
1828
+ )
1829
+
1830
+ def create_input(self, *args, **kwargs) -> RoboticsFlowInput:
1831
+ """In practice used only during inference"""
1832
+ inputs = super().create_input(*args, **kwargs)
1833
+ flow_input: FlowInput = self.sample_t0_input(batch_size=1, device=torch.device('cpu'))
1834
+ inputs = RoboticsFlowInput(**inputs.as_json(), flow_input=flow_input[0, ...])
1835
+ return inputs
1836
+
1837
+ def sample_timestep(self, batch_size: int) -> torch.Tensor:
1838
+ if self.config.timestep_distribution.lower() == 'uniform':
1839
+ eps = 1e-05
1840
+ sample = (torch.rand(1, generator=self.generator) + torch.arange(batch_size) / batch_size) % (
1841
+ 1 - eps
1842
+ )
1843
+ elif self.config.timestep_distribution.lower() == 'beta':
1844
+ sample = self.beta_distribution.sample([batch_size, 1, 1])
1845
+ sample = (1 - self.config.sig_min) * (1 - sample)
1846
+ else:
1847
+ raise NotImplementedError(self.config.timestep_distribution)
1848
+ sample = sample.view(batch_size, 1, 1)
1849
+ return sample
1850
+
1851
+ def _psi_t(self, timestep: torch.Tensor, x_0: torch.Tensor, x_1: torch.Tensor) -> torch.Tensor:
1852
+ return (1 - (1 - self.config.sig_min) * timestep) * x_0 + timestep * x_1
1853
+
1854
+ def _dpsi_dt(self, x_0: torch.Tensor, x_1: torch.Tensor) -> torch.Tensor:
1855
+ return x_1 - (1 - self.config.sig_min) * x_0
1856
+
1857
+ def sample_t0_input(self, batch_size: int, device: torch.device) -> FlowInput:
1858
+ if self.config.r0_distribution == 'normal':
1859
+ controls_t0 = torch.randn(
1860
+ [
1861
+ batch_size,
1862
+ self.config.control_io_config.future_controls_sequence_length,
1863
+ 3 + self.rotation_components + 1,
1864
+ ],
1865
+ generator=self.generator,
1866
+ ).to(device=device)
1867
+ (translation_t0, rotation_t0, gripper_t0) = torch.split(
1868
+ controls_t0, [3, self.rotation_components, 1], dim=-1
1869
+ )
1870
+ rotation_t0 = normalize_rotation(rotation_t0)
1871
+ elif self.config.r0_distribution == 'uniform':
1872
+ controls_t0 = torch.randn(
1873
+ [batch_size, self.config.control_io_config.future_controls_sequence_length, 4],
1874
+ generator=self.generator,
1875
+ ).to(device=device)
1876
+ (translation_t0, gripper_t0) = torch.split(controls_t0, [3, 1], dim=-1)
1877
+ rotation_t0 = convert_rotation(
1878
+ roma.random_unitquat(
1879
+ (batch_size, self.config.control_io_config.future_controls_sequence_length), device=device
1880
+ ),
1881
+ self.config.rotation_format,
1882
+ )
1883
+ else:
1884
+ raise NotImplementedError(self.config.r0_distribution)
1885
+ if self.config.rotation_format == RotationFormat.QUATERNION:
1886
+ rotation_t0 = quaternion_half_cover(rotation_t0)
1887
+ timestep = torch.zeros([batch_size, 1, 1], device=device)
1888
+ return FlowInput(
1889
+ timestep=timestep,
1890
+ translation_t0=translation_t0,
1891
+ rotation_t0=rotation_t0,
1892
+ gripper_t0=gripper_t0,
1893
+ translation_t=None,
1894
+ rotation_t=None,
1895
+ gripper_t=None,
1896
+ )
1897
+
1898
+ def policy_control_plan_from_model_output(
1899
+ self, model_output: RoboticsOutput, dataset_name: np.ndarray, valid_mask: torch.Tensor
1900
+ ) -> RoboticsControlPlan:
1901
+ """
1902
+ Called during inference to create control plan from model output
1903
+ See VLAMProcessor.policy_control_plan_from_model_output for arguments
1904
+ """
1905
+ model_output = model_output.replace(
1906
+ translation=torch.clamp(model_output.translation, -1, 1),
1907
+ rotation=torch.clamp(model_output.rotation, -1, 1),
1908
+ )
1909
+ control_plan = super().policy_control_plan_from_model_output(
1910
+ model_output=model_output, dataset_name=dataset_name, valid_mask=valid_mask
1911
+ )
1912
+ control_plan = control_plan.replace(gripper_prob=torch.clamp(model_output.gripper, 0, 1))
1913
+ return control_plan