solarian-diffusion 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- diffusion_policy/__init__.py +0 -0
- diffusion_policy/codecs/__init__.py +0 -0
- diffusion_policy/codecs/imagecodecs_numcodecs.py +1408 -0
- diffusion_policy/common/__init__.py +0 -0
- diffusion_policy/common/checkpoint_util.py +64 -0
- diffusion_policy/common/json_logger.py +117 -0
- diffusion_policy/common/normalize_util.py +251 -0
- diffusion_policy/common/pose_repr_util.py +112 -0
- diffusion_policy/common/pytorch_util.py +82 -0
- diffusion_policy/common/q_critic_util.py +135 -0
- diffusion_policy/common/replay_buffer.py +672 -0
- diffusion_policy/common/sampler.py +204 -0
- diffusion_policy/config/legacy/task/blockpush_lowdim_seed.yaml +34 -0
- diffusion_policy/config/legacy/task/blockpush_lowdim_seed_abs.yaml +34 -0
- diffusion_policy/config/legacy/task/can_image.yaml +64 -0
- diffusion_policy/config/legacy/task/can_image_abs.yaml +64 -0
- diffusion_policy/config/legacy/task/can_lowdim.yaml +45 -0
- diffusion_policy/config/legacy/task/can_lowdim_abs.yaml +46 -0
- diffusion_policy/config/legacy/task/kitchen_lowdim.yaml +33 -0
- diffusion_policy/config/legacy/task/kitchen_lowdim_abs.yaml +38 -0
- diffusion_policy/config/legacy/task/lift_abs.yaml +72 -0
- diffusion_policy/config/legacy/task/lift_image.yaml +64 -0
- diffusion_policy/config/legacy/task/lift_image_abs.yaml +63 -0
- diffusion_policy/config/legacy/task/lift_lowdim.yaml +46 -0
- diffusion_policy/config/legacy/task/lift_lowdim_abs.yaml +47 -0
- diffusion_policy/config/legacy/task/pusht_image.yaml +40 -0
- diffusion_policy/config/legacy/task/pusht_lowdim.yaml +34 -0
- diffusion_policy/config/legacy/task/real_pusht_image.yaml +47 -0
- diffusion_policy/config/legacy/task/square_abs.yaml +72 -0
- diffusion_policy/config/legacy/task/square_image.yaml +64 -0
- diffusion_policy/config/legacy/task/square_image_abs.yaml +64 -0
- diffusion_policy/config/legacy/task/square_lowdim.yaml +46 -0
- diffusion_policy/config/legacy/task/square_lowdim_abs.yaml +47 -0
- diffusion_policy/config/legacy/task/tool_hang_abs.yaml +71 -0
- diffusion_policy/config/legacy/task/tool_hang_image.yaml +63 -0
- diffusion_policy/config/legacy/task/tool_hang_image_abs.yaml +63 -0
- diffusion_policy/config/legacy/task/tool_hang_lowdim.yaml +45 -0
- diffusion_policy/config/legacy/task/tool_hang_lowdim_abs.yaml +46 -0
- diffusion_policy/config/legacy/task/transport_image.yaml +75 -0
- diffusion_policy/config/legacy/task/transport_image_abs.yaml +75 -0
- diffusion_policy/config/legacy/task/transport_lowdim.yaml +49 -0
- diffusion_policy/config/legacy/task/transport_lowdim_abs.yaml +49 -0
- diffusion_policy/config/legacy/task/umi.yaml +45 -0
- diffusion_policy/config/legacy/task/umi_image.yaml +36 -0
- diffusion_policy/config/legacy/task/umi_teleop_image.yaml +41 -0
- diffusion_policy/config/legacy/train_bet_lowdim_workspace.yaml +132 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_hybrid_workspace.yaml +141 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_lowdim_kitchen_workspace.yaml +146 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_lowdim_pusht_workspace.yaml +146 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_lowdim_workspace.yaml +146 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_real_hybrid_workspace.yaml +144 -0
- diffusion_policy/config/legacy/train_diffusion_transformer_umi_teleop_hybrid_workspace.yaml +156 -0
- diffusion_policy/config/legacy/train_diffusion_unet_ddim_hybrid_workspace.yaml +140 -0
- diffusion_policy/config/legacy/train_diffusion_unet_ddim_lowdim_workspace.yaml +146 -0
- diffusion_policy/config/legacy/train_diffusion_unet_hybrid_workspace.yaml +137 -0
- diffusion_policy/config/legacy/train_diffusion_unet_image_pretrained_workspace.yaml +149 -0
- diffusion_policy/config/legacy/train_diffusion_unet_image_workspace.yaml +150 -0
- diffusion_policy/config/legacy/train_diffusion_unet_lowdim_workspace.yaml +143 -0
- diffusion_policy/config/legacy/train_diffusion_unet_real_hybrid_workspace.yaml +141 -0
- diffusion_policy/config/legacy/train_diffusion_unet_real_image_workspace.yaml +152 -0
- diffusion_policy/config/legacy/train_diffusion_unet_real_pretrained_workspace.yaml +152 -0
- diffusion_policy/config/legacy/train_diffusion_unet_timm_workspace.yaml +145 -0
- diffusion_policy/config/legacy/train_diffusion_unet_umi_hybrid_workspace.yaml +141 -0
- diffusion_policy/config/legacy/train_diffusion_unet_umi_teleop_hybrid_workspace.yaml +153 -0
- diffusion_policy/config/legacy/train_diffusion_unet_umi_timm_workspace.yaml +145 -0
- diffusion_policy/config/legacy/train_diffusion_unet_umi_workspace.yaml +150 -0
- diffusion_policy/config/legacy/train_diffusion_unet_video_workspace.yaml +128 -0
- diffusion_policy/config/legacy/train_ibc_dfo_hybrid_workspace.yaml +111 -0
- diffusion_policy/config/legacy/train_ibc_dfo_lowdim_workspace.yaml +109 -0
- diffusion_policy/config/legacy/train_ibc_dfo_real_hybrid_workspace.yaml +111 -0
- diffusion_policy/config/legacy/train_robomimic_image_workspace.yaml +92 -0
- diffusion_policy/config/legacy/train_robomimic_lowdim_workspace.yaml +92 -0
- diffusion_policy/config/legacy/train_robomimic_real_image_workspace.yaml +92 -0
- diffusion_policy/config/rl_annotate.yaml +265 -0
- diffusion_policy/config/rl_annotate_per_frame.yaml +162 -0
- diffusion_policy/config/rl_annotate_per_frame_v4.yaml +162 -0
- diffusion_policy/config/rl_annotate_v2.yaml +141 -0
- diffusion_policy/config/rl_annotate_v3.yaml +165 -0
- diffusion_policy/config/task/lift.yaml +89 -0
- diffusion_policy/config/task/square.yaml +92 -0
- diffusion_policy/config/task/tool_hang.yaml +93 -0
- diffusion_policy/config/task/tool_hang_abs.yaml +71 -0
- diffusion_policy/config/task/umi.yaml +105 -0
- diffusion_policy/config/task/umi_bimanual.yaml +135 -0
- diffusion_policy/config/task/umi_cup_wild.yaml +106 -0
- diffusion_policy/config/task/umi_image.yaml +36 -0
- diffusion_policy/config/task/umi_pick_lego.yaml +106 -0
- diffusion_policy/config/task/umi_pick_single_lego_175.yaml +106 -0
- diffusion_policy/config/task/umi_pick_single_lego_bbox.yaml +135 -0
- diffusion_policy/config/task/umi_teleop_image.yaml +41 -0
- diffusion_policy/config/train_diffusion_transformer_umi_bimanual_workspace.yaml +185 -0
- diffusion_policy/config/train_diffusion_transformer_umi_workspace.yaml +185 -0
- diffusion_policy/config/train_diffusion_unet_image_workspace.yaml +152 -0
- diffusion_policy/config/train_diffusion_unet_timm_robomimic_workspace.yaml +152 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_cup_wild.yaml +260 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_lego_pile_bbox.yaml +265 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_lego_pile_bbox_awr.yaml +324 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_lego_pile_bbox_parl.yaml +399 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_single_lego.yaml +258 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_single_lego_125.yaml +260 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_single_lego_175.yaml +260 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_single_lego_bbox.yaml +265 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_pick_two_legos_bbox.yaml +265 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_workspace.yaml +224 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_workspace_lego.yaml +213 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_workspace_lego_resume.yaml +213 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_workspace_pick_lego.yaml +254 -0
- diffusion_policy/config/train_diffusion_unet_timm_umi_workspace_red_cup.yaml +219 -0
- diffusion_policy/config/train_diffusion_unet_umi_bimanual_workspace.yaml +203 -0
- diffusion_policy/config/train_iql_umi.yaml +186 -0
- diffusion_policy/config/train_iql_umi_v2.yaml +212 -0
- diffusion_policy/config/train_iql_umi_v3.yaml +212 -0
- diffusion_policy/dataset/__init__.py +0 -0
- diffusion_policy/dataset/base_dataset.py +76 -0
- diffusion_policy/dataset/umi_dataset.py +652 -0
- diffusion_policy/env_runner/__init__.py +0 -0
- diffusion_policy/env_runner/base_image_runner.py +9 -0
- diffusion_policy/env_runner/real_pusht_image_runner.py +10 -0
- diffusion_policy/model/__init__.py +0 -0
- diffusion_policy/model/common/__init__.py +0 -0
- diffusion_policy/model/common/dict_of_tensor_mixin.py +38 -0
- diffusion_policy/model/common/lr_scheduler.py +46 -0
- diffusion_policy/model/common/module_attr_mixin.py +14 -0
- diffusion_policy/model/common/normalizer.py +353 -0
- diffusion_policy/model/common/tensor_util.py +960 -0
- diffusion_policy/model/diffusion/__init__.py +0 -0
- diffusion_policy/model/diffusion/conditional_unet1d.py +238 -0
- diffusion_policy/model/diffusion/conv1d_components.py +46 -0
- diffusion_policy/model/diffusion/ema_model.py +88 -0
- diffusion_policy/model/diffusion/mask_generator.py +221 -0
- diffusion_policy/model/diffusion/positional_embedding.py +17 -0
- diffusion_policy/model/vision/__init__.py +0 -0
- diffusion_policy/model/vision/crop_randomizer.py +288 -0
- diffusion_policy/model/vision/multi_image_obs_encoder.py +195 -0
- diffusion_policy/model/vision/timm_obs_encoder.py +525 -0
- diffusion_policy/policy/__init__.py +0 -0
- diffusion_policy/policy/base_image_policy.py +27 -0
- diffusion_policy/policy/diffusion_unet_image_policy.py +264 -0
- diffusion_policy/policy/diffusion_unet_timm_policy.py +607 -0
- diffusion_policy/policy/parl_action_optimizer.py +675 -0
- diffusion_policy/workspace/__init__.py +0 -0
- diffusion_policy/workspace/base_workspace.py +146 -0
- diffusion_policy/workspace/train_diffusion_unet_image_workspace.py +1585 -0
- solarian_diffusion/__init__.py +32 -0
- solarian_diffusion/config.py +31 -0
- solarian_diffusion/policy_server.py +316 -0
- solarian_diffusion/rtc.py +272 -0
- solarian_diffusion/service.py +16 -0
- solarian_diffusion/trainer.py +213 -0
- solarian_diffusion-0.1.0.dist-info/METADATA +126 -0
- solarian_diffusion-0.1.0.dist-info/RECORD +159 -0
- solarian_diffusion-0.1.0.dist-info/WHEEL +5 -0
- solarian_diffusion-0.1.0.dist-info/entry_points.txt +8 -0
- solarian_diffusion-0.1.0.dist-info/licenses/LICENSE +201 -0
- solarian_diffusion-0.1.0.dist-info/licenses/NOTICE +45 -0
- solarian_diffusion-0.1.0.dist-info/top_level.txt +3 -0
- umi/__init__.py +0 -0
- umi/common/__init__.py +0 -0
- umi/common/pose_util.py +115 -0
|
@@ -0,0 +1,652 @@
|
|
|
1
|
+
import copy
|
|
2
|
+
from typing import Dict, Optional
|
|
3
|
+
|
|
4
|
+
import os
|
|
5
|
+
from datetime import datetime
|
|
6
|
+
import pathlib
|
|
7
|
+
import numpy as np
|
|
8
|
+
import torch
|
|
9
|
+
import zarr
|
|
10
|
+
from threadpoolctl import threadpool_limits
|
|
11
|
+
from tqdm import trange, tqdm
|
|
12
|
+
from filelock import FileLock
|
|
13
|
+
import shutil
|
|
14
|
+
|
|
15
|
+
from diffusion_policy.codecs.imagecodecs_numcodecs import register_codecs
|
|
16
|
+
from diffusion_policy.common.normalize_util import (
|
|
17
|
+
array_to_stats, concatenate_normalizer, get_identity_normalizer_from_stat,
|
|
18
|
+
get_image_identity_normalizer, get_range_normalizer_from_stat)
|
|
19
|
+
from diffusion_policy.common.pose_repr_util import convert_pose_mat_rep
|
|
20
|
+
from diffusion_policy.common.pytorch_util import dict_apply
|
|
21
|
+
from diffusion_policy.common.replay_buffer import ReplayBuffer
|
|
22
|
+
from diffusion_policy.common.sampler import SequenceSampler, get_val_mask
|
|
23
|
+
from diffusion_policy.dataset.base_dataset import BaseDataset
|
|
24
|
+
from diffusion_policy.model.common.normalizer import LinearNormalizer
|
|
25
|
+
from umi.common.pose_util import pose_to_mat, mat_to_pose10d
|
|
26
|
+
|
|
27
|
+
register_codecs()
|
|
28
|
+
|
|
29
|
+
def _try_load_rl_q_v(dataset_path: str):
|
|
30
|
+
"""Best-effort load of per-frame Q and V arrays from the source zarr.
|
|
31
|
+
|
|
32
|
+
``zarr_q_v.py`` writes ``rl/q`` and ``rl/v`` (shape ``(n_frames,)``,
|
|
33
|
+
NaN where the frame is not a valid IQL anchor) alongside ``data/``
|
|
34
|
+
and ``meta/`` in the source ``.zarr.zip``. ``ReplayBuffer.copy_from_store``
|
|
35
|
+
only copies ``meta/`` and ``data/``, so the in-memory replay buffer
|
|
36
|
+
used by the rest of this dataset never sees these arrays. We open
|
|
37
|
+
the source zip-store directly, copy them into RAM (a few MB at
|
|
38
|
+
most, since they're per-frame fp32 vectors), and return them so
|
|
39
|
+
``__getitem__`` can hand back per-sample advantages without touching
|
|
40
|
+
the zarr again. Returns ``(None, None)`` if the source zarr has no
|
|
41
|
+
``rl/q`` / ``rl/v`` -- callers treat this as "no advantage info"
|
|
42
|
+
and fall back to plain BC behavior.
|
|
43
|
+
"""
|
|
44
|
+
try:
|
|
45
|
+
with zarr.ZipStore(dataset_path, mode='r') as zip_store:
|
|
46
|
+
root = zarr.open(zip_store, mode='r')
|
|
47
|
+
if 'rl' not in root:
|
|
48
|
+
return None, None
|
|
49
|
+
rl_grp = root['rl']
|
|
50
|
+
if 'q' not in rl_grp or 'v' not in rl_grp:
|
|
51
|
+
return None, None
|
|
52
|
+
# Force full read into RAM. The arrays are 1-D float32 of
|
|
53
|
+
# length n_frames (a few MB even for the largest UMI sets).
|
|
54
|
+
q = np.asarray(rl_grp['q'][:], dtype=np.float32)
|
|
55
|
+
v = np.asarray(rl_grp['v'][:], dtype=np.float32)
|
|
56
|
+
if q.shape != v.shape or q.ndim != 1:
|
|
57
|
+
# Defensive: if these ever go out of sync just disable AWR
|
|
58
|
+
# rather than indexing into a mismatched array.
|
|
59
|
+
return None, None
|
|
60
|
+
return q, v
|
|
61
|
+
except Exception:
|
|
62
|
+
# Any I/O hiccup -> treat as "no advantage info". The caller
|
|
63
|
+
# will silently skip the AWR weighting path.
|
|
64
|
+
return None, None
|
|
65
|
+
|
|
66
|
+
|
|
67
|
+
class UmiDataset(BaseDataset):
|
|
68
|
+
def __init__(self,
|
|
69
|
+
shape_meta: dict,
|
|
70
|
+
dataset_path: str,
|
|
71
|
+
cache_dir: Optional[str]=None,
|
|
72
|
+
pose_repr: dict={},
|
|
73
|
+
action_padding: bool=False,
|
|
74
|
+
temporally_independent_normalization: bool=False,
|
|
75
|
+
repeat_frame_prob: float=0.0,
|
|
76
|
+
seed: int=42,
|
|
77
|
+
val_ratio: float=0.0,
|
|
78
|
+
max_duration: Optional[float]=None,
|
|
79
|
+
# PA-RL: optional per-frame cache of "optimized" action chunks
|
|
80
|
+
# produced by a relabel pass (see
|
|
81
|
+
# diffusion_policy.policy.parl_action_optimizer.optimize_actions
|
|
82
|
+
# and the workspace's _parl_relabel hook). When present, each
|
|
83
|
+
# __getitem__ emits two extra keys:
|
|
84
|
+
#
|
|
85
|
+
# action_optimized: (H, A) UNNORMALIZED action chunk that
|
|
86
|
+
# the diffusion policy should distill
|
|
87
|
+
# toward via the standard DDPM loss.
|
|
88
|
+
# has_action_optimized: scalar 1.0 / 0.0 gate (matches the
|
|
89
|
+
# has_advantage convention).
|
|
90
|
+
#
|
|
91
|
+
# When None (the default), no extra keys are emitted and this
|
|
92
|
+
# dataset is byte-identical to the legacy AWR / BC code paths.
|
|
93
|
+
# Both arguments accept either a numpy memmap-able .npy path
|
|
94
|
+
# OR an already-loaded numpy array (used by the workspace's
|
|
95
|
+
# in-memory epoch-boundary swap).
|
|
96
|
+
action_optimized_path: Optional[str]=None,
|
|
97
|
+
action_optimized_valid_path: Optional[str]=None,
|
|
98
|
+
):
|
|
99
|
+
self.pose_repr = pose_repr
|
|
100
|
+
self.obs_pose_repr = self.pose_repr.get('obs_pose_repr', 'rel')
|
|
101
|
+
self.action_pose_repr = self.pose_repr.get('action_pose_repr', 'rel')
|
|
102
|
+
|
|
103
|
+
if cache_dir is None:
|
|
104
|
+
# load into memory store
|
|
105
|
+
with zarr.ZipStore(dataset_path, mode='r') as zip_store:
|
|
106
|
+
replay_buffer = ReplayBuffer.copy_from_store(
|
|
107
|
+
src_store=zip_store,
|
|
108
|
+
store=zarr.MemoryStore()
|
|
109
|
+
)
|
|
110
|
+
else:
|
|
111
|
+
# TODO: refactor into a stand alone function?
|
|
112
|
+
# determine path name
|
|
113
|
+
mod_time = os.path.getmtime(dataset_path)
|
|
114
|
+
stamp = datetime.fromtimestamp(mod_time).isoformat()
|
|
115
|
+
stem_name = os.path.basename(dataset_path).split('.')[0]
|
|
116
|
+
cache_name = '_'.join([stem_name, stamp])
|
|
117
|
+
cache_dir = pathlib.Path(os.path.expanduser(cache_dir))
|
|
118
|
+
cache_dir.mkdir(parents=True, exist_ok=True)
|
|
119
|
+
cache_path = cache_dir.joinpath(cache_name + '.zarr.mdb')
|
|
120
|
+
lock_path = cache_dir.joinpath(cache_name + '.lock')
|
|
121
|
+
|
|
122
|
+
# load cached file
|
|
123
|
+
print('Acquiring lock on cache.')
|
|
124
|
+
with FileLock(lock_path):
|
|
125
|
+
# cache does not exist
|
|
126
|
+
if not cache_path.exists():
|
|
127
|
+
try:
|
|
128
|
+
with zarr.LMDBStore(str(cache_path),
|
|
129
|
+
writemap=True, metasync=False, sync=False, map_async=True, lock=False
|
|
130
|
+
) as lmdb_store:
|
|
131
|
+
with zarr.ZipStore(dataset_path, mode='r') as zip_store:
|
|
132
|
+
print(f"Copying data to {str(cache_path)}")
|
|
133
|
+
ReplayBuffer.copy_from_store(
|
|
134
|
+
src_store=zip_store,
|
|
135
|
+
store=lmdb_store
|
|
136
|
+
)
|
|
137
|
+
print("Cache written to disk!")
|
|
138
|
+
except Exception as e:
|
|
139
|
+
shutil.rmtree(cache_path)
|
|
140
|
+
raise e
|
|
141
|
+
|
|
142
|
+
# open read-only lmdb store
|
|
143
|
+
store = zarr.LMDBStore(str(cache_path), readonly=True, lock=False)
|
|
144
|
+
replay_buffer = ReplayBuffer.create_from_group(
|
|
145
|
+
group=zarr.group(store)
|
|
146
|
+
)
|
|
147
|
+
|
|
148
|
+
self.num_robot = 0
|
|
149
|
+
rgb_keys = list()
|
|
150
|
+
lowdim_keys = list()
|
|
151
|
+
key_horizon = dict()
|
|
152
|
+
key_down_sample_steps = dict()
|
|
153
|
+
key_latency_steps = dict()
|
|
154
|
+
obs_shape_meta = shape_meta['obs']
|
|
155
|
+
for key, attr in obs_shape_meta.items():
|
|
156
|
+
# solve obs type
|
|
157
|
+
type = attr.get('type', 'low_dim')
|
|
158
|
+
if type == 'rgb':
|
|
159
|
+
rgb_keys.append(key)
|
|
160
|
+
elif type == 'low_dim':
|
|
161
|
+
lowdim_keys.append(key)
|
|
162
|
+
|
|
163
|
+
if key.endswith('eef_pos'):
|
|
164
|
+
self.num_robot += 1
|
|
165
|
+
|
|
166
|
+
# solve obs_horizon
|
|
167
|
+
horizon = shape_meta['obs'][key]['horizon']
|
|
168
|
+
key_horizon[key] = horizon
|
|
169
|
+
|
|
170
|
+
# solve latency_steps
|
|
171
|
+
latency_steps = shape_meta['obs'][key]['latency_steps']
|
|
172
|
+
key_latency_steps[key] = latency_steps
|
|
173
|
+
|
|
174
|
+
# solve down_sample_steps
|
|
175
|
+
down_sample_steps = shape_meta['obs'][key]['down_sample_steps']
|
|
176
|
+
key_down_sample_steps[key] = down_sample_steps
|
|
177
|
+
|
|
178
|
+
# solve action
|
|
179
|
+
key_horizon['action'] = shape_meta['action']['horizon']
|
|
180
|
+
key_latency_steps['action'] = shape_meta['action']['latency_steps']
|
|
181
|
+
key_down_sample_steps['action'] = shape_meta['action']['down_sample_steps']
|
|
182
|
+
|
|
183
|
+
val_mask = get_val_mask(
|
|
184
|
+
n_episodes=replay_buffer.n_episodes,
|
|
185
|
+
val_ratio=val_ratio,
|
|
186
|
+
seed=seed
|
|
187
|
+
)
|
|
188
|
+
train_mask = ~val_mask
|
|
189
|
+
|
|
190
|
+
self.sampler_lowdim_keys = list()
|
|
191
|
+
for key in lowdim_keys:
|
|
192
|
+
if not 'wrt' in key:
|
|
193
|
+
self.sampler_lowdim_keys.append(key)
|
|
194
|
+
|
|
195
|
+
for key in replay_buffer.keys():
|
|
196
|
+
if key.endswith('_demo_start_pose') or key.endswith('_demo_end_pose'):
|
|
197
|
+
self.sampler_lowdim_keys.append(key)
|
|
198
|
+
query_key = key.split('_')[0] + '_eef_pos'
|
|
199
|
+
key_horizon[key] = shape_meta['obs'][query_key]['horizon']
|
|
200
|
+
key_latency_steps[key] = shape_meta['obs'][query_key]['latency_steps']
|
|
201
|
+
key_down_sample_steps[key] = shape_meta['obs'][query_key]['down_sample_steps']
|
|
202
|
+
|
|
203
|
+
sampler = SequenceSampler(
|
|
204
|
+
shape_meta=shape_meta,
|
|
205
|
+
replay_buffer=replay_buffer,
|
|
206
|
+
rgb_keys=rgb_keys,
|
|
207
|
+
lowdim_keys=self.sampler_lowdim_keys,
|
|
208
|
+
key_horizon=key_horizon,
|
|
209
|
+
key_latency_steps=key_latency_steps,
|
|
210
|
+
key_down_sample_steps=key_down_sample_steps,
|
|
211
|
+
episode_mask=train_mask,
|
|
212
|
+
action_padding=action_padding,
|
|
213
|
+
repeat_frame_prob=repeat_frame_prob,
|
|
214
|
+
max_duration=max_duration
|
|
215
|
+
)
|
|
216
|
+
self.shape_meta = shape_meta
|
|
217
|
+
self.replay_buffer = replay_buffer
|
|
218
|
+
self.rgb_keys = rgb_keys
|
|
219
|
+
self.lowdim_keys = lowdim_keys
|
|
220
|
+
self.key_horizon = key_horizon
|
|
221
|
+
self.key_latency_steps = key_latency_steps
|
|
222
|
+
self.key_down_sample_steps = key_down_sample_steps
|
|
223
|
+
self.val_mask = val_mask
|
|
224
|
+
self.action_padding = action_padding
|
|
225
|
+
self.repeat_frame_prob = repeat_frame_prob
|
|
226
|
+
self.max_duration = max_duration
|
|
227
|
+
self.sampler = sampler
|
|
228
|
+
self.temporally_independent_normalization = temporally_independent_normalization
|
|
229
|
+
self.threadpool_limits_is_applied = False
|
|
230
|
+
|
|
231
|
+
# PA-RL optimized-action cache. Configured here from the
|
|
232
|
+
# ctor; the workspace also calls ``set_action_optimized`` to
|
|
233
|
+
# swap in fresh in-memory arrays after each relabel pass. We
|
|
234
|
+
# store the action shape parameters so ``set_action_optimized``
|
|
235
|
+
# can sanity-check inputs without re-deriving them. n_frames
|
|
236
|
+
# is the per-frame index space the cache must cover, matching
|
|
237
|
+
# rl/q + rl/v written by zarr_q_v.py.
|
|
238
|
+
self.action_horizon = int(key_horizon['action'])
|
|
239
|
+
self.action_dim = int(shape_meta['action']['shape'][0])
|
|
240
|
+
self.n_frames_total = int(replay_buffer.n_steps)
|
|
241
|
+
self.action_optimized: Optional[np.ndarray] = None
|
|
242
|
+
self.action_optimized_valid: Optional[np.ndarray] = None
|
|
243
|
+
if action_optimized_path is not None:
|
|
244
|
+
self._load_action_optimized_from_paths(
|
|
245
|
+
action_optimized_path, action_optimized_valid_path,
|
|
246
|
+
)
|
|
247
|
+
|
|
248
|
+
# Best-effort per-frame Q/V (written by zarr_q_v.py). Always
|
|
249
|
+
# loaded if available; the consuming policy decides whether to
|
|
250
|
+
# use them via its own awr_beta knob -- so this is a strict
|
|
251
|
+
# superset of the legacy BC dataset shape and never *requires*
|
|
252
|
+
# rl/q + rl/v to be present.
|
|
253
|
+
self.rl_q, self.rl_v = _try_load_rl_q_v(dataset_path)
|
|
254
|
+
self.has_rl_qv = self.rl_q is not None and self.rl_v is not None
|
|
255
|
+
if self.has_rl_qv:
|
|
256
|
+
valid_mask = ~(np.isnan(self.rl_q) | np.isnan(self.rl_v))
|
|
257
|
+
n_valid = int(valid_mask.sum())
|
|
258
|
+
n_total = int(self.rl_q.shape[0])
|
|
259
|
+
adv_valid = (self.rl_q[valid_mask] - self.rl_v[valid_mask]) \
|
|
260
|
+
if n_valid > 0 else np.zeros(0, dtype=np.float32)
|
|
261
|
+
print(
|
|
262
|
+
f"[UmiDataset] loaded rl/q + rl/v from {dataset_path}: "
|
|
263
|
+
f"{n_valid}/{n_total} frames have advantage "
|
|
264
|
+
f"({100.0 * n_valid / max(1, n_total):.1f}%). "
|
|
265
|
+
f"A: mean={adv_valid.mean() if n_valid else float('nan'):+.4f} "
|
|
266
|
+
f"std={adv_valid.std() if n_valid else float('nan'):.4f} "
|
|
267
|
+
f"min={adv_valid.min() if n_valid else float('nan'):+.4f} "
|
|
268
|
+
f"max={adv_valid.max() if n_valid else float('nan'):+.4f}"
|
|
269
|
+
)
|
|
270
|
+
else:
|
|
271
|
+
print(
|
|
272
|
+
f"[UmiDataset] no rl/q + rl/v in {dataset_path}; "
|
|
273
|
+
f"AWR weighting will be a no-op even if policy.awr_beta > 0."
|
|
274
|
+
)
|
|
275
|
+
|
|
276
|
+
|
|
277
|
+
def _load_action_optimized_from_paths(
|
|
278
|
+
self,
|
|
279
|
+
actions_path: str,
|
|
280
|
+
valid_path: Optional[str],
|
|
281
|
+
) -> None:
|
|
282
|
+
"""Memory-map the per-frame PA-RL optimized-action cache.
|
|
283
|
+
|
|
284
|
+
Args:
|
|
285
|
+
actions_path: ``.npy`` of shape ``(n_frames_total, H, A)``
|
|
286
|
+
``float32``. Out-of-cache rows can hold any value;
|
|
287
|
+
the valid mask is what gates them.
|
|
288
|
+
valid_path: optional ``.npy`` of shape ``(n_frames_total,)``
|
|
289
|
+
``bool``. When ``None`` we assume every row is valid;
|
|
290
|
+
this is only safe if the writer guarantees a value at
|
|
291
|
+
every anchor index in the dataset.
|
|
292
|
+
"""
|
|
293
|
+
actions = np.load(actions_path, mmap_mode='r')
|
|
294
|
+
if actions.ndim != 3:
|
|
295
|
+
raise ValueError(
|
|
296
|
+
f"action_optimized array at {actions_path} has shape "
|
|
297
|
+
f"{actions.shape}, expected 3D (n_frames, H, A)"
|
|
298
|
+
)
|
|
299
|
+
if actions.shape[1] != self.action_horizon \
|
|
300
|
+
or actions.shape[2] != self.action_dim:
|
|
301
|
+
raise ValueError(
|
|
302
|
+
f"action_optimized chunk shape {actions.shape[1:]} != "
|
|
303
|
+
f"({self.action_horizon}, {self.action_dim})"
|
|
304
|
+
)
|
|
305
|
+
if actions.shape[0] != self.n_frames_total:
|
|
306
|
+
print(
|
|
307
|
+
f"[UmiDataset] WARN: action_optimized n_frames "
|
|
308
|
+
f"{actions.shape[0]} != dataset n_frames "
|
|
309
|
+
f"{self.n_frames_total}; will gate by valid mask only."
|
|
310
|
+
)
|
|
311
|
+
if valid_path is not None:
|
|
312
|
+
valid = np.load(valid_path, mmap_mode='r').astype(bool, copy=False)
|
|
313
|
+
if valid.ndim != 1 or valid.shape[0] != actions.shape[0]:
|
|
314
|
+
raise ValueError(
|
|
315
|
+
f"action_optimized_valid shape {valid.shape} "
|
|
316
|
+
f"!= ({actions.shape[0]},)"
|
|
317
|
+
)
|
|
318
|
+
else:
|
|
319
|
+
# Caller asserted "every row valid". We still need a real
|
|
320
|
+
# array so ``__getitem__`` can index uniformly.
|
|
321
|
+
valid = np.ones(actions.shape[0], dtype=bool)
|
|
322
|
+
self.action_optimized = actions
|
|
323
|
+
self.action_optimized_valid = valid
|
|
324
|
+
n_valid = int(valid.sum())
|
|
325
|
+
print(
|
|
326
|
+
f"[UmiDataset] loaded action_optimized cache from "
|
|
327
|
+
f"{actions_path}: {n_valid}/{valid.shape[0]} valid frames "
|
|
328
|
+
f"({100.0 * n_valid / max(1, valid.shape[0]):.1f}%)."
|
|
329
|
+
)
|
|
330
|
+
|
|
331
|
+
def set_action_optimized(
|
|
332
|
+
self,
|
|
333
|
+
actions: Optional[np.ndarray],
|
|
334
|
+
valid: Optional[np.ndarray] = None,
|
|
335
|
+
) -> None:
|
|
336
|
+
"""In-memory swap of the PA-RL optimized-action cache.
|
|
337
|
+
|
|
338
|
+
Used by ``TrainDiffusionUnetImageWorkspace`` to repoint the
|
|
339
|
+
train + val datasets at a freshly-produced optimized-action
|
|
340
|
+
array between epochs without rebuilding the dataset (which
|
|
341
|
+
would re-pay the ZipStore -> ReplayBuffer copy).
|
|
342
|
+
|
|
343
|
+
Pass ``actions=None`` to disable the cache (back to plain BC
|
|
344
|
+
/ AWR behavior). When ``valid is None`` we assume every row is
|
|
345
|
+
valid (only safe if you wrote a value at every anchor index).
|
|
346
|
+
"""
|
|
347
|
+
if actions is None:
|
|
348
|
+
self.action_optimized = None
|
|
349
|
+
self.action_optimized_valid = None
|
|
350
|
+
return
|
|
351
|
+
if actions.ndim != 3 or actions.shape[1] != self.action_horizon \
|
|
352
|
+
or actions.shape[2] != self.action_dim:
|
|
353
|
+
raise ValueError(
|
|
354
|
+
f"set_action_optimized: actions shape {actions.shape} "
|
|
355
|
+
f"!= (*, {self.action_horizon}, {self.action_dim})"
|
|
356
|
+
)
|
|
357
|
+
if valid is None:
|
|
358
|
+
valid = np.ones(actions.shape[0], dtype=bool)
|
|
359
|
+
else:
|
|
360
|
+
valid = valid.astype(bool, copy=False)
|
|
361
|
+
if valid.shape != (actions.shape[0],):
|
|
362
|
+
raise ValueError(
|
|
363
|
+
f"set_action_optimized: valid shape {valid.shape} "
|
|
364
|
+
f"!= ({actions.shape[0]},)"
|
|
365
|
+
)
|
|
366
|
+
self.action_optimized = actions
|
|
367
|
+
self.action_optimized_valid = valid
|
|
368
|
+
|
|
369
|
+
@property
|
|
370
|
+
def has_action_optimized_cache(self) -> bool:
|
|
371
|
+
return self.action_optimized is not None
|
|
372
|
+
|
|
373
|
+
def get_validation_dataset(self):
|
|
374
|
+
val_set = copy.copy(self)
|
|
375
|
+
val_set.sampler = SequenceSampler(
|
|
376
|
+
shape_meta=self.shape_meta,
|
|
377
|
+
replay_buffer=self.replay_buffer,
|
|
378
|
+
rgb_keys=self.rgb_keys,
|
|
379
|
+
lowdim_keys=self.sampler_lowdim_keys,
|
|
380
|
+
key_horizon=self.key_horizon,
|
|
381
|
+
key_latency_steps=self.key_latency_steps,
|
|
382
|
+
key_down_sample_steps=self.key_down_sample_steps,
|
|
383
|
+
episode_mask=self.val_mask,
|
|
384
|
+
action_padding=self.action_padding,
|
|
385
|
+
repeat_frame_prob=self.repeat_frame_prob,
|
|
386
|
+
max_duration=self.max_duration
|
|
387
|
+
)
|
|
388
|
+
val_set.val_mask = ~self.val_mask
|
|
389
|
+
return val_set
|
|
390
|
+
|
|
391
|
+
def get_normalizer(self, **kwargs) -> LinearNormalizer:
|
|
392
|
+
normalizer = LinearNormalizer()
|
|
393
|
+
|
|
394
|
+
# enumerate the dataset and save low_dim data
|
|
395
|
+
data_cache = {key: list() for key in self.lowdim_keys + ['action']}
|
|
396
|
+
self.sampler.ignore_rgb(True)
|
|
397
|
+
dataloader = torch.utils.data.DataLoader(
|
|
398
|
+
dataset=self,
|
|
399
|
+
batch_size=64,
|
|
400
|
+
num_workers=32,
|
|
401
|
+
)
|
|
402
|
+
for batch in tqdm(dataloader, desc='iterating dataset to get normalization'):
|
|
403
|
+
for key in self.lowdim_keys:
|
|
404
|
+
data_cache[key].append(copy.deepcopy(batch['obs'][key]))
|
|
405
|
+
data_cache['action'].append(copy.deepcopy(batch['action']))
|
|
406
|
+
self.sampler.ignore_rgb(False)
|
|
407
|
+
|
|
408
|
+
for key in data_cache.keys():
|
|
409
|
+
data_cache[key] = np.concatenate(data_cache[key])
|
|
410
|
+
assert data_cache[key].shape[0] == len(self.sampler)
|
|
411
|
+
assert len(data_cache[key].shape) == 3
|
|
412
|
+
B, T, D = data_cache[key].shape
|
|
413
|
+
if not self.temporally_independent_normalization:
|
|
414
|
+
data_cache[key] = data_cache[key].reshape(B*T, D)
|
|
415
|
+
|
|
416
|
+
# action
|
|
417
|
+
assert data_cache['action'].shape[-1] % self.num_robot == 0
|
|
418
|
+
dim_a = data_cache['action'].shape[-1] // self.num_robot
|
|
419
|
+
action_normalizers = list()
|
|
420
|
+
for i in range(self.num_robot):
|
|
421
|
+
action_normalizers.append(get_range_normalizer_from_stat(array_to_stats(data_cache['action'][..., i * dim_a: i * dim_a + 3]))) # pos
|
|
422
|
+
action_normalizers.append(get_identity_normalizer_from_stat(array_to_stats(data_cache['action'][..., i * dim_a + 3: (i + 1) * dim_a - 1]))) # rot
|
|
423
|
+
action_normalizers.append(get_range_normalizer_from_stat(array_to_stats(data_cache['action'][..., (i + 1) * dim_a - 1: (i + 1) * dim_a]))) # gripper
|
|
424
|
+
|
|
425
|
+
normalizer['action'] = concatenate_normalizer(action_normalizers)
|
|
426
|
+
|
|
427
|
+
# obs
|
|
428
|
+
for key in self.lowdim_keys:
|
|
429
|
+
stat = array_to_stats(data_cache[key])
|
|
430
|
+
|
|
431
|
+
if key.endswith('pos') or 'pos_wrt' in key:
|
|
432
|
+
this_normalizer = get_range_normalizer_from_stat(stat)
|
|
433
|
+
elif key.endswith('pos_abs'):
|
|
434
|
+
this_normalizer = get_range_normalizer_from_stat(stat)
|
|
435
|
+
elif key.endswith('rot_axis_angle') or 'rot_axis_angle_wrt' in key:
|
|
436
|
+
this_normalizer = get_identity_normalizer_from_stat(stat)
|
|
437
|
+
elif key.endswith('gripper_width'):
|
|
438
|
+
this_normalizer = get_range_normalizer_from_stat(stat)
|
|
439
|
+
elif key.endswith('bbox'):
|
|
440
|
+
# Pixel-space bbox [x0, y0, x1, y1]. Two modes:
|
|
441
|
+
# * low_dim: map observed range -> [-1, 1] (the bbox is
|
|
442
|
+
# concatenated as 4 scalars onto the obs-
|
|
443
|
+
# encoder feature vector, so a [-1, 1] scale
|
|
444
|
+
# matches the proprio features).
|
|
445
|
+
# * mask_channel: keep raw pixel coords (identity) because
|
|
446
|
+
# the encoder uses them to rasterize a binary
|
|
447
|
+
# mask in pixel space. Normalizing to [-1, 1]
|
|
448
|
+
# would force the encoder to invert the
|
|
449
|
+
# normalization before rasterizing.
|
|
450
|
+
bbox_attr = self.shape_meta['obs'].get(key, {})
|
|
451
|
+
bbox_mode = bbox_attr.get('bbox_mode', 'low_dim')
|
|
452
|
+
if bbox_mode == 'mask_channel':
|
|
453
|
+
this_normalizer = get_identity_normalizer_from_stat(stat)
|
|
454
|
+
else:
|
|
455
|
+
this_normalizer = get_range_normalizer_from_stat(stat)
|
|
456
|
+
else:
|
|
457
|
+
raise RuntimeError('unsupported')
|
|
458
|
+
normalizer[key] = this_normalizer
|
|
459
|
+
|
|
460
|
+
# image
|
|
461
|
+
for key in self.rgb_keys:
|
|
462
|
+
normalizer[key] = get_image_identity_normalizer()
|
|
463
|
+
return normalizer
|
|
464
|
+
|
|
465
|
+
def __len__(self):
|
|
466
|
+
return len(self.sampler)
|
|
467
|
+
|
|
468
|
+
def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
|
|
469
|
+
if not self.threadpool_limits_is_applied:
|
|
470
|
+
threadpool_limits(1)
|
|
471
|
+
self.threadpool_limits_is_applied = True
|
|
472
|
+
# Resolve the dataset frame this sample anchors at BEFORE we
|
|
473
|
+
# delegate to the sampler. The sampler stores
|
|
474
|
+
# ``indices = [(current_idx, start_idx, end_idx, ...)]`` and
|
|
475
|
+
# ``current_idx`` is the global frame index at which the action
|
|
476
|
+
# chunk starts -- exactly the frame whose Q/V we need.
|
|
477
|
+
try:
|
|
478
|
+
anchor_idx = int(self.sampler.indices[idx][0])
|
|
479
|
+
except Exception:
|
|
480
|
+
anchor_idx = -1
|
|
481
|
+
data = self.sampler.sample_sequence(idx)
|
|
482
|
+
|
|
483
|
+
obs_dict = dict()
|
|
484
|
+
for key in self.rgb_keys:
|
|
485
|
+
if not key in data:
|
|
486
|
+
continue
|
|
487
|
+
# move channel last to channel first
|
|
488
|
+
# T,H,W,C
|
|
489
|
+
# convert uint8 image to float32
|
|
490
|
+
obs_dict[key] = np.moveaxis(data[key], -1, 1).astype(np.float32) / 255.
|
|
491
|
+
# T,C,H,W
|
|
492
|
+
del data[key]
|
|
493
|
+
for key in self.sampler_lowdim_keys:
|
|
494
|
+
obs_dict[key] = data[key].astype(np.float32)
|
|
495
|
+
del data[key]
|
|
496
|
+
|
|
497
|
+
# generate relative pose between two ees
|
|
498
|
+
for robot_id in range(self.num_robot):
|
|
499
|
+
# convert pose to mat
|
|
500
|
+
pose_mat = pose_to_mat(np.concatenate([
|
|
501
|
+
obs_dict[f'robot{robot_id}_eef_pos'],
|
|
502
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle']
|
|
503
|
+
], axis=-1))
|
|
504
|
+
for other_robot_id in range(self.num_robot):
|
|
505
|
+
if robot_id == other_robot_id:
|
|
506
|
+
continue
|
|
507
|
+
if not f'robot{robot_id}_eef_pos_wrt{other_robot_id}' in self.lowdim_keys:
|
|
508
|
+
continue
|
|
509
|
+
other_pose_mat = pose_to_mat(np.concatenate([
|
|
510
|
+
obs_dict[f'robot{other_robot_id}_eef_pos'],
|
|
511
|
+
obs_dict[f'robot{other_robot_id}_eef_rot_axis_angle']
|
|
512
|
+
], axis=-1))
|
|
513
|
+
rel_obs_pose_mat = convert_pose_mat_rep(
|
|
514
|
+
pose_mat,
|
|
515
|
+
base_pose_mat=other_pose_mat[-1],
|
|
516
|
+
pose_rep='relative',
|
|
517
|
+
backward=False)
|
|
518
|
+
rel_obs_pose = mat_to_pose10d(rel_obs_pose_mat)
|
|
519
|
+
obs_dict[f'robot{robot_id}_eef_pos_wrt{other_robot_id}'] = rel_obs_pose[:,:3]
|
|
520
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle_wrt{other_robot_id}'] = rel_obs_pose[:,3:]
|
|
521
|
+
|
|
522
|
+
# generate relative pose with respect to episode start
|
|
523
|
+
for robot_id in range(self.num_robot):
|
|
524
|
+
# HACK: add noise to episode start pose
|
|
525
|
+
if (f'robot{other_robot_id}_eef_pos_wrt_start' not in self.shape_meta['obs']) and \
|
|
526
|
+
(f'robot{other_robot_id}_eef_rot_axis_angle_wrt_start' not in self.shape_meta['obs']):
|
|
527
|
+
continue
|
|
528
|
+
|
|
529
|
+
# convert pose to mat
|
|
530
|
+
pose_mat = pose_to_mat(np.concatenate([
|
|
531
|
+
obs_dict[f'robot{robot_id}_eef_pos'],
|
|
532
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle']
|
|
533
|
+
], axis=-1))
|
|
534
|
+
|
|
535
|
+
# get start pose
|
|
536
|
+
start_pose = obs_dict[f'robot{robot_id}_demo_start_pose'][0]
|
|
537
|
+
# HACK: add noise to episode start pose
|
|
538
|
+
start_pose += np.random.normal(scale=[0.05,0.05,0.05,0.05,0.05,0.05],size=start_pose.shape)
|
|
539
|
+
start_pose_mat = pose_to_mat(start_pose)
|
|
540
|
+
rel_obs_pose_mat = convert_pose_mat_rep(
|
|
541
|
+
pose_mat,
|
|
542
|
+
base_pose_mat=start_pose_mat,
|
|
543
|
+
pose_rep='relative',
|
|
544
|
+
backward=False)
|
|
545
|
+
|
|
546
|
+
rel_obs_pose = mat_to_pose10d(rel_obs_pose_mat)
|
|
547
|
+
# HACK: add noise to episode start pose
|
|
548
|
+
# obs_dict[f'robot{robot_id}_eef_pos_wrt_start'] = rel_obs_pose[:,:3]
|
|
549
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle_wrt_start'] = rel_obs_pose[:,3:]
|
|
550
|
+
|
|
551
|
+
del_keys = list()
|
|
552
|
+
for key in obs_dict:
|
|
553
|
+
if key.endswith('_demo_start_pose') or key.endswith('_demo_end_pose'):
|
|
554
|
+
del_keys.append(key)
|
|
555
|
+
for key in del_keys:
|
|
556
|
+
del obs_dict[key]
|
|
557
|
+
|
|
558
|
+
actions = list()
|
|
559
|
+
for robot_id in range(self.num_robot):
|
|
560
|
+
# convert pose to mat
|
|
561
|
+
pose_mat = pose_to_mat(np.concatenate([
|
|
562
|
+
obs_dict[f'robot{robot_id}_eef_pos'],
|
|
563
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle']
|
|
564
|
+
], axis=-1))
|
|
565
|
+
action_mat = pose_to_mat(data['action'][...,7 * robot_id: 7 * robot_id + 6])
|
|
566
|
+
|
|
567
|
+
# solve relative obs
|
|
568
|
+
obs_pose_mat = convert_pose_mat_rep(
|
|
569
|
+
pose_mat,
|
|
570
|
+
base_pose_mat=pose_mat[-1],
|
|
571
|
+
pose_rep=self.obs_pose_repr,
|
|
572
|
+
backward=False)
|
|
573
|
+
action_pose_mat = convert_pose_mat_rep(
|
|
574
|
+
action_mat,
|
|
575
|
+
base_pose_mat=pose_mat[-1],
|
|
576
|
+
pose_rep=self.obs_pose_repr,
|
|
577
|
+
backward=False)
|
|
578
|
+
|
|
579
|
+
# convert pose to pos + rot6d representation
|
|
580
|
+
obs_pose = mat_to_pose10d(obs_pose_mat)
|
|
581
|
+
action_pose = mat_to_pose10d(action_pose_mat)
|
|
582
|
+
|
|
583
|
+
action_gripper = data['action'][..., 7 * robot_id + 6: 7 * robot_id + 7]
|
|
584
|
+
actions.append(np.concatenate([action_pose, action_gripper], axis=-1))
|
|
585
|
+
|
|
586
|
+
# generate data
|
|
587
|
+
obs_dict[f'robot{robot_id}_eef_pos'] = obs_pose[:,:3]
|
|
588
|
+
obs_dict[f'robot{robot_id}_eef_rot_axis_angle'] = obs_pose[:,3:]
|
|
589
|
+
|
|
590
|
+
data['action'] = np.concatenate(actions, axis=-1)
|
|
591
|
+
|
|
592
|
+
# Per-sample advantage for AWR-style finetuning. Always emitted
|
|
593
|
+
# so the batch shape is identical regardless of whether the
|
|
594
|
+
# source zarr carries rl/q + rl/v -- the policy decides whether
|
|
595
|
+
# to use them. ``has_advantage`` is the gate the policy reads:
|
|
596
|
+
# 1.0 means "this sample has a real Q-V signal, use it";
|
|
597
|
+
# 0.0 means "fall back to plain BC weight=1 for this sample".
|
|
598
|
+
# Frames flagged by zarr_q_v.py as not-an-IQL-anchor (NaN in
|
|
599
|
+
# rl/q or rl/v) are degraded to has_advantage=0 here so the
|
|
600
|
+
# downstream loss never sees a NaN.
|
|
601
|
+
if self.has_rl_qv and 0 <= anchor_idx < int(self.rl_q.shape[0]):
|
|
602
|
+
q = float(self.rl_q[anchor_idx])
|
|
603
|
+
v = float(self.rl_v[anchor_idx])
|
|
604
|
+
if np.isnan(q) or np.isnan(v):
|
|
605
|
+
advantage = 0.0
|
|
606
|
+
has_advantage = 0.0
|
|
607
|
+
else:
|
|
608
|
+
advantage = q - v
|
|
609
|
+
has_advantage = 1.0
|
|
610
|
+
else:
|
|
611
|
+
advantage = 0.0
|
|
612
|
+
has_advantage = 0.0
|
|
613
|
+
|
|
614
|
+
torch_data = {
|
|
615
|
+
'obs': dict_apply(obs_dict, torch.from_numpy),
|
|
616
|
+
'action': torch.from_numpy(data['action'].astype(np.float32)),
|
|
617
|
+
'advantage': torch.tensor(advantage, dtype=torch.float32),
|
|
618
|
+
'has_advantage': torch.tensor(has_advantage, dtype=torch.float32),
|
|
619
|
+
# PA-RL relabel pass writes optimized actions back into a
|
|
620
|
+
# per-frame cache indexed by global anchor frame, so we
|
|
621
|
+
# surface the anchor here (anchor_idx == -1 = unknown,
|
|
622
|
+
# treated as "do not write" by the relabel pass).
|
|
623
|
+
'anchor_idx': torch.tensor(int(anchor_idx), dtype=torch.long),
|
|
624
|
+
}
|
|
625
|
+
|
|
626
|
+
# PA-RL: when an optimized-action cache is loaded, emit per-
|
|
627
|
+
# sample optimized chunks alongside the dataset action. The
|
|
628
|
+
# downstream policy decides whether to swap the DDPM target
|
|
629
|
+
# to ``action_optimized`` (controlled by its own knob).
|
|
630
|
+
# Frames flagged as not-cached (valid=False) emit zeros + a
|
|
631
|
+
# has_=0 gate so the loss falls back to dataset action without
|
|
632
|
+
# ever introducing NaNs into the batch.
|
|
633
|
+
if self.action_optimized is not None:
|
|
634
|
+
ao = self.action_optimized
|
|
635
|
+
av = self.action_optimized_valid
|
|
636
|
+
if 0 <= anchor_idx < ao.shape[0] and bool(av[anchor_idx]):
|
|
637
|
+
# mmap row -> own copy (so DataLoader workers can be
|
|
638
|
+
# forked safely and so tensor-from-numpy doesn't pin
|
|
639
|
+
# the mmap page). Cheap: ~640 B per sample at H=16/A=10.
|
|
640
|
+
a_opt = np.asarray(ao[anchor_idx], dtype=np.float32).copy()
|
|
641
|
+
torch_data['action_optimized'] = torch.from_numpy(a_opt)
|
|
642
|
+
torch_data['has_action_optimized'] = torch.tensor(
|
|
643
|
+
1.0, dtype=torch.float32)
|
|
644
|
+
else:
|
|
645
|
+
torch_data['action_optimized'] = torch.zeros(
|
|
646
|
+
self.action_horizon, self.action_dim,
|
|
647
|
+
dtype=torch.float32,
|
|
648
|
+
)
|
|
649
|
+
torch_data['has_action_optimized'] = torch.tensor(
|
|
650
|
+
0.0, dtype=torch.float32)
|
|
651
|
+
|
|
652
|
+
return torch_data
|
|
File without changes
|
|
@@ -0,0 +1,9 @@
|
|
|
1
|
+
from typing import Dict
|
|
2
|
+
from diffusion_policy.policy.base_image_policy import BaseImagePolicy
|
|
3
|
+
|
|
4
|
+
class BaseImageRunner:
|
|
5
|
+
def __init__(self, output_dir):
|
|
6
|
+
self.output_dir = output_dir
|
|
7
|
+
|
|
8
|
+
def run(self, policy: BaseImagePolicy) -> Dict:
|
|
9
|
+
raise NotImplementedError()
|
|
@@ -0,0 +1,10 @@
|
|
|
1
|
+
from diffusion_policy.policy.base_image_policy import BaseImagePolicy
|
|
2
|
+
from diffusion_policy.env_runner.base_image_runner import BaseImageRunner
|
|
3
|
+
|
|
4
|
+
class RealPushTImageRunner(BaseImageRunner):
|
|
5
|
+
def __init__(self,
|
|
6
|
+
output_dir):
|
|
7
|
+
super().__init__(output_dir)
|
|
8
|
+
|
|
9
|
+
def run(self, policy: BaseImagePolicy):
|
|
10
|
+
return dict()
|
|
File without changes
|
|
File without changes
|