diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_lowdim_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_lowdim_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..601e21cb141ed1ca0f5df106369f6f609a82875c
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_lowdim_dataset.py
@@ -0,0 +1,91 @@
+from typing import Dict
+import torch
+import numpy as np
+import copy
+import pathlib
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import SequenceSampler, get_val_mask
+from diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer
+from diffusion_policy.dataset.base_dataset import BaseLowdimDataset
+
+class KitchenLowdimDataset(BaseLowdimDataset):
+ def __init__(self,
+ dataset_dir,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ seed=42,
+ val_ratio=0.0
+ ):
+ super().__init__()
+
+ data_directory = pathlib.Path(dataset_dir)
+ observations = np.load(data_directory / "observations_seq.npy")
+ actions = np.load(data_directory / "actions_seq.npy")
+ masks = np.load(data_directory / "existence_mask.npy")
+
+ self.replay_buffer = ReplayBuffer.create_empty_numpy()
+ for i in range(len(masks)):
+ eps_len = int(masks[i].sum())
+ obs = observations[i,:eps_len].astype(np.float32)
+ action = actions[i,:eps_len].astype(np.float32)
+ data = {
+ 'obs': obs,
+ 'action': action
+ }
+ self.replay_buffer.add_episode(data)
+
+ val_mask = get_val_mask(
+ n_episodes=self.replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ self.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask)
+
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, mode='limits', **kwargs):
+ data = {
+ 'obs': self.replay_buffer['obs'],
+ 'action': self.replay_buffer['action']
+ }
+ if 'range_eps' not in kwargs:
+ # to prevent blowing up dims that barely change
+ kwargs['range_eps'] = 5e-2
+ normalizer = LinearNormalizer()
+ normalizer.fit(data=data, last_n_dims=1, mode=mode, **kwargs)
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer['action'])
+
+ def __len__(self) -> int:
+ return len(self.sampler)
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ sample = self.sampler.sample_sequence(idx)
+ data = sample
+
+ torch_data = dict_apply(data, torch.from_numpy)
+ return torch_data
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_mjl_lowdim_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_mjl_lowdim_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..e3173818c36e1b4109aeab2a13d3ec2c8e98c391
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/kitchen_mjl_lowdim_dataset.py
@@ -0,0 +1,112 @@
+from typing import Dict
+import torch
+import numpy as np
+import copy
+import pathlib
+from tqdm import tqdm
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import SequenceSampler, get_val_mask
+from diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer
+from diffusion_policy.dataset.base_dataset import BaseLowdimDataset
+from diffusion_policy.env.kitchen.kitchen_util import parse_mjl_logs
+
+class KitchenMjlLowdimDataset(BaseLowdimDataset):
+ def __init__(self,
+ dataset_dir,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ abs_action=True,
+ robot_noise_ratio=0.0,
+ seed=42,
+ val_ratio=0.0
+ ):
+ super().__init__()
+
+ if not abs_action:
+ raise NotImplementedError()
+
+ robot_pos_noise_amp = np.array([0.1 , 0.1 , 0.1 , 0.1 , 0.1 , 0.1 , 0.1 , 0.1 ,
+ 0.1 , 0.005 , 0.005 , 0.0005, 0.0005, 0.0005, 0.0005, 0.0005,
+ 0.0005, 0.005 , 0.005 , 0.005 , 0.1 , 0.1 , 0.1 , 0.005 ,
+ 0.005 , 0.005 , 0.1 , 0.1 , 0.1 , 0.005 ], dtype=np.float32)
+ rng = np.random.default_rng(seed=seed)
+
+ data_directory = pathlib.Path(dataset_dir)
+ self.replay_buffer = ReplayBuffer.create_empty_numpy()
+ for i, mjl_path in enumerate(tqdm(list(data_directory.glob('*/*.mjl')))):
+ try:
+ data = parse_mjl_logs(str(mjl_path.absolute()), skipamount=40)
+ qpos = data['qpos'].astype(np.float32)
+ obs = np.concatenate([
+ qpos[:,:9],
+ qpos[:,-21:],
+ np.zeros((len(qpos),30),dtype=np.float32)
+ ], axis=-1)
+ if robot_noise_ratio > 0:
+ # add observation noise to match real robot
+ noise = robot_noise_ratio * robot_pos_noise_amp * rng.uniform(
+ low=-1., high=1., size=(obs.shape[0], 30))
+ obs[:,:30] += noise
+ episode = {
+ 'obs': obs,
+ 'action': data['ctrl'].astype(np.float32)
+ }
+ self.replay_buffer.add_episode(episode)
+ except Exception as e:
+ print(i, e)
+
+ val_mask = get_val_mask(
+ n_episodes=self.replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ self.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask)
+
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, mode='limits', **kwargs):
+ data = {
+ 'obs': self.replay_buffer['obs'],
+ 'action': self.replay_buffer['action']
+ }
+ if 'range_eps' not in kwargs:
+ # to prevent blowing up dims that barely change
+ kwargs['range_eps'] = 5e-2
+ normalizer = LinearNormalizer()
+ normalizer.fit(data=data, last_n_dims=1, mode=mode, **kwargs)
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer['action'])
+
+ def __len__(self) -> int:
+ return len(self.sampler)
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ sample = self.sampler.sample_sequence(idx)
+ data = sample
+
+ torch_data = dict_apply(data, torch.from_numpy)
+ return torch_data
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/pusht_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/pusht_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..dc3ec1c81310762d8a50027c4bb4f33959287249
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/pusht_dataset.py
@@ -0,0 +1,97 @@
+from typing import Dict
+import torch
+import numpy as np
+import copy
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import (
+ SequenceSampler, get_val_mask, downsample_mask)
+from diffusion_policy.model.common.normalizer import LinearNormalizer
+from diffusion_policy.dataset.base_dataset import BaseLowdimDataset
+
+class PushTLowdimDataset(BaseLowdimDataset):
+ def __init__(self,
+ zarr_path,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ obs_key='keypoint',
+ state_key='state',
+ action_key='action',
+ seed=42,
+ val_ratio=0.0,
+ max_train_episodes=None
+ ):
+ super().__init__()
+ self.replay_buffer = ReplayBuffer.copy_from_path(
+ zarr_path, keys=[obs_key, state_key, action_key])
+
+ val_mask = get_val_mask(
+ n_episodes=self.replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ train_mask = downsample_mask(
+ mask=train_mask,
+ max_n=max_train_episodes,
+ seed=seed)
+
+ self.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask
+ )
+ self.obs_key = obs_key
+ self.state_key = state_key
+ self.action_key = action_key
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, mode='limits', **kwargs):
+ data = self._sample_to_data(self.replay_buffer)
+ normalizer = LinearNormalizer()
+ normalizer.fit(data=data, last_n_dims=1, mode=mode, **kwargs)
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer[self.action_key])
+
+ def __len__(self) -> int:
+ return len(self.sampler)
+
+ def _sample_to_data(self, sample):
+ keypoint = sample[self.obs_key]
+ state = sample[self.state_key]
+ agent_pos = state[:,:2]
+ obs = np.concatenate([
+ keypoint.reshape(keypoint.shape[0], -1),
+ agent_pos], axis=-1)
+
+ data = {
+ 'obs': obs, # T, D_o
+ 'action': sample[self.action_key], # T, D_a
+ }
+ return data
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ sample = self.sampler.sample_sequence(idx)
+ data = self._sample_to_data(sample)
+
+ torch_data = dict_apply(data, torch.from_numpy)
+ return torch_data
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/pusht_image_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/pusht_image_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..f096a8f0f2708e863b0f4d04568e5d2ec53c667b
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/pusht_image_dataset.py
@@ -0,0 +1,102 @@
+from typing import Dict
+import torch
+import numpy as np
+import copy
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import (
+ SequenceSampler, get_val_mask, downsample_mask)
+from diffusion_policy.model.common.normalizer import LinearNormalizer
+from diffusion_policy.dataset.base_dataset import BaseImageDataset
+from diffusion_policy.common.normalize_util import get_image_range_normalizer
+
+class PushTImageDataset(BaseImageDataset):
+ def __init__(self,
+ zarr_path,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ seed=42,
+ val_ratio=0.0,
+ max_train_episodes=None
+ ):
+
+ super().__init__()
+ self.replay_buffer = ReplayBuffer.copy_from_path(
+ zarr_path, keys=['img', 'state', 'action'])
+ val_mask = get_val_mask(
+ n_episodes=self.replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ train_mask = downsample_mask(
+ mask=train_mask,
+ max_n=max_train_episodes,
+ seed=seed)
+
+ self.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask)
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, mode='limits', **kwargs):
+ data = {
+ 'action': self.replay_buffer['action'],
+ 'agent_pos': self.replay_buffer['state'][...,:2]
+ }
+ normalizer = LinearNormalizer()
+ normalizer.fit(data=data, last_n_dims=1, mode=mode, **kwargs)
+ normalizer['image'] = get_image_range_normalizer()
+ return normalizer
+
+ def __len__(self) -> int:
+ return len(self.sampler)
+
+ def _sample_to_data(self, sample):
+ agent_pos = sample['state'][:,:2].astype(np.float32) # (agent_posx2, block_posex3)
+ image = np.moveaxis(sample['img'],-1,1)/255
+
+ data = {
+ 'obs': {
+ 'image': image, # T, 3, 96, 96
+ 'agent_pos': agent_pos, # T, 2
+ },
+ 'action': sample['action'].astype(np.float32) # T, 2
+ }
+ return data
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ sample = self.sampler.sample_sequence(idx)
+ data = self._sample_to_data(sample)
+ torch_data = dict_apply(data, torch.from_numpy)
+ return torch_data
+
+
+def test():
+ import os
+ zarr_path = os.path.expanduser('~/dev/diffusion_policy/data/pusht/pusht_cchi_v7_replay.zarr')
+ dataset = PushTImageDataset(zarr_path, horizon=16)
+
+ # from matplotlib import pyplot as plt
+ # normalizer = dataset.get_normalizer()
+ # nactions = normalizer['action'].normalize(dataset.replay_buffer['action'])
+ # diff = np.diff(nactions, axis=0)
+ # dists = np.linalg.norm(np.diff(nactions, axis=0), axis=-1)
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/real_pusht_image_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/real_pusht_image_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..c5a238f7f3255646e85fcbe8c36f2ad8dc82b3be
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/real_pusht_image_dataset.py
@@ -0,0 +1,291 @@
+from typing import Dict, List
+import torch
+import numpy as np
+import zarr
+import os
+import shutil
+from filelock import FileLock
+from threadpoolctl import threadpool_limits
+from omegaconf import OmegaConf
+import cv2
+import json
+import hashlib
+import copy
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.dataset.base_dataset import BaseImageDataset
+from diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import (
+ SequenceSampler, get_val_mask, downsample_mask)
+from diffusion_policy.real_world.real_data_conversion import real_data_to_replay_buffer
+from diffusion_policy.common.normalize_util import (
+ get_range_normalizer_from_stat,
+ get_image_range_normalizer,
+ get_identity_normalizer_from_stat,
+ array_to_stats
+)
+
+class RealPushTImageDataset(BaseImageDataset):
+ def __init__(self,
+ shape_meta: dict,
+ dataset_path: str,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ n_obs_steps=None,
+ n_latency_steps=0,
+ use_cache=False,
+ seed=42,
+ val_ratio=0.0,
+ max_train_episodes=None,
+ delta_action=False,
+ ):
+ assert os.path.isdir(dataset_path)
+
+ replay_buffer = None
+ if use_cache:
+ # fingerprint shape_meta
+ shape_meta_json = json.dumps(OmegaConf.to_container(shape_meta), sort_keys=True)
+ shape_meta_hash = hashlib.md5(shape_meta_json.encode('utf-8')).hexdigest()
+ cache_zarr_path = os.path.join(dataset_path, shape_meta_hash + '.zarr.zip')
+ cache_lock_path = cache_zarr_path + '.lock'
+ print('Acquiring lock on cache.')
+ with FileLock(cache_lock_path):
+ if not os.path.exists(cache_zarr_path):
+ # cache does not exists
+ try:
+ print('Cache does not exist. Creating!')
+ replay_buffer = _get_replay_buffer(
+ dataset_path=dataset_path,
+ shape_meta=shape_meta,
+ store=zarr.MemoryStore()
+ )
+ print('Saving cache to disk.')
+ with zarr.ZipStore(cache_zarr_path) as zip_store:
+ replay_buffer.save_to_store(
+ store=zip_store
+ )
+ except Exception as e:
+ shutil.rmtree(cache_zarr_path)
+ raise e
+ else:
+ print('Loading cached ReplayBuffer from Disk.')
+ with zarr.ZipStore(cache_zarr_path, mode='r') as zip_store:
+ replay_buffer = ReplayBuffer.copy_from_store(
+ src_store=zip_store, store=zarr.MemoryStore())
+ print('Loaded!')
+ else:
+ replay_buffer = _get_replay_buffer(
+ dataset_path=dataset_path,
+ shape_meta=shape_meta,
+ store=zarr.MemoryStore()
+ )
+
+ if delta_action:
+ # replace action as relative to previous frame
+ actions = replay_buffer['action'][:]
+ # support positions only at this time
+ assert actions.shape[1] <= 3
+ actions_diff = np.zeros_like(actions)
+ episode_ends = replay_buffer.episode_ends[:]
+ for i in range(len(episode_ends)):
+ start = 0
+ if i > 0:
+ start = episode_ends[i-1]
+ end = episode_ends[i]
+ # delta action is the difference between previous desired position and the current
+ # it should be scheduled at the previous timestep for the current timestep
+ # to ensure consistency with positional mode
+ actions_diff[start+1:end] = np.diff(actions[start:end], axis=0)
+ replay_buffer['action'][:] = actions_diff
+
+ rgb_keys = list()
+ lowdim_keys = list()
+ obs_shape_meta = shape_meta['obs']
+ for key, attr in obs_shape_meta.items():
+ type = attr.get('type', 'low_dim')
+ if type == 'rgb':
+ rgb_keys.append(key)
+ elif type == 'low_dim':
+ lowdim_keys.append(key)
+
+ key_first_k = dict()
+ if n_obs_steps is not None:
+ # only take first k obs from images
+ for key in rgb_keys + lowdim_keys:
+ key_first_k[key] = n_obs_steps
+
+ val_mask = get_val_mask(
+ n_episodes=replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ train_mask = downsample_mask(
+ mask=train_mask,
+ max_n=max_train_episodes,
+ seed=seed)
+
+ sampler = SequenceSampler(
+ replay_buffer=replay_buffer,
+ sequence_length=horizon+n_latency_steps,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask,
+ key_first_k=key_first_k)
+
+ self.replay_buffer = replay_buffer
+ self.sampler = sampler
+ self.shape_meta = shape_meta
+ self.rgb_keys = rgb_keys
+ self.lowdim_keys = lowdim_keys
+ self.n_obs_steps = n_obs_steps
+ self.val_mask = val_mask
+ self.horizon = horizon
+ self.n_latency_steps = n_latency_steps
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon+self.n_latency_steps,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=self.val_mask
+ )
+ val_set.val_mask = ~self.val_mask
+ return val_set
+
+ def get_normalizer(self, **kwargs) -> LinearNormalizer:
+ normalizer = LinearNormalizer()
+
+ # action
+ normalizer['action'] = SingleFieldLinearNormalizer.create_fit(
+ self.replay_buffer['action'])
+
+ # obs
+ for key in self.lowdim_keys:
+ normalizer[key] = SingleFieldLinearNormalizer.create_fit(
+ self.replay_buffer[key])
+
+ # image
+ for key in self.rgb_keys:
+ normalizer[key] = get_image_range_normalizer()
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer['action'])
+
+ def __len__(self):
+ return len(self.sampler)
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ threadpool_limits(1)
+ data = self.sampler.sample_sequence(idx)
+
+ # to save RAM, only return first n_obs_steps of OBS
+ # since the rest will be discarded anyway.
+ # when self.n_obs_steps is None
+ # this slice does nothing (takes all)
+ T_slice = slice(self.n_obs_steps)
+
+ obs_dict = dict()
+ for key in self.rgb_keys:
+ # move channel last to channel first
+ # T,H,W,C
+ # convert uint8 image to float32
+ obs_dict[key] = np.moveaxis(data[key][T_slice],-1,1
+ ).astype(np.float32) / 255.
+ # T,C,H,W
+ # save ram
+ del data[key]
+ for key in self.lowdim_keys:
+ obs_dict[key] = data[key][T_slice].astype(np.float32)
+ # save ram
+ del data[key]
+
+ action = data['action'].astype(np.float32)
+ # handle latency by dropping first n_latency_steps action
+ # observations are already taken care of by T_slice
+ if self.n_latency_steps > 0:
+ action = action[self.n_latency_steps:]
+
+ torch_data = {
+ 'obs': dict_apply(obs_dict, torch.from_numpy),
+ 'action': torch.from_numpy(action)
+ }
+ return torch_data
+
+def zarr_resize_index_last_dim(zarr_arr, idxs):
+ actions = zarr_arr[:]
+ actions = actions[...,idxs]
+ zarr_arr.resize(zarr_arr.shape[:-1] + (len(idxs),))
+ zarr_arr[:] = actions
+ return zarr_arr
+
+def _get_replay_buffer(dataset_path, shape_meta, store):
+ # parse shape meta
+ rgb_keys = list()
+ lowdim_keys = list()
+ out_resolutions = dict()
+ lowdim_shapes = dict()
+ obs_shape_meta = shape_meta['obs']
+ for key, attr in obs_shape_meta.items():
+ type = attr.get('type', 'low_dim')
+ shape = tuple(attr.get('shape'))
+ if type == 'rgb':
+ rgb_keys.append(key)
+ c,h,w = shape
+ out_resolutions[key] = (w,h)
+ elif type == 'low_dim':
+ lowdim_keys.append(key)
+ lowdim_shapes[key] = tuple(shape)
+ if 'pose' in key:
+ assert tuple(shape) in [(2,),(6,)]
+
+ action_shape = tuple(shape_meta['action']['shape'])
+ assert action_shape in [(2,),(6,)]
+
+ # load data
+ cv2.setNumThreads(1)
+ with threadpool_limits(1):
+ replay_buffer = real_data_to_replay_buffer(
+ dataset_path=dataset_path,
+ out_store=store,
+ out_resolutions=out_resolutions,
+ lowdim_keys=lowdim_keys + ['action'],
+ image_keys=rgb_keys
+ )
+
+ # transform lowdim dimensions
+ if action_shape == (2,):
+ # 2D action space, only controls X and Y
+ zarr_arr = replay_buffer['action']
+ zarr_resize_index_last_dim(zarr_arr, idxs=[0,1])
+
+ for key, shape in lowdim_shapes.items():
+ if 'pose' in key and shape == (2,):
+ # only take X and Y
+ zarr_arr = replay_buffer[key]
+ zarr_resize_index_last_dim(zarr_arr, idxs=[0,1])
+
+ return replay_buffer
+
+
+def test():
+ import hydra
+ from omegaconf import OmegaConf
+ OmegaConf.register_new_resolver("eval", eval, replace=True)
+
+ with hydra.initialize('../diffusion_policy/config'):
+ cfg = hydra.compose('train_robomimic_real_image_workspace')
+ OmegaConf.resolve(cfg)
+ dataset = hydra.utils.instantiate(cfg.task.dataset)
+
+ from matplotlib import pyplot as plt
+ normalizer = dataset.get_normalizer()
+ nactions = normalizer['action'].normalize(dataset.replay_buffer['action'][:])
+ diff = np.diff(nactions, axis=0)
+ dists = np.linalg.norm(np.diff(nactions, axis=0), axis=-1)
+ _ = plt.hist(dists, bins=100); plt.title('real action velocity')
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_image_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_image_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..2728e9e9ee0580cb25ee3aedaca5f22770aa76b6
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_image_dataset.py
@@ -0,0 +1,373 @@
+from typing import Dict, List
+import torch
+import numpy as np
+import h5py
+from tqdm import tqdm
+import zarr
+import os
+import shutil
+import copy
+import json
+import hashlib
+from filelock import FileLock
+from threadpoolctl import threadpool_limits
+import concurrent.futures
+import multiprocessing
+from omegaconf import OmegaConf
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.dataset.base_dataset import BaseImageDataset, LinearNormalizer
+from diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer
+from diffusion_policy.model.common.rotation_transformer import RotationTransformer
+from diffusion_policy.codecs.imagecodecs_numcodecs import register_codecs, Jpeg2k
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import SequenceSampler, get_val_mask
+from diffusion_policy.common.normalize_util import (
+ robomimic_abs_action_only_normalizer_from_stat,
+ robomimic_abs_action_only_dual_arm_normalizer_from_stat,
+ get_range_normalizer_from_stat,
+ get_image_range_normalizer,
+ get_identity_normalizer_from_stat,
+ array_to_stats
+)
+register_codecs()
+
+class RobomimicReplayImageDataset(BaseImageDataset):
+ def __init__(self,
+ shape_meta: dict,
+ dataset_path: str,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ n_obs_steps=None,
+ abs_action=False,
+ rotation_rep='rotation_6d', # ignored when abs_action=False
+ use_legacy_normalizer=False,
+ use_cache=False,
+ seed=42,
+ val_ratio=0.0
+ ):
+ rotation_transformer = RotationTransformer(
+ from_rep='axis_angle', to_rep=rotation_rep)
+
+ replay_buffer = None
+ if use_cache:
+ cache_zarr_path = dataset_path + '.zarr.zip'
+ cache_lock_path = cache_zarr_path + '.lock'
+ print('Acquiring lock on cache.')
+ with FileLock(cache_lock_path):
+ if not os.path.exists(cache_zarr_path):
+ # cache does not exists
+ try:
+ print('Cache does not exist. Creating!')
+ # store = zarr.DirectoryStore(cache_zarr_path)
+ replay_buffer = _convert_robomimic_to_replay(
+ store=zarr.MemoryStore(),
+ shape_meta=shape_meta,
+ dataset_path=dataset_path,
+ abs_action=abs_action,
+ rotation_transformer=rotation_transformer)
+ print('Saving cache to disk.')
+ with zarr.ZipStore(cache_zarr_path) as zip_store:
+ replay_buffer.save_to_store(
+ store=zip_store
+ )
+ except Exception as e:
+ shutil.rmtree(cache_zarr_path)
+ raise e
+ else:
+ print('Loading cached ReplayBuffer from Disk.')
+ with zarr.ZipStore(cache_zarr_path, mode='r') as zip_store:
+ replay_buffer = ReplayBuffer.copy_from_store(
+ src_store=zip_store, store=zarr.MemoryStore())
+ print('Loaded!')
+ else:
+ replay_buffer = _convert_robomimic_to_replay(
+ store=zarr.MemoryStore(),
+ shape_meta=shape_meta,
+ dataset_path=dataset_path,
+ abs_action=abs_action,
+ rotation_transformer=rotation_transformer)
+
+ rgb_keys = list()
+ lowdim_keys = list()
+ obs_shape_meta = shape_meta['obs']
+ for key, attr in obs_shape_meta.items():
+ type = attr.get('type', 'low_dim')
+ if type == 'rgb':
+ rgb_keys.append(key)
+ elif type == 'low_dim':
+ lowdim_keys.append(key)
+
+ # for key in rgb_keys:
+ # replay_buffer[key].compressor.numthreads=1
+
+ key_first_k = dict()
+ if n_obs_steps is not None:
+ # only take first k obs from images
+ for key in rgb_keys + lowdim_keys:
+ key_first_k[key] = n_obs_steps
+
+ val_mask = get_val_mask(
+ n_episodes=replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ sampler = SequenceSampler(
+ replay_buffer=replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask,
+ key_first_k=key_first_k)
+
+ self.replay_buffer = replay_buffer
+ self.sampler = sampler
+ self.shape_meta = shape_meta
+ self.rgb_keys = rgb_keys
+ self.lowdim_keys = lowdim_keys
+ self.abs_action = abs_action
+ self.n_obs_steps = n_obs_steps
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+ self.use_legacy_normalizer = use_legacy_normalizer
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, **kwargs) -> LinearNormalizer:
+ normalizer = LinearNormalizer()
+
+ # action
+ stat = array_to_stats(self.replay_buffer['action'])
+ if self.abs_action:
+ if stat['mean'].shape[-1] > 10:
+ # dual arm
+ this_normalizer = robomimic_abs_action_only_dual_arm_normalizer_from_stat(stat)
+ else:
+ this_normalizer = robomimic_abs_action_only_normalizer_from_stat(stat)
+
+ if self.use_legacy_normalizer:
+ this_normalizer = normalizer_from_stat(stat)
+ else:
+ # already normalized
+ this_normalizer = get_identity_normalizer_from_stat(stat)
+ normalizer['action'] = this_normalizer
+
+ # obs
+ for key in self.lowdim_keys:
+ stat = array_to_stats(self.replay_buffer[key])
+
+ if key.endswith('pos'):
+ this_normalizer = get_range_normalizer_from_stat(stat)
+ elif key.endswith('quat'):
+ # quaternion is in [-1,1] already
+ this_normalizer = get_identity_normalizer_from_stat(stat)
+ elif key.endswith('qpos'):
+ this_normalizer = get_range_normalizer_from_stat(stat)
+ else:
+ raise RuntimeError('unsupported')
+ normalizer[key] = this_normalizer
+
+ # image
+ for key in self.rgb_keys:
+ normalizer[key] = get_image_range_normalizer()
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer['action'])
+
+ def __len__(self):
+ return len(self.sampler)
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ threadpool_limits(1)
+ data = self.sampler.sample_sequence(idx)
+
+ # to save RAM, only return first n_obs_steps of OBS
+ # since the rest will be discarded anyway.
+ # when self.n_obs_steps is None
+ # this slice does nothing (takes all)
+ T_slice = slice(self.n_obs_steps)
+
+ obs_dict = dict()
+ for key in self.rgb_keys:
+ # move channel last to channel first
+ # T,H,W,C
+ # convert uint8 image to float32
+ obs_dict[key] = np.moveaxis(data[key][T_slice],-1,1
+ ).astype(np.float32) / 255.
+ # T,C,H,W
+ del data[key]
+ for key in self.lowdim_keys:
+ obs_dict[key] = data[key][T_slice].astype(np.float32)
+ del data[key]
+
+ torch_data = {
+ 'obs': dict_apply(obs_dict, torch.from_numpy),
+ 'action': torch.from_numpy(data['action'].astype(np.float32))
+ }
+ return torch_data
+
+
+def _convert_actions(raw_actions, abs_action, rotation_transformer):
+ actions = raw_actions
+ if abs_action:
+ is_dual_arm = False
+ if raw_actions.shape[-1] == 14:
+ # dual arm
+ raw_actions = raw_actions.reshape(-1,2,7)
+ is_dual_arm = True
+
+ pos = raw_actions[...,:3]
+ rot = raw_actions[...,3:6]
+ gripper = raw_actions[...,6:]
+ rot = rotation_transformer.forward(rot)
+ raw_actions = np.concatenate([
+ pos, rot, gripper
+ ], axis=-1).astype(np.float32)
+
+ if is_dual_arm:
+ raw_actions = raw_actions.reshape(-1,20)
+ actions = raw_actions
+ return actions
+
+
+def _convert_robomimic_to_replay(store, shape_meta, dataset_path, abs_action, rotation_transformer,
+ n_workers=None, max_inflight_tasks=None):
+ if n_workers is None:
+ n_workers = multiprocessing.cpu_count()
+ if max_inflight_tasks is None:
+ max_inflight_tasks = n_workers * 5
+
+ # parse shape_meta
+ rgb_keys = list()
+ lowdim_keys = list()
+ # construct compressors and chunks
+ obs_shape_meta = shape_meta['obs']
+ for key, attr in obs_shape_meta.items():
+ shape = attr['shape']
+ type = attr.get('type', 'low_dim')
+ if type == 'rgb':
+ rgb_keys.append(key)
+ elif type == 'low_dim':
+ lowdim_keys.append(key)
+
+ root = zarr.group(store)
+ data_group = root.require_group('data', overwrite=True)
+ meta_group = root.require_group('meta', overwrite=True)
+
+ with h5py.File(dataset_path) as file:
+ # count total steps
+ demos = file['data']
+ episode_ends = list()
+ prev_end = 0
+ for i in range(len(demos)):
+ demo = demos[f'demo_{i}']
+ episode_length = demo['actions'].shape[0]
+ episode_end = prev_end + episode_length
+ prev_end = episode_end
+ episode_ends.append(episode_end)
+ n_steps = episode_ends[-1]
+ episode_starts = [0] + episode_ends[:-1]
+ _ = meta_group.array('episode_ends', episode_ends,
+ dtype=np.int64, compressor=None, overwrite=True)
+
+ # save lowdim data
+ for key in tqdm(lowdim_keys + ['action'], desc="Loading lowdim data"):
+ data_key = 'obs/' + key
+ if key == 'action':
+ data_key = 'actions'
+ this_data = list()
+ for i in range(len(demos)):
+ demo = demos[f'demo_{i}']
+ this_data.append(demo[data_key][:].astype(np.float32))
+ this_data = np.concatenate(this_data, axis=0)
+ if key == 'action':
+ this_data = _convert_actions(
+ raw_actions=this_data,
+ abs_action=abs_action,
+ rotation_transformer=rotation_transformer
+ )
+ assert this_data.shape == (n_steps,) + tuple(shape_meta['action']['shape'])
+ else:
+ assert this_data.shape == (n_steps,) + tuple(shape_meta['obs'][key]['shape'])
+ _ = data_group.array(
+ name=key,
+ data=this_data,
+ shape=this_data.shape,
+ chunks=this_data.shape,
+ compressor=None,
+ dtype=this_data.dtype
+ )
+
+ def img_copy(zarr_arr, zarr_idx, hdf5_arr, hdf5_idx):
+ try:
+ zarr_arr[zarr_idx] = hdf5_arr[hdf5_idx]
+ # make sure we can successfully decode
+ _ = zarr_arr[zarr_idx]
+ return True
+ except Exception as e:
+ return False
+
+ with tqdm(total=n_steps*len(rgb_keys), desc="Loading image data", mininterval=1.0) as pbar:
+ # one chunk per thread, therefore no synchronization needed
+ with concurrent.futures.ThreadPoolExecutor(max_workers=n_workers) as executor:
+ futures = set()
+ for key in rgb_keys:
+ data_key = 'obs/' + key
+ shape = tuple(shape_meta['obs'][key]['shape'])
+ c,h,w = shape
+ this_compressor = Jpeg2k(level=50)
+ img_arr = data_group.require_dataset(
+ name=key,
+ shape=(n_steps,h,w,c),
+ chunks=(1,h,w,c),
+ compressor=this_compressor,
+ dtype=np.uint8
+ )
+ for episode_idx in range(len(demos)):
+ demo = demos[f'demo_{episode_idx}']
+ hdf5_arr = demo['obs'][key]
+ for hdf5_idx in range(hdf5_arr.shape[0]):
+ if len(futures) >= max_inflight_tasks:
+ # limit number of inflight tasks
+ completed, futures = concurrent.futures.wait(futures,
+ return_when=concurrent.futures.FIRST_COMPLETED)
+ for f in completed:
+ if not f.result():
+ raise RuntimeError('Failed to encode image!')
+ pbar.update(len(completed))
+
+ zarr_idx = episode_starts[episode_idx] + hdf5_idx
+ futures.add(
+ executor.submit(img_copy,
+ img_arr, zarr_idx, hdf5_arr, hdf5_idx))
+ completed, futures = concurrent.futures.wait(futures)
+ for f in completed:
+ if not f.result():
+ raise RuntimeError('Failed to encode image!')
+ pbar.update(len(completed))
+
+ replay_buffer = ReplayBuffer(root)
+ return replay_buffer
+
+def normalizer_from_stat(stat):
+ max_abs = np.maximum(stat['max'].max(), np.abs(stat['min']).max())
+ scale = np.full_like(stat['max'], fill_value=1/max_abs)
+ offset = np.zeros_like(stat['max'])
+ return SingleFieldLinearNormalizer.create_manual(
+ scale=scale,
+ offset=offset,
+ input_stats_dict=stat
+ )
diff --git a/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_lowdim_dataset.py b/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_lowdim_dataset.py
new file mode 100644
index 0000000000000000000000000000000000000000..c642da9f26d63212d693821fec696c9894764ef4
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/dataset/robomimic_replay_lowdim_dataset.py
@@ -0,0 +1,168 @@
+from typing import Dict, List
+import torch
+import numpy as np
+import h5py
+from tqdm import tqdm
+import copy
+from diffusion_policy.common.pytorch_util import dict_apply
+from diffusion_policy.dataset.base_dataset import BaseLowdimDataset, LinearNormalizer
+from diffusion_policy.model.common.normalizer import LinearNormalizer, SingleFieldLinearNormalizer
+from diffusion_policy.model.common.rotation_transformer import RotationTransformer
+from diffusion_policy.common.replay_buffer import ReplayBuffer
+from diffusion_policy.common.sampler import (
+ SequenceSampler, get_val_mask, downsample_mask)
+from diffusion_policy.common.normalize_util import (
+ robomimic_abs_action_only_normalizer_from_stat,
+ robomimic_abs_action_only_dual_arm_normalizer_from_stat,
+ get_identity_normalizer_from_stat,
+ array_to_stats
+)
+
+class RobomimicReplayLowdimDataset(BaseLowdimDataset):
+ def __init__(self,
+ dataset_path: str,
+ horizon=1,
+ pad_before=0,
+ pad_after=0,
+ obs_keys: List[str]=[
+ 'object',
+ 'robot0_eef_pos',
+ 'robot0_eef_quat',
+ 'robot0_gripper_qpos'],
+ abs_action=False,
+ rotation_rep='rotation_6d',
+ use_legacy_normalizer=False,
+ seed=42,
+ val_ratio=0.0,
+ max_train_episodes=None
+ ):
+ obs_keys = list(obs_keys)
+ rotation_transformer = RotationTransformer(
+ from_rep='axis_angle', to_rep=rotation_rep)
+
+ replay_buffer = ReplayBuffer.create_empty_numpy()
+ with h5py.File(dataset_path) as file:
+ demos = file['data']
+ for i in tqdm(range(len(demos)), desc="Loading hdf5 to ReplayBuffer"):
+ demo = demos[f'demo_{i}']
+ episode = _data_to_obs(
+ raw_obs=demo['obs'],
+ raw_actions=demo['actions'][:].astype(np.float32),
+ obs_keys=obs_keys,
+ abs_action=abs_action,
+ rotation_transformer=rotation_transformer)
+ replay_buffer.add_episode(episode)
+
+ val_mask = get_val_mask(
+ n_episodes=replay_buffer.n_episodes,
+ val_ratio=val_ratio,
+ seed=seed)
+ train_mask = ~val_mask
+ train_mask = downsample_mask(
+ mask=train_mask,
+ max_n=max_train_episodes,
+ seed=seed)
+
+ sampler = SequenceSampler(
+ replay_buffer=replay_buffer,
+ sequence_length=horizon,
+ pad_before=pad_before,
+ pad_after=pad_after,
+ episode_mask=train_mask)
+
+ self.replay_buffer = replay_buffer
+ self.sampler = sampler
+ self.abs_action = abs_action
+ self.train_mask = train_mask
+ self.horizon = horizon
+ self.pad_before = pad_before
+ self.pad_after = pad_after
+ self.use_legacy_normalizer = use_legacy_normalizer
+
+ def get_validation_dataset(self):
+ val_set = copy.copy(self)
+ val_set.sampler = SequenceSampler(
+ replay_buffer=self.replay_buffer,
+ sequence_length=self.horizon,
+ pad_before=self.pad_before,
+ pad_after=self.pad_after,
+ episode_mask=~self.train_mask
+ )
+ val_set.train_mask = ~self.train_mask
+ return val_set
+
+ def get_normalizer(self, **kwargs) -> LinearNormalizer:
+ normalizer = LinearNormalizer()
+
+ # action
+ stat = array_to_stats(self.replay_buffer['action'])
+ if self.abs_action:
+ if stat['mean'].shape[-1] > 10:
+ # dual arm
+ this_normalizer = robomimic_abs_action_only_dual_arm_normalizer_from_stat(stat)
+ else:
+ this_normalizer = robomimic_abs_action_only_normalizer_from_stat(stat)
+
+ if self.use_legacy_normalizer:
+ this_normalizer = normalizer_from_stat(stat)
+ else:
+ # already normalized
+ this_normalizer = get_identity_normalizer_from_stat(stat)
+ normalizer['action'] = this_normalizer
+
+ # aggregate obs stats
+ obs_stat = array_to_stats(self.replay_buffer['obs'])
+
+
+ normalizer['obs'] = normalizer_from_stat(obs_stat)
+ return normalizer
+
+ def get_all_actions(self) -> torch.Tensor:
+ return torch.from_numpy(self.replay_buffer['action'])
+
+ def __len__(self):
+ return len(self.sampler)
+
+ def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
+ data = self.sampler.sample_sequence(idx)
+ torch_data = dict_apply(data, torch.from_numpy)
+ return torch_data
+
+def normalizer_from_stat(stat):
+ max_abs = np.maximum(stat['max'].max(), np.abs(stat['min']).max())
+ scale = np.full_like(stat['max'], fill_value=1/max_abs)
+ offset = np.zeros_like(stat['max'])
+ return SingleFieldLinearNormalizer.create_manual(
+ scale=scale,
+ offset=offset,
+ input_stats_dict=stat
+ )
+
+def _data_to_obs(raw_obs, raw_actions, obs_keys, abs_action, rotation_transformer):
+ obs = np.concatenate([
+ raw_obs[key] for key in obs_keys
+ ], axis=-1).astype(np.float32)
+
+ if abs_action:
+ is_dual_arm = False
+ if raw_actions.shape[-1] == 14:
+ # dual arm
+ raw_actions = raw_actions.reshape(-1,2,7)
+ is_dual_arm = True
+
+ pos = raw_actions[...,:3]
+ rot = raw_actions[...,3:6]
+ gripper = raw_actions[...,6:]
+ rot = rotation_transformer.forward(rot)
+ raw_actions = np.concatenate([
+ pos, rot, gripper
+ ], axis=-1).astype(np.float32)
+
+ if is_dual_arm:
+ raw_actions = raw_actions.reshape(-1,20)
+
+ data = {
+ 'obs': obs,
+ 'action': raw_actions
+ }
+ return data
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..10b44ddc943a9f9f83a890392042cf8a62b9ccfa
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block.urdf
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block2.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block2.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..36c081bbfa1d03c124e33bbef0f5139514e3f224
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block2.urdf
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/blue_cube.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/blue_cube.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..089e8041640d77c4043f089367887f7295a9d185
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/blue_cube.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/cube.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/cube.obj
new file mode 100644
index 0000000000000000000000000000000000000000..bc1d0078fc4fdc693bba07c129ad66585e13b1d8
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/cube.obj
@@ -0,0 +1,446 @@
+# Blender v2.92.0 OBJ File: ''
+# www.blender.org
+mtllib cube.mtl
+o square_blue_block_Cube.001
+v 0.000000 0.000000 -0.000000
+v 0.000000 0.038100 -0.000000
+v -0.014865 0.000000 0.018106
+v -0.016277 0.002032 0.019826
+v -0.015863 0.000595 0.019322
+v -0.019826 0.002032 0.016277
+v -0.018106 0.000000 0.014865
+v -0.019322 0.000595 0.015863
+v -0.018052 0.002032 0.019351
+v -0.016494 0.000000 0.017681
+v -0.017595 0.000595 0.018862
+v -0.019351 0.002032 0.018052
+v -0.017681 0.000000 0.016494
+v -0.018862 0.000595 0.017595
+v -0.018106 0.038100 0.014865
+v -0.019826 0.036068 0.016277
+v -0.019322 0.037505 0.015863
+v -0.016277 0.036068 0.019826
+v -0.014865 0.038100 0.018106
+v -0.015863 0.037505 0.019322
+v -0.017681 0.038100 0.016494
+v -0.019351 0.036068 0.018052
+v -0.018862 0.037505 0.017595
+v -0.016494 0.038100 0.017681
+v -0.018052 0.036068 0.019351
+v -0.017595 0.037505 0.018862
+v -0.018106 0.000000 -0.014865
+v -0.019826 0.002032 -0.016277
+v -0.019322 0.000595 -0.015863
+v -0.016277 0.002032 -0.019826
+v -0.014865 0.000000 -0.018106
+v -0.015863 0.000595 -0.019322
+v -0.017681 0.000000 -0.016494
+v -0.019351 0.002032 -0.018052
+v -0.018862 0.000595 -0.017595
+v -0.016494 0.000000 -0.017681
+v -0.018052 0.002032 -0.019351
+v -0.017595 0.000595 -0.018862
+v -0.014865 0.038100 -0.018106
+v -0.016277 0.036068 -0.019826
+v -0.015863 0.037505 -0.019322
+v -0.019826 0.036068 -0.016277
+v -0.018106 0.038100 -0.014865
+v -0.019322 0.037505 -0.015863
+v -0.018052 0.036068 -0.019351
+v -0.016494 0.038100 -0.017681
+v -0.017595 0.037505 -0.018862
+v -0.019351 0.036068 -0.018052
+v -0.017681 0.038100 -0.016494
+v -0.018862 0.037505 -0.017595
+v 0.018106 0.000000 0.014865
+v 0.019826 0.002032 0.016277
+v 0.019322 0.000595 0.015863
+v 0.016277 0.002032 0.019826
+v 0.014865 0.000000 0.018106
+v 0.015863 0.000595 0.019322
+v 0.017681 0.000000 0.016494
+v 0.019351 0.002032 0.018052
+v 0.018862 0.000595 0.017595
+v 0.016494 0.000000 0.017681
+v 0.018052 0.002032 0.019351
+v 0.017595 0.000595 0.018862
+v 0.014865 0.038100 0.018106
+v 0.016277 0.036068 0.019826
+v 0.015863 0.037505 0.019322
+v 0.019826 0.036068 0.016277
+v 0.018106 0.038100 0.014865
+v 0.019322 0.037505 0.015863
+v 0.018052 0.036068 0.019351
+v 0.016494 0.038100 0.017681
+v 0.017595 0.037505 0.018862
+v 0.019351 0.036068 0.018052
+v 0.017681 0.038100 0.016494
+v 0.018862 0.037505 0.017595
+v 0.014865 0.000000 -0.018106
+v 0.016277 0.002032 -0.019826
+v 0.015863 0.000595 -0.019322
+v 0.019826 0.002032 -0.016277
+v 0.018106 0.000000 -0.014865
+v 0.019322 0.000595 -0.015863
+v 0.016494 0.000000 -0.017681
+v 0.018052 0.002032 -0.019351
+v 0.017595 0.000595 -0.018862
+v 0.017681 0.000000 -0.016494
+v 0.019351 0.002032 -0.018052
+v 0.018862 0.000595 -0.017595
+v 0.018106 0.038100 -0.014865
+v 0.019826 0.036068 -0.016277
+v 0.019322 0.037505 -0.015863
+v 0.016277 0.036068 -0.019826
+v 0.014865 0.038100 -0.018106
+v 0.015863 0.037505 -0.019322
+v 0.019351 0.036068 -0.018052
+v 0.017681 0.038100 -0.016494
+v 0.018862 0.037505 -0.017595
+v 0.018052 0.036068 -0.019351
+v 0.016494 0.038100 -0.017681
+v 0.017595 0.037505 -0.018862
+vt 0.811987 0.285513
+vt 0.811986 0.268118
+vt 0.822561 0.276819
+vt 0.831255 0.287393
+vt 0.813860 0.287394
+vt 0.836073 0.319870
+vt 0.856069 0.319870
+vt 0.856069 0.320950
+vt 0.836073 0.320950
+vt 0.813867 0.266245
+vt 0.831262 0.266244
+vt 0.832202 0.266496
+vt 0.832884 0.267184
+vt 0.856069 0.297507
+vt 0.836074 0.297507
+vt 0.836074 0.278381
+vt 0.856070 0.278381
+vt 0.812238 0.267178
+vt 0.812926 0.266496
+vt 0.856070 0.275143
+vt 0.836074 0.275143
+vt 0.836074 0.256018
+vt 0.856070 0.256018
+vt 0.833135 0.268125
+vt 0.833136 0.285520
+vt 0.833136 0.260613
+vt 0.832884 0.261553
+vt 0.822561 0.251912
+vt 0.836074 0.342234
+vt 0.856070 0.342234
+vt 0.856070 0.343313
+vt 0.836074 0.343313
+vt 0.832196 0.287142
+vt 0.831262 0.241337
+vt 0.832202 0.241589
+vt 0.832884 0.286460
+vt 0.812238 0.286454
+vt 0.811986 0.243211
+vt 0.812238 0.242271
+vt 0.836073 0.300745
+vt 0.856069 0.300745
+vt 0.836073 0.298586
+vt 0.856069 0.298586
+vt 0.856069 0.299665
+vt 0.836073 0.299665
+vt 0.856070 0.276222
+vt 0.836074 0.276222
+vt 0.836074 0.277302
+vt 0.856070 0.277302
+vt 0.836074 0.253859
+vt 0.856070 0.253859
+vt 0.856070 0.254938
+vt 0.836074 0.254938
+vt 0.812920 0.287142
+vt 0.856069 0.322029
+vt 0.836073 0.322029
+vt 0.813860 0.262487
+vt 0.812920 0.262235
+vt 0.836073 0.323108
+vt 0.856069 0.323108
+vt 0.832884 0.242277
+vt 0.833135 0.243218
+vt 0.812238 0.261547
+vt 0.811987 0.260606
+vt 0.813868 0.241338
+vt 0.812926 0.241589
+vt 0.831255 0.262486
+vt 0.832196 0.262235
+vt 0.811196 0.267507
+vt 0.811506 0.266500
+vt 0.810258 0.267085
+vt 0.810726 0.265896
+vt 0.811523 0.262225
+vt 0.811189 0.261224
+vt 0.810732 0.262868
+vt 0.810258 0.261658
+vt 0.812248 0.265781
+vt 0.811605 0.264990
+vt 0.812242 0.262967
+vt 0.811638 0.263747
+vt 0.813249 0.265447
+vt 0.812815 0.264516
+vt 0.813249 0.263277
+vt 0.812827 0.264215
+vt 0.833926 0.261224
+vt 0.833616 0.262231
+vt 0.834864 0.261646
+vt 0.834396 0.262835
+vt 0.833599 0.266506
+vt 0.833933 0.267507
+vt 0.834390 0.265863
+vt 0.834864 0.267073
+vt 0.832874 0.262950
+vt 0.833517 0.263741
+vt 0.832880 0.265764
+vt 0.833484 0.264984
+vt 0.831873 0.263284
+vt 0.832307 0.264215
+vt 0.831873 0.265453
+vt 0.832295 0.264516
+vt 0.831873 0.240547
+vt 0.832880 0.240857
+vt 0.832295 0.239609
+vt 0.833484 0.240077
+vt 0.832874 0.287857
+vt 0.831873 0.288191
+vt 0.833517 0.288648
+vt 0.832307 0.289122
+vt 0.833599 0.241599
+vt 0.834390 0.240956
+vt 0.833616 0.287138
+vt 0.834396 0.287742
+vt 0.833933 0.242600
+vt 0.834864 0.242166
+vt 0.833926 0.286131
+vt 0.834864 0.286553
+vt 0.811196 0.242600
+vt 0.811506 0.241593
+vt 0.810258 0.242178
+vt 0.810726 0.240989
+vt 0.811523 0.287132
+vt 0.811189 0.286131
+vt 0.810732 0.287775
+vt 0.810258 0.286565
+vt 0.812248 0.240874
+vt 0.811605 0.240083
+vt 0.812242 0.287874
+vt 0.811638 0.288654
+vt 0.813249 0.240540
+vt 0.812815 0.239609
+vt 0.813249 0.288184
+vt 0.812827 0.289122
+vn 0.0000 -1.0000 0.0000
+vn 0.9739 0.1816 -0.1363
+vn 0.9739 -0.1816 -0.1363
+vn 0.8444 -0.2052 -0.4948
+vn 0.8444 0.2052 -0.4948
+vn 0.1363 -0.1816 0.9739
+vn 0.1363 0.1816 0.9739
+vn -0.1363 0.1816 0.9739
+vn -0.1363 -0.1816 0.9739
+vn -0.9739 -0.1816 0.1362
+vn -0.9739 0.1816 0.1362
+vn -0.9739 0.1816 -0.1362
+vn -0.9739 -0.1816 -0.1362
+vn -0.0000 1.0000 -0.0000
+vn -0.1363 0.1816 -0.9739
+vn -0.1363 -0.1816 -0.9739
+vn -0.4948 -0.2052 -0.8444
+vn -0.4948 0.2052 -0.8444
+vn 0.9739 0.1816 0.1363
+vn 0.9739 -0.1816 0.1363
+vn 0.4948 0.2052 0.8444
+vn 0.4948 -0.2052 0.8444
+vn 0.8444 -0.2052 0.4948
+vn 0.8444 0.2052 0.4948
+vn -0.8444 -0.2052 0.4948
+vn -0.8444 0.2052 0.4948
+vn -0.4948 0.2052 0.8444
+vn -0.4948 -0.2052 0.8444
+vn -0.8444 -0.2052 -0.4948
+vn -0.8444 0.2052 -0.4948
+vn 0.4948 -0.2052 -0.8444
+vn 0.4948 0.2052 -0.8444
+vn 0.1363 0.1816 -0.9739
+vn 0.1363 -0.1816 -0.9739
+vn -0.0965 -0.9775 0.1874
+vn -0.0187 -0.9731 0.2298
+vn -0.0935 -0.6743 0.7325
+vn -0.3529 -0.6994 0.6215
+vn -0.0187 0.9731 0.2298
+vn -0.0965 0.9775 0.1874
+vn -0.3529 0.6994 0.6215
+vn -0.0935 0.6743 0.7325
+vn -0.1874 -0.9775 0.0965
+vn -0.6215 -0.6994 0.3529
+vn -0.1874 0.9775 0.0965
+vn -0.6215 0.6994 0.3529
+vn -0.2298 -0.9731 0.0187
+vn -0.7325 -0.6743 0.0935
+vn -0.2298 0.9731 0.0187
+vn -0.7325 0.6743 0.0935
+vn -0.0965 0.9775 -0.1874
+vn -0.0187 0.9731 -0.2298
+vn -0.0935 0.6743 -0.7325
+vn -0.3529 0.6994 -0.6215
+vn -0.0187 -0.9731 -0.2298
+vn -0.0965 -0.9775 -0.1874
+vn -0.3529 -0.6994 -0.6215
+vn -0.0935 -0.6743 -0.7325
+vn -0.1874 0.9775 -0.0965
+vn -0.6215 0.6994 -0.3529
+vn -0.1874 -0.9775 -0.0965
+vn -0.6215 -0.6994 -0.3529
+vn -0.2298 0.9731 -0.0187
+vn -0.7325 0.6743 -0.0935
+vn -0.2298 -0.9731 -0.0187
+vn -0.7325 -0.6743 -0.0935
+vn 0.1874 0.9775 -0.0965
+vn 0.2298 0.9731 -0.0187
+vn 0.7325 0.6743 -0.0935
+vn 0.6215 0.6994 -0.3529
+vn 0.2298 -0.9731 -0.0187
+vn 0.1874 -0.9775 -0.0965
+vn 0.6215 -0.6994 -0.3529
+vn 0.7325 -0.6743 -0.0935
+vn 0.0965 0.9775 -0.1874
+vn 0.3529 0.6994 -0.6215
+vn 0.0965 -0.9775 -0.1874
+vn 0.3529 -0.6994 -0.6215
+vn 0.0187 0.9731 -0.2298
+vn 0.0935 0.6743 -0.7325
+vn 0.0187 -0.9731 -0.2298
+vn 0.0935 -0.6743 -0.7325
+vn 0.0965 0.9775 0.1874
+vn 0.0187 0.9731 0.2298
+vn 0.0935 0.6743 0.7325
+vn 0.3529 0.6994 0.6215
+vn 0.0187 -0.9731 0.2298
+vn 0.0965 -0.9775 0.1874
+vn 0.3529 -0.6994 0.6215
+vn 0.0935 -0.6743 0.7325
+vn 0.1874 0.9775 0.0965
+vn 0.6215 0.6994 0.3529
+vn 0.1874 -0.9775 0.0965
+vn 0.6215 -0.6994 0.3529
+vn 0.2298 0.9731 0.0187
+vn 0.7325 0.6743 0.0935
+vn 0.2298 -0.9731 0.0187
+vn 0.7325 -0.6743 0.0935
+usemtl toybox.001
+s 1
+f 55/1/1 3/2/1 1/3/1
+f 79/4/1 51/5/1 1/3/1
+f 88/6/2 78/7/3 85/8/4 93/9/5
+f 7/10/1 27/11/1 1/3/1
+f 33/12/1 36/13/1 1/3/1
+f 54/14/6 64/15/7 18/16/8 4/17/9
+f 10/18/1 13/19/1 1/3/1
+f 6/20/10 16/21/11 42/22/12 28/23/13
+f 31/24/1 75/25/1 1/3/1
+f 39/26/14 46/27/14 2/28/14
+f 40/29/15 30/30/16 37/31/17 45/32/18
+f 84/33/1 79/4/1 1/3/1
+f 87/34/14 94/35/14 2/28/14
+f 75/25/1 81/36/1 1/3/1
+f 60/37/1 55/1/1 1/3/1
+f 63/38/14 70/39/14 2/28/14
+f 78/7/3 88/6/2 66/40/19 52/41/20
+f 69/42/21 61/43/22 58/44/23 72/45/24
+f 13/19/1 7/10/1 1/3/1
+f 12/46/25 22/47/26 16/21/11 6/20/10
+f 81/36/1 84/33/1 1/3/1
+f 4/17/9 18/16/8 25/48/27 9/49/28
+f 45/50/18 37/51/17 34/52/29 48/53/30
+f 72/45/24 58/44/23 52/41/20 66/40/19
+f 57/54/1 60/37/1 1/3/1
+f 93/9/5 85/8/4 82/55/31 96/56/32
+f 9/49/28 25/48/27 22/47/26 12/46/25
+f 48/53/30 34/52/29 28/23/13 42/22/12
+f 36/13/1 31/24/1 1/3/1
+f 15/57/14 21/58/14 2/28/14
+f 30/30/16 40/29/15 90/59/33 76/60/34
+f 64/15/7 54/14/6 61/43/22 69/42/21
+f 97/61/14 91/62/14 2/28/14
+f 51/5/1 57/54/1 1/3/1
+f 24/63/14 19/64/14 2/28/14
+f 67/65/14 87/34/14 2/28/14
+f 73/66/14 67/65/14 2/28/14
+f 21/58/14 24/63/14 2/28/14
+f 43/67/14 15/57/14 2/28/14
+f 70/39/14 73/66/14 2/28/14
+f 19/64/14 63/38/14 2/28/14
+f 46/27/14 49/68/14 2/28/14
+f 49/68/14 43/67/14 2/28/14
+f 91/62/14 39/26/14 2/28/14
+f 96/56/32 82/55/31 76/60/34 90/59/33
+f 94/35/14 97/61/14 2/28/14
+f 27/11/1 33/12/1 1/3/1
+f 10/18/35 3/2/36 5/69/37 11/70/38
+f 11/70/38 5/69/37 4/71/9 9/72/28
+f 19/64/39 24/63/40 26/73/41 20/74/42
+f 20/74/42 26/73/41 25/75/27 18/76/8
+f 13/19/43 10/18/35 11/70/38 14/77/44
+f 14/77/44 11/70/38 9/72/28 12/78/25
+f 24/63/40 21/58/45 23/79/46 26/73/41
+f 26/73/41 23/79/46 22/80/26 25/75/27
+f 7/10/47 13/19/43 14/77/44 8/81/48
+f 8/81/48 14/77/44 12/78/25 6/82/10
+f 21/58/45 15/57/49 17/83/50 23/79/46
+f 23/79/46 17/83/50 16/84/11 22/80/26
+f 46/27/51 39/26/52 41/85/53 47/86/54
+f 47/86/54 41/85/53 40/87/15 45/88/18
+f 31/24/55 36/13/56 38/89/57 32/90/58
+f 32/90/58 38/89/57 37/91/17 30/92/16
+f 49/68/59 46/27/51 47/86/54 50/93/60
+f 50/93/60 47/86/54 45/88/18 48/94/30
+f 36/13/56 33/12/61 35/95/62 38/89/57
+f 38/89/57 35/95/62 34/96/29 37/91/17
+f 43/67/63 49/68/59 50/93/60 44/97/64
+f 44/97/64 50/93/60 48/94/30 42/98/12
+f 33/12/61 27/11/65 29/99/66 35/95/62
+f 35/95/62 29/99/66 28/100/13 34/96/29
+f 94/35/67 87/34/68 89/101/69 95/102/70
+f 95/102/70 89/101/69 88/103/2 93/104/5
+f 79/4/71 84/33/72 86/105/73 80/106/74
+f 80/106/74 86/105/73 85/107/4 78/108/3
+f 97/61/75 94/35/67 95/102/70 98/109/76
+f 98/109/76 95/102/70 93/104/5 96/110/32
+f 84/33/72 81/36/77 83/111/78 86/105/73
+f 86/105/73 83/111/78 82/112/31 85/107/4
+f 91/62/79 97/61/75 98/109/76 92/113/80
+f 92/113/80 98/109/76 96/110/32 90/114/33
+f 81/36/77 75/25/81 77/115/82 83/111/78
+f 83/111/78 77/115/82 76/116/34 82/112/31
+f 70/39/83 63/38/84 65/117/85 71/118/86
+f 71/118/86 65/117/85 64/119/7 69/120/21
+f 55/1/87 60/37/88 62/121/89 56/122/90
+f 56/122/90 62/121/89 61/123/22 54/124/6
+f 73/66/91 70/39/83 71/118/86 74/125/92
+f 74/125/92 71/118/86 69/120/21 72/126/24
+f 60/37/88 57/54/93 59/127/94 62/121/89
+f 62/121/89 59/127/94 58/128/23 61/123/22
+f 67/65/95 73/66/91 74/125/92 68/129/96
+f 68/129/96 74/125/92 72/126/24 66/130/19
+f 57/54/93 51/5/97 53/131/98 59/127/94
+f 59/127/94 53/131/98 52/132/20 58/128/23
+f 6/82/10 28/100/13 29/99/66 8/81/48
+f 8/81/48 29/99/66 27/11/65 7/10/47
+f 30/92/16 76/116/34 77/115/82 32/90/58
+f 32/90/58 77/115/82 75/25/81 31/24/55
+f 78/108/3 52/132/20 53/131/98 80/106/74
+f 80/106/74 53/131/98 51/5/97 79/4/71
+f 54/124/6 4/71/9 5/69/37 56/122/90
+f 56/122/90 5/69/37 3/2/36 55/1/87
+f 15/57/49 43/67/63 44/97/64 17/83/50
+f 17/83/50 44/97/64 42/98/12 16/84/11
+f 63/38/84 19/64/39 20/74/42 65/117/85
+f 65/117/85 20/74/42 18/76/8 64/119/7
+f 66/130/19 88/103/2 89/101/69 68/129/96
+f 68/129/96 89/101/69 87/34/68 67/65/95
+f 90/114/33 40/87/15 41/85/53 92/113/80
+f 92/113/80 41/85/53 39/26/52 91/62/79
+f 3/2/1 10/18/1 1/3/1
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/green_star.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/green_star.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..d1c303aa5266840696ff8e7930258c044eaa1884
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/green_star.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/moon.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/moon.obj
new file mode 100644
index 0000000000000000000000000000000000000000..2a862c636f1a2752f363d4f0581f01061fde1b26
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/moon.obj
@@ -0,0 +1,446 @@
+# Blender v2.92.0 OBJ File: ''
+# www.blender.org
+mtllib block.mtl
+o moon_red_block_Cylinder.012
+v -0.009181 0.000000 -0.006901
+v -0.012205 0.002032 -0.009549
+v -0.011319 0.000595 -0.008773
+v -0.021856 0.000000 -0.011560
+v -0.021140 0.002032 -0.012614
+v -0.021633 0.000595 -0.012305
+v -0.021140 0.036068 -0.012614
+v -0.021856 0.038100 -0.011560
+v -0.021633 0.037505 -0.012305
+v -0.014039 0.002032 0.009151
+v -0.012620 0.000000 0.008341
+v -0.013624 0.000595 0.008914
+v -0.012620 0.038100 0.008341
+v -0.014039 0.036068 0.009151
+v -0.013624 0.037505 0.008914
+v -0.024317 0.002032 -0.000986
+v -0.021966 0.000000 -0.000780
+v -0.023628 0.000595 -0.000925
+v -0.012205 0.036068 -0.009549
+v -0.009181 0.038100 -0.006901
+v -0.011319 0.037505 -0.008773
+v -0.021966 0.038100 -0.000780
+v -0.024317 0.036068 -0.000986
+v -0.023628 0.037505 -0.000925
+v -0.023768 0.000000 -0.007207
+v -0.026243 0.002032 -0.008076
+v -0.025518 0.000595 -0.007821
+v -0.023815 0.000000 -0.010019
+v -0.026267 0.002032 -0.011168
+v -0.025549 0.000595 -0.010832
+v -0.024498 0.002032 -0.012861
+v -0.023166 0.000000 -0.011548
+v -0.023825 0.000595 -0.012476
+v -0.026243 0.036068 -0.008076
+v -0.023768 0.038100 -0.007207
+v -0.025518 0.037505 -0.007821
+v -0.023166 0.038100 -0.011548
+v -0.024498 0.036068 -0.012861
+v -0.023825 0.037505 -0.012476
+v -0.026267 0.036068 -0.011168
+v -0.023815 0.038100 -0.010019
+v -0.025549 0.037505 -0.010832
+v -0.000000 0.038100 0.001138
+v -0.000000 0.000000 0.001138
+v -0.000000 0.000000 -0.006070
+v -0.000000 0.002032 -0.008427
+v -0.000000 0.000595 -0.007737
+v -0.000000 0.036068 -0.008427
+v -0.000000 0.038100 -0.006070
+v -0.000000 0.037505 -0.007737
+v 0.009181 0.000000 -0.006901
+v 0.012205 0.002032 -0.009549
+v 0.011319 0.000595 -0.008773
+v 0.021856 0.000000 -0.011560
+v 0.021140 0.002032 -0.012614
+v 0.021633 0.000595 -0.012305
+v 0.021140 0.036068 -0.012614
+v 0.021856 0.038100 -0.011560
+v 0.021633 0.037505 -0.012305
+v -0.000000 0.002032 0.012861
+v -0.000000 0.000000 0.011641
+v -0.000000 0.000595 0.012504
+v -0.000000 0.038100 0.011640
+v -0.000000 0.036068 0.012861
+v -0.000000 0.037505 0.012503
+v 0.014039 0.002032 0.009151
+v 0.012620 0.000000 0.008341
+v 0.013624 0.000595 0.008914
+v 0.012620 0.038100 0.008341
+v 0.014039 0.036068 0.009151
+v 0.013624 0.037505 0.008914
+v 0.024317 0.002032 -0.000986
+v 0.021966 0.000000 -0.000780
+v 0.023628 0.000595 -0.000925
+v 0.012205 0.036068 -0.009549
+v 0.009181 0.038100 -0.006901
+v 0.011319 0.037505 -0.008773
+v 0.021966 0.038100 -0.000780
+v 0.024317 0.036068 -0.000986
+v 0.023628 0.037505 -0.000925
+v 0.023768 0.000000 -0.007207
+v 0.026243 0.002032 -0.008076
+v 0.025518 0.000595 -0.007821
+v 0.023815 0.000000 -0.010019
+v 0.026267 0.002032 -0.011168
+v 0.025549 0.000595 -0.010832
+v 0.024498 0.002032 -0.012861
+v 0.023166 0.000000 -0.011548
+v 0.023825 0.000595 -0.012476
+v 0.026243 0.036068 -0.008076
+v 0.023768 0.038100 -0.007207
+v 0.025518 0.037505 -0.007821
+v 0.023166 0.038100 -0.011548
+v 0.024498 0.036068 -0.012861
+v 0.023825 0.037505 -0.012476
+v 0.026267 0.036068 -0.011168
+v 0.023815 0.038100 -0.010019
+v 0.025549 0.037505 -0.010832
+vt 0.710579 0.167405
+vt 0.710864 0.161788
+vt 0.714864 0.161788
+vt 0.686288 0.163021
+vt 0.706306 0.163021
+vt 0.706306 0.165002
+vt 0.686288 0.165002
+vt 0.708556 0.175784
+vt 0.708475 0.175088
+vt 0.721180 0.161788
+vt 0.719512 0.169043
+vt 0.714650 0.174731
+vt 0.711007 0.176048
+vt 0.720186 0.145021
+vt 0.718589 0.144931
+vt 0.714732 0.130671
+vt 0.709411 0.176139
+vt 0.721121 0.143971
+vt 0.721041 0.144667
+vt 0.686288 0.138411
+vt 0.706306 0.138411
+vt 0.706306 0.146951
+vt 0.686288 0.146951
+vt 0.706306 0.155441
+vt 0.686288 0.155441
+vt 0.706306 0.159762
+vt 0.686288 0.159762
+vt 0.714946 0.143614
+vt 0.686288 0.177766
+vt 0.686288 0.170557
+vt 0.706306 0.170557
+vt 0.706306 0.177766
+vt 0.686288 0.161581
+vt 0.706306 0.161581
+vt 0.718732 0.130671
+vt 0.719018 0.136287
+vt 0.710084 0.137925
+vt 0.708416 0.130671
+vt 0.709447 0.168778
+vt 0.709791 0.161788
+vt 0.708408 0.169256
+vt 0.708838 0.161788
+vt 0.707932 0.175020
+vt 0.707041 0.174762
+vt 0.722722 0.161788
+vt 0.720733 0.170191
+vt 0.719979 0.169645
+vt 0.721789 0.161788
+vt 0.708863 0.139073
+vt 0.706874 0.130671
+vt 0.707807 0.130671
+vt 0.709618 0.138527
+vt 0.715051 0.176717
+vt 0.714702 0.175778
+vt 0.719806 0.130670
+vt 0.720149 0.137661
+vt 0.720758 0.130670
+vt 0.721188 0.138138
+vt 0.721665 0.143903
+vt 0.722555 0.143645
+vt 0.714546 0.145599
+vt 0.714894 0.144661
+vt 0.721564 0.145099
+vt 0.720661 0.146046
+vt 0.722312 0.145803
+vt 0.721098 0.146965
+vt 0.708935 0.177163
+vt 0.708032 0.176217
+vt 0.708498 0.178082
+vt 0.707284 0.176921
+vt 0.718902 0.146070
+vt 0.719002 0.147136
+vt 0.710695 0.177187
+vt 0.710594 0.178254
+vt 0.710579 0.156171
+vt 0.686288 0.192510
+vt 0.686288 0.190530
+vt 0.706306 0.190530
+vt 0.706306 0.192510
+vt 0.708556 0.147792
+vt 0.708475 0.148488
+vt 0.719512 0.154534
+vt 0.714650 0.148845
+vt 0.711007 0.147528
+vt 0.720186 0.116320
+vt 0.718589 0.116410
+vt 0.709411 0.147437
+vt 0.721121 0.117370
+vt 0.721041 0.116674
+vt 0.686288 0.129870
+vt 0.706306 0.129870
+vt 0.686288 0.121380
+vt 0.706306 0.121380
+vt 0.686288 0.117059
+vt 0.706306 0.117059
+vt 0.714946 0.117727
+vt 0.706306 0.184974
+vt 0.686288 0.184974
+vt 0.706306 0.193950
+vt 0.686288 0.193950
+vt 0.719018 0.125054
+vt 0.710084 0.123416
+vt 0.706306 0.115240
+vt 0.686288 0.115240
+vt 0.709447 0.154798
+vt 0.708408 0.154321
+vt 0.707932 0.148556
+vt 0.707041 0.148814
+vt 0.719979 0.153931
+vt 0.720733 0.153385
+vt 0.708863 0.122268
+vt 0.709618 0.122814
+vt 0.714702 0.147798
+vt 0.715051 0.146860
+vt 0.720149 0.123680
+vt 0.721188 0.123203
+vt 0.721665 0.117438
+vt 0.722555 0.117696
+vt 0.714546 0.115742
+vt 0.714894 0.116680
+vt 0.720661 0.115295
+vt 0.721564 0.116241
+vt 0.721098 0.114376
+vt 0.722312 0.115538
+vt 0.708032 0.147359
+vt 0.708935 0.146413
+vt 0.707284 0.146655
+vt 0.708498 0.145494
+vt 0.718901 0.115271
+vt 0.719002 0.114205
+vt 0.710695 0.146389
+vt 0.710595 0.145323
+vn 0.0000 -1.0000 -0.0000
+vn -0.3609 -0.1941 -0.9122
+vn -0.3609 0.1941 -0.9122
+vn 0.2092 0.1359 -0.9684
+vn 0.2092 -0.1359 -0.9684
+vn 0.0000 1.0000 -0.0000
+vn -0.0000 -0.1176 0.9931
+vn -0.0000 0.1176 0.9931
+vn -0.5010 0.1369 0.8546
+vn -0.5010 -0.1369 0.8546
+vn -0.8572 0.1847 0.4806
+vn -0.8572 -0.1847 0.4806
+vn -0.9678 0.2183 0.1257
+vn -0.9678 -0.2183 0.1257
+vn 0.0000 -0.2255 -0.9742
+vn 0.2279 -0.1771 -0.9575
+vn 0.2279 0.1771 -0.9575
+vn 0.0000 0.2255 -0.9742
+vn -0.8945 -0.2361 -0.3797
+vn -0.8945 0.2361 -0.3797
+vn 0.0000 -0.9870 -0.1606
+vn 0.0247 -0.9852 -0.1694
+vn 0.1624 -0.6740 -0.7206
+vn 0.0000 -0.7551 -0.6556
+vn 0.0583 -0.9460 -0.3190
+vn 0.1517 -0.5680 -0.8089
+vn -0.4024 -0.5836 0.7053
+vn 0.0000 -0.5393 0.8421
+vn -0.1318 -0.9470 0.2929
+vn 0.0000 -0.9390 0.3440
+vn 0.0000 0.5393 0.8421
+vn -0.4024 0.5836 0.7053
+vn 0.0000 0.9390 0.3440
+vn -0.1318 0.9470 0.2929
+vn -0.6387 -0.6655 0.3863
+vn -0.2030 -0.9669 0.1548
+vn 0.0247 0.9852 -0.1694
+vn -0.0000 0.9870 -0.1606
+vn 0.0000 0.7551 -0.6556
+vn 0.1624 0.6740 -0.7206
+vn 0.0583 0.9460 -0.3190
+vn 0.1517 0.5680 -0.8089
+vn -0.6387 0.6655 0.3863
+vn -0.2030 0.9669 0.1548
+vn -0.1804 0.9821 -0.0543
+vn -0.0951 0.9531 -0.2874
+vn -0.2234 0.6362 -0.7384
+vn -0.6431 0.7203 -0.2600
+vn -0.0951 -0.9531 -0.2874
+vn -0.1804 -0.9821 -0.0543
+vn -0.6431 -0.7203 -0.2600
+vn -0.2234 -0.6362 -0.7384
+vn -0.1858 0.9820 0.0343
+vn -0.6801 0.7265 0.0987
+vn -0.1858 -0.9820 0.0343
+vn -0.6801 -0.7265 0.0987
+vn 0.3609 -0.1941 -0.9122
+vn -0.2092 -0.1359 -0.9684
+vn -0.2092 0.1359 -0.9684
+vn 0.3609 0.1941 -0.9122
+vn 0.5010 -0.1369 0.8546
+vn 0.5010 0.1369 0.8546
+vn 0.8572 -0.1847 0.4806
+vn 0.8572 0.1847 0.4806
+vn 0.9678 -0.2183 0.1257
+vn 0.9678 0.2183 0.1257
+vn -0.2279 0.1771 -0.9575
+vn -0.2279 -0.1771 -0.9575
+vn 0.8945 0.2361 -0.3797
+vn 0.8945 -0.2361 -0.3797
+vn -0.1624 -0.6740 -0.7206
+vn -0.0247 -0.9852 -0.1694
+vn -0.1517 -0.5680 -0.8089
+vn -0.0583 -0.9460 -0.3190
+vn 0.4024 -0.5836 0.7053
+vn 0.1318 -0.9470 0.2929
+vn 0.4024 0.5836 0.7053
+vn 0.1318 0.9470 0.2929
+vn 0.6387 -0.6655 0.3863
+vn 0.2030 -0.9669 0.1548
+vn -0.0247 0.9852 -0.1694
+vn -0.1624 0.6740 -0.7206
+vn -0.0583 0.9460 -0.3190
+vn -0.1517 0.5680 -0.8089
+vn 0.6387 0.6655 0.3863
+vn 0.2030 0.9669 0.1548
+vn 0.1804 0.9821 -0.0543
+vn 0.6431 0.7203 -0.2600
+vn 0.2234 0.6362 -0.7384
+vn 0.0951 0.9531 -0.2874
+vn 0.0951 -0.9531 -0.2874
+vn 0.2234 -0.6362 -0.7384
+vn 0.6431 -0.7203 -0.2600
+vn 0.1804 -0.9821 -0.0543
+vn 0.1858 0.9820 0.0343
+vn 0.6801 0.7265 0.0987
+vn 0.6801 -0.7265 0.0987
+vn 0.1858 -0.9820 0.0343
+usemtl toybox
+s 1
+f 1/1/1 45/2/1 44/3/1
+f 31/4/2 38/5/3 7/6/4 5/7/5
+f 32/8/1 4/9/1 44/3/1
+f 61/10/1 11/11/1 44/3/1
+f 11/11/1 17/12/1 44/3/1
+f 17/12/1 25/13/1 44/3/1
+f 41/14/6 35/15/6 43/16/6
+f 28/17/1 32/8/1 44/3/1
+f 8/18/6 37/19/6 43/16/6
+f 37/19/6 41/14/6 43/16/6
+f 25/13/1 28/17/1 44/3/1
+f 60/20/7 64/21/8 14/22/9 10/23/10
+f 10/23/10 14/22/9 23/24/11 16/25/12
+f 16/25/12 23/24/11 34/26/13 26/27/14
+f 35/15/6 22/28/6 43/16/6
+f 4/9/1 1/1/1 44/3/1
+f 46/29/15 2/30/16 19/31/17 48/32/18
+f 38/5/3 31/4/2 29/33/19 40/34/20
+f 49/35/6 20/36/6 43/16/6
+f 13/37/6 63/38/6 43/16/6
+f 2/30/16 5/7/5 7/6/4 19/31/17
+f 40/34/20 29/33/19 26/27/14 34/26/13
+f 20/36/6 8/18/6 43/16/6
+f 22/28/6 13/37/6 43/16/6
+f 45/2/21 1/1/22 3/39/23 47/40/24
+f 47/40/24 3/39/23 2/41/16 46/42/15
+f 1/1/22 4/9/25 6/43/26 3/39/23
+f 3/39/23 6/43/26 5/44/5 2/41/16
+f 60/45/7 10/46/10 12/47/27 62/48/28
+f 62/48/28 12/47/27 11/11/29 61/10/30
+f 14/49/9 64/50/8 65/51/31 15/52/32
+f 15/52/32 65/51/31 63/38/33 13/37/34
+f 10/46/10 16/53/12 18/54/35 12/47/27
+f 12/47/27 18/54/35 17/12/36 11/11/29
+f 20/36/37 49/35/38 50/55/39 21/56/40
+f 21/56/40 50/55/39 48/57/18 19/58/17
+f 8/18/41 20/36/37 21/56/40 9/59/42
+f 9/59/42 21/56/40 19/58/17 7/60/4
+f 23/61/11 14/49/9 15/52/32 24/62/43
+f 24/62/43 15/52/32 13/37/34 22/28/44
+f 41/14/45 37/19/46 39/63/47 42/64/48
+f 42/64/48 39/63/47 38/65/3 40/66/20
+f 32/8/49 28/17/50 30/67/51 33/68/52
+f 33/68/52 30/67/51 29/69/19 31/70/2
+f 35/15/53 41/14/45 42/64/48 36/71/54
+f 36/71/54 42/64/48 40/66/20 34/72/13
+f 28/17/50 25/13/55 27/73/56 30/67/51
+f 30/67/51 27/73/56 26/74/14 29/69/19
+f 25/13/55 17/12/36 18/54/35 27/73/56
+f 27/73/56 18/54/35 16/53/12 26/74/14
+f 22/28/44 35/15/53 36/71/54 24/62/43
+f 24/62/43 36/71/54 34/72/13 23/61/11
+f 4/9/25 32/8/49 33/68/52 6/43/26
+f 6/43/26 33/68/52 31/70/2 5/44/5
+f 37/19/46 8/18/41 9/59/42 39/63/47
+f 39/63/47 9/59/42 7/60/4 38/65/3
+f 51/75/1 44/3/1 45/2/1
+f 87/76/57 55/77/58 57/78/59 94/79/60
+f 88/80/1 44/3/1 54/81/1
+f 61/10/1 44/3/1 67/82/1
+f 67/82/1 44/3/1 73/83/1
+f 73/83/1 44/3/1 81/84/1
+f 97/85/6 43/16/6 91/86/6
+f 84/87/1 44/3/1 88/80/1
+f 58/88/6 43/16/6 93/89/6
+f 93/89/6 43/16/6 97/85/6
+f 81/84/1 44/3/1 84/87/1
+f 60/20/7 66/90/61 70/91/62 64/21/8
+f 66/90/61 72/92/63 79/93/64 70/91/62
+f 72/92/63 82/94/65 90/95/66 79/93/64
+f 91/86/6 43/16/6 78/96/6
+f 54/81/1 44/3/1 51/75/1
+f 46/29/15 48/32/18 75/97/67 52/98/68
+f 94/79/60 96/99/69 85/100/70 87/76/57
+f 49/35/6 43/16/6 76/101/6
+f 69/102/6 43/16/6 63/38/6
+f 52/98/68 75/97/67 57/78/59 55/77/58
+f 96/103/69 90/95/66 82/94/65 85/104/70
+f 76/101/6 43/16/6 58/88/6
+f 78/96/6 43/16/6 69/102/6
+f 45/2/21 47/40/24 53/105/71 51/75/72
+f 47/40/24 46/42/15 52/106/68 53/105/71
+f 51/75/72 53/105/71 56/107/73 54/81/74
+f 53/105/71 52/106/68 55/108/58 56/107/73
+f 60/45/7 62/48/28 68/109/75 66/110/61
+f 62/48/28 61/10/30 67/82/76 68/109/75
+f 70/111/62 71/112/77 65/51/31 64/50/8
+f 71/112/77 69/102/78 63/38/33 65/51/31
+f 66/110/61 68/109/75 74/113/79 72/114/63
+f 68/109/75 67/82/76 73/83/80 74/113/79
+f 76/101/81 77/115/82 50/55/39 49/35/38
+f 77/115/82 75/116/67 48/57/18 50/55/39
+f 58/88/83 59/117/84 77/115/82 76/101/81
+f 59/117/84 57/118/59 75/116/67 77/115/82
+f 79/119/64 80/120/85 71/112/77 70/111/62
+f 80/120/85 78/96/86 69/102/78 71/112/77
+f 97/85/87 98/121/88 95/122/89 93/89/90
+f 98/121/88 96/123/69 94/124/60 95/122/89
+f 88/80/91 89/125/92 86/126/93 84/87/94
+f 89/125/92 87/127/57 85/128/70 86/126/93
+f 91/86/95 92/129/96 98/121/88 97/85/87
+f 92/129/96 90/130/66 96/123/69 98/121/88
+f 84/87/94 86/126/93 83/131/97 81/84/98
+f 86/126/93 85/128/70 82/132/65 83/131/97
+f 81/84/98 83/131/97 74/113/79 73/83/80
+f 83/131/97 82/132/65 72/114/63 74/113/79
+f 78/96/86 80/120/85 92/129/96 91/86/95
+f 80/120/85 79/119/64 90/130/66 92/129/96
+f 54/81/74 56/107/73 89/125/92 88/80/91
+f 56/107/73 55/108/58 87/127/57 89/125/92
+f 93/89/90 95/122/89 59/117/84 58/88/83
+f 95/122/89 94/124/60 57/118/59 59/117/84
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/pentagon.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/pentagon.obj
new file mode 100644
index 0000000000000000000000000000000000000000..7054e4172efb0fc6c4cbfc774afe86fa8db41a71
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/pentagon.obj
@@ -0,0 +1,419 @@
+# Blender v2.92.0 OBJ File: ''
+# www.blender.org
+mtllib pentagon.mtl
+o pentagon_yellow_block_Cube.003
+v -0.000000 0.000000 -0.001329
+v -0.000000 0.038100 -0.001329
+v -0.010789 0.000000 0.015873
+v -0.011940 0.002032 0.017708
+v -0.011603 0.000595 0.017170
+v -0.014201 0.002032 0.016092
+v -0.012833 0.000000 0.014413
+v -0.013800 0.000595 0.015600
+v -0.013310 0.002032 0.017235
+v -0.012029 0.000000 0.015448
+v -0.012935 0.000595 0.016711
+v -0.012833 0.038100 0.014413
+v -0.014201 0.036068 0.016092
+v -0.013800 0.037505 0.015600
+v -0.011940 0.036068 0.017708
+v -0.010789 0.038100 0.015873
+v -0.011603 0.037505 0.017170
+v -0.012029 0.038100 0.015448
+v -0.013310 0.036068 0.017235
+v -0.012935 0.037505 0.016711
+v -0.020074 0.000000 -0.006755
+v -0.022213 0.002032 -0.007333
+v -0.021586 0.000595 -0.007164
+v -0.021345 0.002032 -0.009898
+v -0.019288 0.000000 -0.009072
+v -0.020743 0.000595 -0.009656
+v -0.022188 0.002032 -0.008754
+v -0.020054 0.000000 -0.008040
+v -0.021563 0.000595 -0.008545
+v -0.019288 0.038100 -0.009072
+v -0.021345 0.036068 -0.009898
+v -0.020743 0.037505 -0.009656
+v -0.022213 0.036068 -0.007333
+v -0.020074 0.038100 -0.006755
+v -0.021586 0.037505 -0.007164
+v -0.022188 0.036068 -0.008754
+v -0.020054 0.038100 -0.008040
+v -0.021563 0.037505 -0.008545
+v 0.012833 0.000000 0.014413
+v 0.014201 0.002032 0.016092
+v 0.013800 0.000595 0.015600
+v 0.011940 0.002032 0.017708
+v 0.010789 0.000000 0.015873
+v 0.011603 0.000595 0.017170
+v 0.012029 0.000000 0.015448
+v 0.013310 0.002032 0.017235
+v 0.012935 0.000595 0.016711
+v 0.010789 0.038100 0.015873
+v 0.011940 0.036068 0.017708
+v 0.011603 0.037505 0.017170
+v 0.014201 0.036068 0.016092
+v 0.012833 0.038100 0.014413
+v 0.013800 0.037505 0.015600
+v 0.013310 0.036068 0.017235
+v 0.012029 0.038100 0.015448
+v 0.012935 0.037505 0.016711
+v 0.019288 0.000000 -0.009072
+v 0.021345 0.002032 -0.009898
+v 0.020743 0.000595 -0.009656
+v 0.022213 0.002032 -0.007333
+v 0.020074 0.000000 -0.006755
+v 0.021586 0.000595 -0.007164
+v 0.022188 0.002032 -0.008754
+v 0.020054 0.000000 -0.008040
+v 0.021563 0.000595 -0.008545
+v 0.020074 0.038100 -0.006755
+v 0.022213 0.036068 -0.007333
+v 0.021586 0.037505 -0.007164
+v 0.021345 0.036068 -0.009898
+v 0.019288 0.038100 -0.009072
+v 0.020743 0.037505 -0.009656
+v 0.022188 0.036068 -0.008754
+v 0.020054 0.038100 -0.008040
+v 0.021563 0.037505 -0.008545
+v 0.001283 0.038100 -0.021099
+v 0.001420 0.036068 -0.023214
+v 0.001380 0.037505 -0.022595
+v -0.001420 0.036068 -0.023214
+v -0.001283 0.038100 -0.021099
+v -0.001380 0.037505 -0.022595
+v 0.000000 0.036068 -0.023607
+v -0.000000 0.038100 -0.021456
+v -0.000000 0.037505 -0.022977
+v -0.001283 0.000000 -0.021099
+v -0.001420 0.002032 -0.023214
+v -0.001380 0.000595 -0.022595
+v 0.001420 0.002032 -0.023214
+v 0.001283 0.000000 -0.021099
+v 0.001380 0.000595 -0.022595
+v -0.000000 0.000000 -0.021456
+v 0.000000 0.002032 -0.023607
+v -0.000000 0.000595 -0.022977
+vt 0.718822 0.029303
+vt 0.730852 0.033346
+vt 0.721614 0.040891
+vt 0.709480 0.040139
+vt 0.717419 0.029729
+vt 0.717181 0.051685
+vt 0.709481 0.041565
+vt 0.686374 0.033371
+vt 0.706477 0.033371
+vt 0.706477 0.034211
+vt 0.686374 0.034211
+vt 0.731711 0.034535
+vt 0.731731 0.047644
+vt 0.732155 0.013538
+vt 0.732386 0.014251
+vt 0.720023 0.014212
+vt 0.724218 0.025374
+vt 0.723581 0.025796
+vt 0.731459 0.033813
+vt 0.686374 0.065488
+vt 0.706477 0.065488
+vt 0.706477 0.066344
+vt 0.686374 0.066344
+vt 0.730865 0.048779
+vt 0.718604 0.052165
+vt 0.717826 0.052123
+vt 0.710784 0.021757
+vt 0.710178 0.021290
+vt 0.706477 0.050528
+vt 0.686374 0.050528
+vt 0.686374 0.049672
+vt 0.706477 0.049672
+vt 0.709250 0.040852
+vt 0.718055 0.029306
+vt 0.686374 0.051384
+vt 0.706477 0.051384
+vt 0.686374 0.035050
+vt 0.706477 0.035050
+vt 0.686374 0.019217
+vt 0.706477 0.019217
+vt 0.731482 0.048351
+vt 0.709906 0.007458
+vt 0.710155 0.006751
+vt 0.686374 0.003322
+vt 0.706477 0.003322
+vt 0.706477 0.017476
+vt 0.686374 0.017476
+vt 0.706477 0.081821
+vt 0.686374 0.081821
+vt 0.686374 0.067199
+vt 0.706477 0.067199
+vt 0.723811 0.002980
+vt 0.724455 0.003418
+vt 0.706477 0.082661
+vt 0.686374 0.082661
+vt 0.686374 0.002483
+vt 0.706477 0.002483
+vt 0.732157 0.014964
+vt 0.723033 0.002938
+vt 0.722814 0.025800
+vt 0.686374 0.018347
+vt 0.706477 0.018347
+vt 0.709926 0.020568
+vt 0.710771 0.006324
+vt 0.733509 0.048629
+vt 0.733072 0.049555
+vt 0.732273 0.048949
+vt 0.732572 0.048190
+vt 0.732296 0.050225
+vt 0.731620 0.049441
+vt 0.734168 0.013254
+vt 0.734379 0.014256
+vt 0.733377 0.014254
+vt 0.733156 0.013468
+vt 0.734166 0.015257
+vt 0.733156 0.015039
+vt 0.707468 0.041849
+vt 0.707257 0.040847
+vt 0.708259 0.040849
+vt 0.708480 0.041634
+vt 0.707471 0.039846
+vt 0.708480 0.040063
+vt 0.708128 0.006474
+vt 0.708564 0.005547
+vt 0.709364 0.006153
+vt 0.709064 0.006913
+vt 0.709341 0.004877
+vt 0.710016 0.005662
+vt 0.731599 0.032714
+vt 0.732251 0.033244
+vt 0.732301 0.031933
+vt 0.733073 0.032652
+vt 0.723869 0.026728
+vt 0.723033 0.026764
+vt 0.724166 0.027687
+vt 0.723129 0.027790
+vt 0.732546 0.034029
+vt 0.733509 0.033612
+vt 0.724576 0.026281
+vt 0.725089 0.027190
+vt 0.710038 0.022388
+vt 0.709386 0.021859
+vt 0.709336 0.023170
+vt 0.708563 0.022451
+vt 0.717767 0.028375
+vt 0.718604 0.028339
+vt 0.717470 0.027416
+vt 0.718508 0.027313
+vt 0.709090 0.021074
+vt 0.708128 0.021490
+vt 0.717060 0.028822
+vt 0.716547 0.027913
+vt 0.723265 0.002005
+vt 0.724118 0.002071
+vt 0.723385 0.000970
+vt 0.724440 0.001117
+vt 0.717519 0.053032
+vt 0.716801 0.052568
+vt 0.717197 0.053986
+vt 0.716269 0.053464
+vt 0.724836 0.002535
+vt 0.725367 0.001639
+vt 0.718372 0.053098
+vt 0.718251 0.054133
+vn 0.0000 -1.0000 0.0000
+vn -0.6839 0.1913 -0.7040
+vn -0.6839 -0.1913 -0.7040
+vn -0.9271 -0.2054 -0.3134
+vn -0.9271 0.2054 -0.3134
+vn 0.0000 1.0000 -0.0000
+vn 0.1696 0.1897 0.9671
+vn 0.1696 -0.1897 0.9671
+vn 0.5696 -0.2019 0.7968
+vn 0.5696 0.2019 0.7968
+vn -0.5696 -0.2019 0.7968
+vn -0.5696 0.2019 0.7968
+vn -0.8602 0.1898 0.4734
+vn -0.8602 -0.1898 0.4734
+vn -0.1696 0.1897 0.9671
+vn -0.1696 -0.1897 0.9671
+vn -0.9708 0.1916 0.1446
+vn -0.9708 -0.1916 0.1446
+vn -0.4034 0.1878 -0.8955
+vn -0.4034 -0.1878 -0.8955
+vn 0.6839 0.1913 -0.7040
+vn 0.6839 -0.1913 -0.7040
+vn 0.4034 -0.1878 -0.8955
+vn 0.4034 0.1878 -0.8955
+vn 0.9708 -0.1916 0.1446
+vn 0.9708 0.1916 0.1446
+vn 0.8602 0.1898 0.4734
+vn 0.8602 -0.1898 0.4734
+vn 0.9271 -0.2054 -0.3134
+vn 0.9271 0.2054 -0.3134
+vn 0.0000 0.1985 -0.9801
+vn 0.0000 -0.1985 -0.9801
+vn -0.6793 -0.6969 -0.2300
+vn -0.7180 -0.6862 0.1166
+vn -0.2003 -0.9774 -0.0681
+vn -0.2163 -0.9753 0.0454
+vn -0.5001 -0.6857 -0.5289
+vn -0.1444 -0.9752 -0.1679
+vn -0.6793 0.6969 -0.2300
+vn -0.5001 0.6857 -0.5289
+vn -0.2003 0.9774 -0.0681
+vn -0.1444 0.9752 -0.1679
+vn -0.7180 0.6862 0.1166
+vn -0.2163 0.9753 0.0454
+vn 0.6793 -0.6969 -0.2300
+vn 0.5001 -0.6857 -0.5289
+vn 0.2003 -0.9774 -0.0681
+vn 0.1444 -0.9752 -0.1679
+vn 0.7180 -0.6862 0.1166
+vn 0.2163 -0.9753 0.0454
+vn 0.6793 0.6969 -0.2300
+vn 0.7180 0.6862 0.1166
+vn 0.2003 0.9774 -0.0681
+vn 0.2163 0.9753 0.0454
+vn 0.5001 0.6857 -0.5289
+vn 0.1444 0.9752 -0.1679
+vn -0.1241 -0.9769 0.1739
+vn -0.0267 -0.9752 0.2199
+vn -0.1178 -0.6841 0.7198
+vn -0.4189 -0.6935 0.5862
+vn -0.0267 0.9752 0.2199
+vn -0.1242 0.9769 0.1739
+vn -0.4189 0.6935 0.5862
+vn -0.1178 0.6841 0.7198
+vn -0.1992 -0.9752 0.0965
+vn -0.6427 -0.6843 0.3444
+vn -0.1992 0.9752 0.0965
+vn -0.6427 0.6843 0.3444
+vn 0.1242 0.9769 0.1739
+vn 0.0267 0.9752 0.2199
+vn 0.1178 0.6841 0.7198
+vn 0.4189 0.6935 0.5862
+vn 0.0267 -0.9752 0.2199
+vn 0.1241 -0.9769 0.1739
+vn 0.4189 -0.6935 0.5862
+vn 0.1178 -0.6841 0.7198
+vn 0.1992 0.9752 0.0965
+vn 0.6427 0.6843 0.3444
+vn 0.1992 -0.9752 0.0965
+vn 0.6427 -0.6843 0.3444
+vn -0.0000 0.9764 -0.2159
+vn 0.1005 0.9750 -0.1981
+vn 0.3067 0.6821 -0.6638
+vn -0.0000 0.6900 -0.7238
+vn 0.1005 -0.9750 -0.1981
+vn 0.0000 -0.9764 -0.2159
+vn 0.0000 -0.6900 -0.7238
+vn 0.3067 -0.6821 -0.6638
+vn -0.1005 0.9750 -0.1981
+vn -0.3067 0.6821 -0.6638
+vn -0.1005 -0.9750 -0.1981
+vn -0.3067 -0.6821 -0.6638
+usemtl toybox
+s 1
+f 43/1/1 3/2/1 1/3/1
+f 61/4/1 39/5/1 1/3/1
+f 88/6/1 57/7/1 1/3/1
+f 31/8/2 24/9/3 27/10/4 36/11/5
+f 7/12/1 21/13/1 1/3/1
+f 30/14/6 37/15/6 2/16/6
+f 12/17/6 18/18/6 2/16/6
+f 10/19/1 7/12/1 1/3/1
+f 49/20/7 42/21/8 46/22/9 54/23/10
+f 25/24/1 84/25/1 1/3/1
+f 90/26/1 88/6/1 1/3/1
+f 3/2/1 10/19/1 1/3/1
+f 48/27/6 55/28/6 2/16/6
+f 9/29/11 19/30/12 13/31/13 6/32/14
+f 64/33/1 61/4/1 1/3/1
+f 39/5/1 45/34/1 1/3/1
+f 42/21/8 49/20/7 15/35/15 4/36/16
+f 6/32/14 13/31/13 33/37/17 22/38/18
+f 24/9/3 31/8/2 78/39/19 85/40/20
+f 21/13/1 28/41/1 1/3/1
+f 66/42/6 73/43/6 2/16/6
+f 69/44/21 58/45/22 87/46/23 76/47/24
+f 45/34/1 43/1/1 1/3/1
+f 60/48/25 67/49/26 51/50/27 40/51/28
+f 82/52/6 79/53/6 2/16/6
+f 84/25/1 90/26/1 1/3/1
+f 67/49/26 60/48/25 63/54/29 72/55/30
+f 28/41/1 25/24/1 1/3/1
+f 72/56/30 63/57/29 58/45/22 69/44/21
+f 37/15/6 34/58/6 2/16/6
+f 79/53/6 30/14/6 2/16/6
+f 75/59/6 82/52/6 2/16/6
+f 18/18/6 16/60/6 2/16/6
+f 54/23/10 46/22/9 40/51/28 51/50/27
+f 81/61/31 91/62/32 85/40/20 78/39/19
+f 16/60/6 48/27/6 2/16/6
+f 52/63/6 66/42/6 2/16/6
+f 36/11/5 27/10/4 22/38/18 33/37/17
+f 73/43/6 70/64/6 2/16/6
+f 57/7/1 64/33/1 1/3/1
+f 70/64/6 75/59/6 2/16/6
+f 34/58/6 12/17/6 2/16/6
+f 76/47/24 87/46/23 91/62/32 81/61/31
+f 55/28/6 52/63/6 2/16/6
+f 22/65/18 27/66/4 29/67/33 23/68/34
+f 23/68/34 29/67/33 28/41/35 21/13/36
+f 27/66/4 24/69/3 26/70/37 29/67/33
+f 29/67/33 26/70/37 25/24/38 28/41/35
+f 31/71/2 36/72/5 38/73/39 32/74/40
+f 32/74/40 38/73/39 37/15/41 30/14/42
+f 36/72/5 33/75/17 35/76/43 38/73/39
+f 38/73/39 35/76/43 34/58/44 37/15/41
+f 58/77/22 63/78/29 65/79/45 59/80/46
+f 59/80/46 65/79/45 64/33/47 57/7/48
+f 63/78/29 60/81/25 62/82/49 65/79/45
+f 65/79/45 62/82/49 61/4/50 64/33/47
+f 67/83/26 72/84/30 74/85/51 68/86/52
+f 68/86/52 74/85/51 73/43/53 66/42/54
+f 72/84/30 69/87/21 71/88/55 74/85/51
+f 74/85/51 71/88/55 70/64/56 73/43/53
+f 10/19/57 3/2/58 5/89/59 11/90/60
+f 11/90/60 5/89/59 4/91/16 9/92/11
+f 16/60/61 18/18/62 20/93/63 17/94/64
+f 17/94/64 20/93/63 19/95/12 15/96/15
+f 7/12/65 10/19/57 11/90/60 8/97/66
+f 8/97/66 11/90/60 9/92/11 6/98/14
+f 18/18/62 12/17/67 14/99/68 20/93/63
+f 20/93/63 14/99/68 13/100/13 19/95/12
+f 55/28/69 48/27/70 50/101/71 56/102/72
+f 56/102/72 50/101/71 49/103/7 54/104/10
+f 43/1/73 45/34/74 47/105/75 44/106/76
+f 44/106/76 47/105/75 46/107/9 42/108/8
+f 52/63/77 55/28/69 56/102/72 53/109/78
+f 53/109/78 56/102/72 54/104/10 51/110/27
+f 45/34/74 39/5/79 41/111/80 47/105/75
+f 47/105/75 41/111/80 40/112/28 46/107/9
+f 82/52/81 75/59/82 77/113/83 83/114/84
+f 83/114/84 77/113/83 76/115/24 81/116/31
+f 88/6/85 90/26/86 92/117/87 89/118/88
+f 89/118/88 92/117/87 91/119/32 87/120/23
+f 79/53/89 82/52/81 83/114/84 80/121/90
+f 80/121/90 83/114/84 81/116/31 78/122/19
+f 90/26/86 84/25/91 86/123/92 92/117/87
+f 92/117/87 86/123/92 85/124/20 91/119/32
+f 6/98/14 22/65/18 23/68/34 8/97/66
+f 8/97/66 23/68/34 21/13/36 7/12/65
+f 60/81/25 40/112/28 41/111/80 62/82/49
+f 62/82/49 41/111/80 39/5/79 61/4/50
+f 42/108/8 4/91/16 5/89/59 44/106/76
+f 44/106/76 5/89/59 3/2/58 43/1/73
+f 12/17/67 34/58/44 35/76/43 14/99/68
+f 14/99/68 35/76/43 33/75/17 13/100/13
+f 48/27/70 16/60/61 17/94/64 50/101/71
+f 50/101/71 17/94/64 15/96/15 49/103/7
+f 51/110/27 67/83/26 68/86/52 53/109/78
+f 53/109/78 68/86/52 66/42/54 52/63/77
+f 84/25/91 25/24/38 26/70/37 86/123/92
+f 86/123/92 26/70/37 24/69/3 85/124/20
+f 30/14/42 79/53/89 80/121/90 32/74/40
+f 32/74/40 80/121/90 78/122/19 31/71/2
+f 87/120/23 58/77/22 59/80/46 89/118/88
+f 89/118/88 59/80/46 57/7/48 88/6/85
+f 69/87/21 76/115/24 77/113/83 71/88/55
+f 71/88/55 77/113/83 75/59/82 70/64/56
+f 4/36/16 15/35/15 19/30/12 9/29/11
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/red_moon.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/red_moon.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..36c2b5a43b17d90d0cde3385374c18907e73a6e8
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/red_moon.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/star.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/star.obj
new file mode 100644
index 0000000000000000000000000000000000000000..1c1d696145bf2bfc975d81f2029a9af666855430
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/star.obj
@@ -0,0 +1,689 @@
+# Blender v2.92.0 OBJ File: ''
+# www.blender.org
+mtllib star.mtl
+o star_green_block_star0_block
+v -0.000030 0.000000 0.001549
+v -0.000030 0.038100 0.001549
+v 0.006429 0.000000 -0.009092
+v 0.007380 0.002032 -0.010659
+v 0.007101 0.000595 -0.010200
+v 0.009909 0.002032 -0.008940
+v 0.008636 0.000000 -0.007597
+v 0.009536 0.000595 -0.008547
+v 0.008636 0.038100 -0.007597
+v 0.009909 0.036068 -0.008940
+v 0.009536 0.037505 -0.008547
+v 0.007380 0.036068 -0.010659
+v 0.006429 0.038100 -0.009092
+v 0.007101 0.037505 -0.010200
+v -0.006436 0.038100 -0.009091
+v -0.007380 0.036068 -0.010659
+v -0.007103 0.037505 -0.010200
+v -0.009909 0.036068 -0.008940
+v -0.008642 0.038100 -0.007595
+v -0.009538 0.037505 -0.008546
+v -0.008642 0.000000 -0.007595
+v -0.009909 0.002032 -0.008940
+v -0.009538 0.000595 -0.008546
+v -0.007380 0.002032 -0.010659
+v -0.006436 0.000000 -0.009091
+v -0.007103 0.000595 -0.010200
+v -0.011672 0.000000 0.006854
+v -0.013354 0.002032 0.007620
+v -0.012861 0.000595 0.007396
+v -0.014336 0.002032 0.004714
+v -0.012526 0.000000 0.004313
+v -0.013806 0.000595 0.004596
+v -0.012526 0.038100 0.004313
+v -0.014336 0.036068 0.004714
+v -0.013806 0.037505 0.004596
+v -0.013354 0.036068 0.007620
+v -0.011672 0.038100 0.006854
+v -0.012861 0.037505 0.007396
+v 0.012521 0.000000 0.004314
+v 0.014336 0.002032 0.004714
+v 0.013805 0.000595 0.004597
+v 0.013354 0.002032 0.007620
+v 0.011668 0.000000 0.006855
+v 0.012860 0.000595 0.007396
+v 0.011668 0.038100 0.006855
+v 0.013354 0.036068 0.007620
+v 0.012860 0.037505 0.007396
+v 0.014336 0.036068 0.004714
+v 0.012521 0.038100 0.004314
+v 0.013805 0.037505 0.004597
+v -0.010002 0.000000 0.018088
+v -0.011405 0.002032 0.020414
+v -0.010994 0.000595 0.019733
+v -0.013985 0.002032 0.018583
+v -0.012256 0.000000 0.016473
+v -0.013478 0.000595 0.017965
+v -0.013300 0.002032 0.020351
+v -0.011669 0.000000 0.018040
+v -0.012822 0.000595 0.019674
+v -0.012256 0.038100 0.016473
+v -0.013985 0.036068 0.018583
+v -0.013478 0.037505 0.017965
+v -0.011405 0.036068 0.020414
+v -0.010002 0.038100 0.018088
+v -0.010994 0.037505 0.019733
+v -0.011669 0.038100 0.018040
+v -0.013300 0.036068 0.020351
+v -0.012822 0.037505 0.019674
+v -0.018964 0.000000 -0.003158
+v -0.021672 0.002032 -0.003831
+v -0.020879 0.000595 -0.003634
+v -0.020696 0.002032 -0.006698
+v -0.018097 0.000000 -0.005661
+v -0.019935 0.000595 -0.006394
+v -0.019426 0.000000 -0.004715
+v -0.022190 0.002032 -0.005607
+v -0.021380 0.000595 -0.005346
+v -0.018097 0.038100 -0.005661
+v -0.020696 0.036068 -0.006698
+v -0.019935 0.037505 -0.006394
+v -0.021672 0.036068 -0.003831
+v -0.018964 0.038100 -0.003158
+v -0.020879 0.037505 -0.003634
+v -0.022190 0.036068 -0.005607
+v -0.019426 0.038100 -0.004715
+v -0.021380 0.037505 -0.005346
+v 0.012251 0.000000 0.016476
+v 0.013985 0.002032 0.018583
+v 0.013477 0.000595 0.017966
+v 0.011405 0.002032 0.020414
+v 0.009996 0.000000 0.018089
+v 0.010992 0.000595 0.019733
+v 0.011663 0.000000 0.018043
+v 0.013300 0.002032 0.020351
+v 0.012821 0.000595 0.019675
+v 0.009996 0.038100 0.018089
+v 0.011405 0.036068 0.020414
+v 0.010992 0.037505 0.019733
+v 0.013985 0.036068 0.018583
+v 0.012251 0.038100 0.016476
+v 0.013477 0.037505 0.017966
+v 0.013300 0.036068 0.020351
+v 0.011663 0.038100 0.018043
+v 0.012821 0.037505 0.019675
+v 0.018094 0.000000 -0.005663
+v 0.020696 0.002032 -0.006698
+v 0.019934 0.000595 -0.006395
+v 0.021672 0.002032 -0.003831
+v 0.018961 0.000000 -0.003159
+v 0.020878 0.000595 -0.003635
+v 0.019423 0.000000 -0.004716
+v 0.022190 0.002032 -0.005607
+v 0.021380 0.000595 -0.005346
+v 0.018961 0.038100 -0.003159
+v 0.021672 0.036068 -0.003831
+v 0.020878 0.037505 -0.003635
+v 0.020696 0.036068 -0.006698
+v 0.018094 0.038100 -0.005663
+v 0.019934 0.037505 -0.006395
+v 0.022190 0.036068 -0.005607
+v 0.019423 0.038100 -0.004716
+v 0.021380 0.037505 -0.005346
+v 0.001426 0.038100 -0.016730
+v 0.001638 0.036068 -0.019383
+v 0.001576 0.037505 -0.018606
+v -0.001638 0.036068 -0.019383
+v -0.001434 0.038100 -0.016729
+v -0.001578 0.037505 -0.018606
+v 0.000000 0.036068 -0.020414
+v -0.000004 0.038100 -0.017644
+v -0.000001 0.037505 -0.019603
+v -0.001434 0.000000 -0.016729
+v -0.001638 0.002032 -0.019383
+v -0.001578 0.000595 -0.018606
+v 0.001638 0.002032 -0.019383
+v 0.001426 0.000000 -0.016730
+v 0.001576 0.000595 -0.018606
+v -0.000004 0.000000 -0.017644
+v 0.000000 0.002032 -0.020414
+v -0.000001 0.000595 -0.019603
+v 0.001733 0.000000 0.015081
+v 0.001984 0.002032 0.017005
+v 0.001910 0.000595 0.016441
+v -0.001984 0.002032 0.017005
+v -0.001741 0.000000 0.015080
+v -0.001912 0.000595 0.016441
+v -0.001741 0.038100 0.015080
+v -0.001984 0.036068 0.017005
+v -0.001912 0.037505 0.016441
+v 0.001984 0.036068 0.017005
+v 0.001733 0.038100 0.015081
+v 0.001910 0.037505 0.016441
+vt 0.884326 0.143054
+vt 0.881637 0.147199
+vt 0.874485 0.145636
+vt 0.870501 0.139220
+vt 0.873534 0.134807
+vt 0.837942 0.112414
+vt 0.837942 0.131533
+vt 0.832314 0.131533
+vt 0.832314 0.112414
+vt 0.883572 0.123603
+vt 0.883611 0.122735
+vt 0.892897 0.129050
+vt 0.881240 0.148682
+vt 0.881539 0.153913
+vt 0.898370 0.134009
+vt 0.897030 0.139070
+vt 0.873770 0.153188
+vt 0.868788 0.154800
+vt 0.903052 0.129620
+vt 0.899208 0.132723
+vt 0.862147 0.112414
+vt 0.862147 0.131533
+vt 0.855821 0.131533
+vt 0.855821 0.112414
+vt 0.830184 0.112414
+vt 0.830184 0.131533
+vt 0.824015 0.131533
+vt 0.824015 0.112414
+vt 0.874942 0.134807
+vt 0.877932 0.139104
+vt 0.880430 0.154782
+vt 0.875317 0.153178
+vt 0.866663 0.147297
+vt 0.863946 0.143378
+vt 0.884646 0.135960
+vt 0.883837 0.135640
+vt 0.874236 0.134337
+vt 0.813888 0.131533
+vt 0.813888 0.112414
+vt 0.814927 0.112414
+vt 0.814927 0.131533
+vt 0.881273 0.154719
+vt 0.863186 0.112414
+vt 0.863186 0.131533
+vt 0.867919 0.154747
+vt 0.867585 0.153944
+vt 0.845799 0.112414
+vt 0.845799 0.131533
+vt 0.840171 0.131533
+vt 0.840171 0.112414
+vt 0.903484 0.128858
+vt 0.883832 0.141627
+vt 0.884500 0.142196
+vt 0.864368 0.141965
+vt 0.869278 0.140166
+vt 0.854097 0.112414
+vt 0.854097 0.131533
+vt 0.847929 0.131533
+vt 0.847929 0.112414
+vt 0.896527 0.139753
+vt 0.867312 0.149178
+vt 0.822292 0.112414
+vt 0.822292 0.131533
+vt 0.872132 0.131533
+vt 0.872132 0.112414
+vt 0.877999 0.112414
+vt 0.877999 0.131533
+vt 0.864225 0.131533
+vt 0.864225 0.112414
+vt 0.870414 0.112414
+vt 0.870414 0.131533
+vt 0.815966 0.112414
+vt 0.815966 0.131533
+vt 0.891348 0.136441
+vt 0.889875 0.135971
+vt 0.846864 0.131533
+vt 0.846864 0.112414
+vt 0.831249 0.131533
+vt 0.831249 0.112414
+vt 0.879159 0.140026
+vt 0.896695 0.118860
+vt 0.898202 0.123877
+vt 0.895355 0.118422
+vt 0.896169 0.118193
+vt 0.863717 0.142542
+vt 0.889642 0.122199
+vt 0.891100 0.121679
+vt 0.899083 0.125135
+vt 0.805981 0.112414
+vt 0.805981 0.131533
+vt 0.800114 0.131533
+vt 0.800114 0.112414
+vt 0.807699 0.112414
+vt 0.807699 0.131533
+vt 0.884410 0.122387
+vt 0.895707 0.139551
+vt 0.879086 0.112414
+vt 0.879086 0.131533
+vt 0.883766 0.134776
+vt 0.884979 0.130164
+vt 0.903027 0.128111
+vt 0.799027 0.131533
+vt 0.799027 0.112414
+vt 0.884945 0.128175
+vt 0.882437 0.147351
+vt 0.881976 0.149022
+vt 0.883261 0.147612
+vt 0.882837 0.149234
+vt 0.898778 0.123306
+vt 0.899783 0.124719
+vt 0.899515 0.122813
+vt 0.900468 0.124192
+vt 0.899922 0.133113
+vt 0.898965 0.134558
+vt 0.900624 0.133616
+vt 0.899718 0.135026
+vt 0.878303 0.138385
+vt 0.879696 0.139416
+vt 0.878850 0.137688
+vt 0.880184 0.138702
+vt 0.889274 0.121462
+vt 0.890920 0.120885
+vt 0.889004 0.120636
+vt 0.890592 0.120060
+vt 0.875420 0.153985
+vt 0.873676 0.154006
+vt 0.875394 0.154873
+vt 0.873704 0.154874
+vt 0.891195 0.137238
+vt 0.889532 0.136718
+vt 0.890894 0.138073
+vt 0.889289 0.137551
+vt 0.868700 0.139582
+vt 0.870085 0.138524
+vt 0.868187 0.138882
+vt 0.869517 0.137842
+vt 0.862942 0.143128
+vt 0.862743 0.142252
+vt 0.862000 0.143160
+vt 0.861880 0.141993
+vt 0.883020 0.136246
+vt 0.882908 0.135355
+vt 0.882298 0.136785
+vt 0.882012 0.135647
+vt 0.863391 0.141636
+vt 0.862626 0.141085
+vt 0.883840 0.136603
+vt 0.883310 0.137383
+vt 0.897417 0.140057
+vt 0.896867 0.140740
+vt 0.898025 0.140801
+vt 0.897174 0.141610
+vt 0.874217 0.133293
+vt 0.874968 0.133747
+vt 0.874208 0.132371
+vt 0.875284 0.132840
+vt 0.896006 0.140567
+vt 0.896002 0.141529
+vt 0.873468 0.133750
+vt 0.873134 0.132848
+vt 0.895621 0.117396
+vt 0.896476 0.117194
+vt 0.895584 0.116434
+vt 0.896753 0.116313
+vt 0.881903 0.155553
+vt 0.881031 0.155656
+vt 0.882465 0.156286
+vt 0.881325 0.156572
+vt 0.897048 0.117859
+vt 0.897631 0.117095
+vt 0.882214 0.154732
+vt 0.883023 0.155251
+vt 0.882691 0.123051
+vt 0.882773 0.122155
+vt 0.881783 0.122789
+vt 0.882031 0.121638
+vt 0.867329 0.155578
+vt 0.866946 0.154764
+vt 0.866810 0.156317
+vt 0.866182 0.155321
+vt 0.883582 0.121769
+vt 0.883025 0.121005
+vt 0.868222 0.155664
+vt 0.867960 0.156572
+vt 0.904030 0.128049
+vt 0.904478 0.128840
+vt 0.904906 0.127731
+vt 0.905368 0.128825
+vt 0.885439 0.141872
+vt 0.885289 0.142769
+vt 0.886281 0.141582
+vt 0.886221 0.142768
+vt 0.904056 0.129646
+vt 0.904943 0.129934
+vt 0.884767 0.141260
+vt 0.885502 0.140686
+vt 0.865721 0.149838
+vt 0.865007 0.147749
+vt 0.865827 0.147457
+vt 0.866565 0.149562
+vt 0.883268 0.130306
+vt 0.883225 0.128099
+vt 0.884112 0.128070
+vt 0.884138 0.130300
+vn 0.0000 -1.0000 0.0000
+vn 0.1760 -0.1821 0.9674
+vn 0.1760 0.1821 0.9674
+vn 0.1373 0.1995 0.9702
+vn 0.1373 -0.1995 0.9702
+vn -0.0000 1.0000 -0.0000
+vn 0.8706 -0.1992 0.4499
+vn 0.8706 0.1992 0.4499
+vn 0.8522 0.1751 0.4930
+vn 0.8522 -0.1751 0.4930
+vn -0.9656 -0.1964 0.1706
+vn -0.9656 0.1964 0.1706
+vn -0.9767 0.1745 0.1252
+vn -0.9767 -0.1745 0.1252
+vn -0.4160 0.1947 -0.8883
+vn -0.4160 -0.1947 -0.8883
+vn -0.9185 -0.2428 -0.3119
+vn -0.9185 0.2428 -0.3119
+vn 0.9185 -0.2429 -0.3119
+vn 0.9185 0.2429 -0.3119
+vn -0.1372 -0.1993 0.9703
+vn -0.1372 0.1993 0.9703
+vn -0.1761 0.1819 0.9674
+vn -0.1761 -0.1819 0.9674
+vn 0.9766 -0.1749 0.1252
+vn 0.9766 0.1749 0.1252
+vn 0.9655 0.1968 0.1705
+vn 0.9655 -0.1968 0.1705
+vn -0.8523 -0.1748 0.4930
+vn -0.8523 0.1748 0.4930
+vn 0.7051 0.1756 -0.6870
+vn 0.7051 -0.1756 -0.6870
+vn 0.6736 -0.1964 -0.7125
+vn 0.6736 0.1964 -0.7125
+vn 0.4161 0.1947 -0.8883
+vn 0.4161 -0.1947 -0.8883
+vn 0.3770 -0.1709 -0.9103
+vn 0.3770 0.1709 -0.9103
+vn -0.8706 -0.1990 0.4499
+vn -0.8706 0.1990 0.4499
+vn 0.5621 0.2389 0.7918
+vn 0.5621 -0.2389 0.7918
+vn -0.5620 0.2388 0.7919
+vn -0.5620 -0.2388 0.7919
+vn -0.7052 -0.1752 -0.6870
+vn -0.7052 0.1752 -0.6870
+vn -0.6736 0.1961 -0.7126
+vn -0.6736 -0.1961 -0.7126
+vn -0.3770 -0.1709 -0.9103
+vn -0.3770 0.1709 -0.9103
+vn 0.0001 -0.2357 -0.9718
+vn 0.0001 0.2357 -0.9718
+vn 0.0822 -0.9807 -0.1776
+vn 0.1339 -0.9813 -0.1382
+vn 0.5148 -0.6952 -0.5016
+vn 0.2776 -0.6869 -0.6716
+vn 0.1339 0.9813 -0.1382
+vn 0.0822 0.9807 -0.1776
+vn 0.2776 0.6869 -0.6716
+vn 0.5148 0.6952 -0.5016
+vn -0.0822 0.9807 -0.1777
+vn -0.1341 0.9812 -0.1385
+vn -0.5153 0.6946 -0.5020
+vn -0.2777 0.6868 -0.6717
+vn -0.1341 -0.9812 -0.1385
+vn -0.0822 -0.9807 -0.1777
+vn -0.2777 -0.6868 -0.6717
+vn -0.5153 -0.6946 -0.5020
+vn 0.1701 0.9813 0.0905
+vn 0.1906 0.9812 0.0307
+vn 0.7146 0.6935 0.0914
+vn 0.6235 0.6939 0.3603
+vn 0.1906 -0.9812 0.0307
+vn 0.1701 -0.9813 0.0905
+vn 0.6235 -0.6939 0.3603
+vn 0.7146 -0.6935 0.0914
+vn -0.1911 0.9811 0.0308
+vn -0.1703 0.9812 0.0907
+vn -0.6239 0.6934 0.3605
+vn -0.7153 0.6928 0.0914
+vn -0.1703 -0.9812 0.0907
+vn -0.1911 -0.9811 0.0308
+vn -0.7153 -0.6928 0.0914
+vn -0.6239 -0.6934 0.3605
+vn -0.1140 -0.9807 0.1590
+vn 0.0574 -0.9732 0.2225
+vn 0.1230 -0.6920 0.7113
+vn -0.3997 -0.7242 0.5619
+vn 0.0574 0.9732 0.2225
+vn -0.1140 0.9807 0.1590
+vn -0.3997 0.7242 0.5619
+vn 0.1230 0.6920 0.7113
+vn -0.2413 -0.9704 0.0099
+vn -0.7222 -0.6844 0.1004
+vn -0.2413 0.9704 0.0099
+vn -0.7222 0.6844 0.1004
+vn -0.1821 0.9811 -0.0651
+vn -0.0726 0.9686 -0.2377
+vn -0.2844 0.6806 -0.6752
+vn -0.6483 0.7279 -0.2233
+vn -0.0726 -0.9686 -0.2377
+vn -0.1821 -0.9811 -0.0651
+vn -0.6483 -0.7279 -0.2233
+vn -0.2844 -0.6806 -0.6752
+vn -0.1966 0.9707 0.1379
+vn -0.6321 0.6875 0.3576
+vn -0.1966 -0.9707 0.1379
+vn -0.6321 -0.6875 0.3576
+vn 0.1820 0.9811 -0.0652
+vn 0.1964 0.9708 0.1377
+vn 0.6318 0.6878 0.3574
+vn 0.6481 0.7280 -0.2234
+vn 0.1964 -0.9708 0.1377
+vn 0.1820 -0.9811 -0.0652
+vn 0.6481 -0.7280 -0.2234
+vn 0.6318 -0.6878 0.3574
+vn 0.0726 0.9686 -0.2378
+vn 0.2843 0.6806 -0.6752
+vn 0.0726 -0.9686 -0.2378
+vn 0.2843 -0.6806 -0.6752
+vn 0.1137 0.9807 0.1592
+vn -0.0576 0.9731 0.2229
+vn -0.1231 0.6917 0.7116
+vn 0.3994 0.7243 0.5620
+vn -0.0576 -0.9731 0.2229
+vn 0.1137 -0.9807 0.1592
+vn 0.3994 -0.7243 0.5620
+vn -0.1231 -0.6917 0.7116
+vn 0.2406 0.9706 0.0100
+vn 0.7217 0.6849 0.1003
+vn 0.2407 -0.9706 0.0100
+vn 0.7217 -0.6849 0.1003
+vn -0.0002 0.9803 -0.1975
+vn 0.1845 0.9711 -0.1512
+vn 0.5181 0.6852 -0.5119
+vn -0.0002 0.7214 -0.6926
+vn 0.1845 -0.9711 -0.1512
+vn -0.0002 -0.9803 -0.1975
+vn -0.0002 -0.7214 -0.6926
+vn 0.5181 -0.6852 -0.5119
+vn -0.1850 0.9710 -0.1515
+vn -0.5185 0.6847 -0.5122
+vn -0.1850 -0.9710 -0.1515
+vn -0.5185 -0.6847 -0.5122
+vn 0.1263 -0.7034 0.6995
+vn -0.1265 -0.7031 0.6997
+vn 0.0277 -0.9822 0.1859
+vn -0.0277 -0.9821 0.1861
+vn -0.1265 0.7031 0.6997
+vn 0.1263 0.7034 0.6995
+vn -0.0277 0.9821 0.1861
+vn 0.0277 0.9822 0.1859
+usemtl toybox.002
+s 1
+f 136/1/1 3/2/1 1/3/1
+f 31/4/1 69/5/1 1/3/1
+f 144/6/2 148/7/3 63/8/4 52/9/5
+f 96/10/6 103/11/6 2/12/6
+f 7/13/1 105/14/1 1/3/1
+f 19/15/6 78/16/6 2/12/6
+f 43/17/1 87/18/1 1/3/1
+f 127/19/6 15/20/6 2/12/6
+f 15/20/6 19/15/6 2/12/6
+f 108/21/7 115/22/8 48/23/9 40/24/10
+f 54/25/11 61/26/12 36/27/13 28/28/14
+f 73/29/1 21/30/1 1/3/1
+f 109/31/1 39/32/1 1/3/1
+f 1/3/1 145/33/1 51/34/1
+f 60/35/6 66/36/6 2/12/6
+f 75/37/1 73/29/1 1/3/1
+f 79/38/15 72/39/16 76/40/17 84/41/18
+f 111/42/1 109/31/1 1/3/1
+f 115/22/8 108/21/7 112/43/19 120/44/20
+f 93/45/1 91/46/1 1/3/1
+f 90/47/21 97/48/22 150/49/23 142/50/24
+f 130/51/6 127/19/6 2/12/6
+f 132/52/1 138/53/1 1/3/1
+f 55/54/1 27/55/1 1/3/1
+f 42/56/25 46/57/26 99/58/27 88/59/28
+f 78/16/6 85/60/6 2/12/6
+f 91/46/1 141/61/1 1/3/1
+f 69/5/1 75/37/1 1/3/1
+f 30/62/29 28/28/14 36/27/13 34/63/30
+f 12/64/31 4/65/32 135/66/33 124/67/34
+f 117/68/35 106/69/36 6/70/37 10/71/38
+f 84/41/18 76/40/17 70/72/39 81/73/40
+f 33/74/6 37/75/6 2/12/6
+f 42/56/25 40/24/10 48/23/9 46/57/26
+f 102/76/41 94/77/42 88/59/28 99/58/27
+f 31/4/1 1/3/1 27/55/1
+f 52/9/5 63/8/4 67/78/43 57/79/44
+f 25/80/1 132/52/1 1/3/1
+f 118/81/6 9/82/6 2/12/6
+f 114/83/6 121/84/6 2/12/6
+f 58/85/1 55/54/1 1/3/1
+f 148/7/3 144/6/2 142/50/24 150/49/23
+f 57/79/44 67/78/43 61/26/12 54/25/11
+f 45/86/6 49/87/6 2/12/6
+f 120/44/20 112/43/19 106/69/36 117/68/35
+f 138/53/1 136/1/1 1/3/1
+f 9/82/6 13/88/6 2/12/6
+f 30/62/29 34/63/30 81/73/40 70/72/39
+f 39/32/1 43/17/1 1/3/1
+f 51/34/1 58/85/1 1/3/1
+f 24/89/45 16/90/46 126/91/47 133/92/48
+f 24/89/45 22/93/49 18/94/50 16/90/46
+f 87/18/1 93/45/1 1/3/1
+f 103/11/6 100/95/6 2/12/6
+f 105/14/1 111/42/1 1/3/1
+f 82/96/6 33/74/6 2/12/6
+f 37/75/6 60/35/6 2/12/6
+f 72/39/16 79/38/15 18/94/50 22/93/49
+f 121/84/6 118/81/6 2/12/6
+f 100/95/6 45/86/6 2/12/6
+f 124/67/34 135/66/33 139/97/51 129/98/52
+f 25/80/1 1/3/1 21/30/1
+f 64/99/6 147/100/6 2/12/6
+f 123/101/6 130/51/6 2/12/6
+f 66/36/6 64/99/6 2/12/6
+f 129/102/52 139/103/51 133/92/48 126/91/47
+f 49/87/6 114/83/6 2/12/6
+f 13/88/6 123/101/6 2/12/6
+f 85/60/6 82/96/6 2/12/6
+f 2/12/6 151/104/6 96/10/6
+f 3/2/1 7/13/1 1/3/1
+f 147/100/6 151/104/6 2/12/6
+f 141/61/1 145/33/1 1/3/1
+f 97/48/22 90/47/21 94/77/42 102/76/41
+f 7/13/53 3/2/54 5/105/55 8/106/56
+f 8/106/56 5/105/55 4/107/32 6/108/37
+f 13/88/57 9/82/58 11/109/59 14/110/60
+f 14/110/60 11/109/59 10/111/38 12/112/31
+f 19/15/61 15/20/62 17/113/63 20/114/64
+f 20/114/64 17/113/63 16/115/46 18/116/50
+f 25/80/65 21/30/66 23/117/67 26/118/68
+f 26/118/68 23/117/67 22/119/49 24/120/45
+f 49/87/69 45/86/70 47/121/71 50/122/72
+f 50/122/72 47/121/71 46/123/26 48/124/9
+f 43/17/73 39/32/74 41/125/75 44/126/76
+f 44/126/76 41/125/75 40/127/10 42/128/25
+f 37/75/77 33/74/78 35/129/79 38/130/80
+f 38/130/80 35/129/79 34/131/30 36/132/13
+f 31/4/81 27/55/82 29/133/83 32/134/84
+f 32/134/84 29/133/83 28/135/14 30/136/29
+f 58/85/85 51/34/86 53/137/87 59/138/88
+f 59/138/88 53/137/87 52/139/5 57/140/44
+f 64/99/89 66/36/90 68/141/91 65/142/92
+f 65/142/92 68/141/91 67/143/43 63/144/4
+f 55/54/93 58/85/85 59/138/88 56/145/94
+f 56/145/94 59/138/88 57/140/44 54/146/11
+f 66/36/90 60/35/95 62/147/96 68/141/91
+f 68/141/91 62/147/96 61/148/12 67/143/43
+f 85/60/97 78/16/98 80/149/99 86/150/100
+f 86/150/100 80/149/99 79/151/15 84/152/18
+f 73/29/101 75/37/102 77/153/103 74/154/104
+f 74/154/104 77/153/103 76/155/17 72/156/16
+f 82/96/105 85/60/97 86/150/100 83/157/106
+f 83/157/106 86/150/100 84/152/18 81/158/40
+f 75/37/102 69/5/107 71/159/108 77/153/103
+f 77/153/103 71/159/108 70/160/39 76/155/17
+f 121/84/109 114/83/110 116/161/111 122/162/112
+f 122/162/112 116/161/111 115/163/8 120/164/20
+f 109/31/113 111/42/114 113/165/115 110/166/116
+f 110/166/116 113/165/115 112/167/19 108/168/7
+f 118/81/117 121/84/109 122/162/112 119/169/118
+f 119/169/118 122/162/112 120/164/20 117/170/35
+f 111/42/114 105/14/119 107/171/120 113/165/115
+f 113/165/115 107/171/120 106/172/36 112/167/19
+f 103/11/121 96/10/122 98/173/123 104/174/124
+f 104/174/124 98/173/123 97/175/22 102/176/41
+f 91/46/125 93/45/126 95/177/127 92/178/128
+f 92/178/128 95/177/127 94/179/42 90/180/21
+f 100/95/129 103/11/121 104/174/124 101/181/130
+f 101/181/130 104/174/124 102/176/41 99/182/27
+f 93/45/126 87/18/131 89/183/132 95/177/127
+f 95/177/127 89/183/132 88/184/28 94/179/42
+f 130/51/133 123/101/134 125/185/135 131/186/136
+f 131/186/136 125/185/135 124/187/34 129/188/52
+f 136/1/137 138/53/138 140/189/139 137/190/140
+f 137/190/140 140/189/139 139/191/51 135/192/33
+f 127/19/141 130/51/133 131/186/136 128/193/142
+f 128/193/142 131/186/136 129/188/52 126/194/47
+f 138/53/138 132/52/143 134/195/144 140/189/139
+f 140/189/139 134/195/144 133/196/48 139/191/51
+f 27/55/82 55/54/93 56/145/94 29/133/83
+f 29/133/83 56/145/94 54/146/11 28/135/14
+f 60/35/95 37/75/77 38/130/80 62/147/96
+f 62/147/96 38/130/80 36/132/13 61/148/12
+f 69/5/107 31/4/81 32/134/84 71/159/108
+f 71/159/108 32/134/84 30/136/29 70/160/39
+f 33/74/78 82/96/105 83/157/106 35/129/79
+f 35/129/79 83/157/106 81/158/40 34/131/30
+f 21/30/66 73/29/101 74/154/104 23/117/67
+f 23/117/67 74/154/104 72/156/16 22/119/49
+f 78/16/98 19/15/61 20/114/64 80/149/99
+f 80/149/99 20/114/64 18/116/50 79/151/15
+f 87/18/131 43/17/73 44/126/76 89/183/132
+f 89/183/132 44/126/76 42/128/25 88/184/28
+f 45/86/70 100/95/129 101/181/130 47/121/71
+f 47/121/71 101/181/130 99/182/27 46/123/26
+f 9/82/58 118/81/117 119/169/118 11/109/59
+f 11/109/59 119/169/118 117/170/35 10/111/38
+f 105/14/119 7/13/53 8/106/56 107/171/120
+f 107/171/120 8/106/56 6/108/37 106/172/36
+f 39/32/74 109/31/113 110/166/116 41/125/75
+f 41/125/75 110/166/116 108/168/7 40/127/10
+f 114/83/110 49/87/69 50/122/72 116/161/111
+f 116/161/111 50/122/72 48/124/9 115/163/8
+f 123/101/134 13/88/57 14/110/60 125/185/135
+f 125/185/135 14/110/60 12/112/31 124/187/34
+f 3/2/54 136/1/137 137/190/140 5/105/55
+f 5/105/55 137/190/140 135/192/33 4/107/32
+f 132/52/143 25/80/65 26/118/68 134/195/144
+f 134/195/144 26/118/68 24/120/45 133/196/48
+f 15/20/62 127/19/141 128/193/142 17/113/63
+f 17/113/63 128/193/142 126/194/47 16/115/46
+f 142/197/24 144/198/2 146/199/145 143/200/146
+f 143/200/146 146/199/145 145/33/147 141/61/148
+f 148/201/3 150/202/23 152/203/149 149/204/150
+f 149/204/150 152/203/149 151/104/151 147/100/152
+f 141/61/148 91/46/125 92/178/128 143/200/146
+f 143/200/146 92/178/128 90/180/21 142/197/24
+f 96/10/122 151/104/151 152/203/149 98/173/123
+f 98/173/123 152/203/149 150/202/23 97/175/22
+f 51/34/86 145/33/147 146/199/145 53/137/87
+f 53/137/87 146/199/145 144/198/2 52/139/5
+f 147/100/152 64/99/89 65/142/92 149/204/150
+f 149/204/150 65/142/92 63/144/4 148/201/3
+f 12/64/31 10/71/38 6/70/37 4/65/32
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/yellow_pentagon.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/yellow_pentagon.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..4f5bdf6d81d549ab3c5887bca44f658886b92a8f
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/yellow_pentagon.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/insert.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/insert.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..d912ff13b629eee1239cfe5b29f9d92d0d0eb30e
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/insert.urdf
@@ -0,0 +1,66 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/plane.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/plane.obj
new file mode 100644
index 0000000000000000000000000000000000000000..6062095314e3d3f5b9da26a580ca2e6ee14623e6
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/plane.obj
@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000 -15.000000 0.000000
+
+vt 15.000000 0.000000
+vt 15.000000 15.000000
+vt 0.000000 15.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/base.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/base.obj
new file mode 100644
index 0000000000000000000000000000000000000000..bdf1bb330924fd70e004c9aa62d8021cb7cb50e8
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/base.obj
@@ -0,0 +1,396 @@
+# Object Export From Tinkercad Server 2015
+
+mtllib obj.mtl
+
+o obj_0
+v 7.413 37.27 25
+v 7.413 37.27 0
+v 33.513 17.913 25
+v 35.107 14.542 25
+v 35.107 14.542 0
+v 33.513 17.913 0
+v -31.596 21.112 25
+v -31.596 21.112 0
+v -33.513 17.913 0
+v -33.513 17.913 25
+v 3.725 37.817 0
+v 3.725 37.817 25
+v -29.374 24.107 25
+v -29.374 24.107 0
+v 0 38 0
+v 11.031 36.364 25
+v 11.031 36.364 0
+v 14.542 35.107 25
+v 14.542 35.107 0
+v -37.27 -7.413 25
+v 17.913 33.513 25
+v -37.27 -7.413 0
+v -11.031 -36.364 25
+v 17.913 33.513 0
+v -36.364 -11.031 0
+v -14.542 -35.107 25
+v -26.87 26.87 25
+v -36.364 -11.031 25
+v -14.542 -35.107 0
+v -26.87 26.87 0
+v -3.725 37.817 0
+v -11.031 -36.364 0
+v 24.107 -29.374 25
+v -17.913 -33.513 25
+v -17.913 -33.513 0
+v 21.112 -31.596 25
+v 21.112 -31.596 0
+v 24.107 -29.374 0
+v -3.725 37.817 25
+v 0 38 25
+v 17.913 -33.513 25
+v 17.913 -33.513 0
+v 37.817 -3.725 25
+v -37.817 -3.725 25
+v -37.817 -3.725 0
+v 37.817 -3.725 0
+v 38 0 0
+v -21.112 -31.596 25
+v 38 0 25
+v 14.542 -35.107 25
+v -21.112 -31.596 0
+v 36.364 11.031 25
+v 14.542 -35.107 0
+v 36.364 11.031 0
+v 37.27 -7.413 25
+v 37.27 -7.413 0
+v -24.107 29.374 25
+v 21.112 31.596 25
+v -24.107 29.374 0
+v 36.364 -11.031 25
+v 21.112 31.596 0
+v 36.364 -11.031 0
+v -38 0 25
+v 11.031 -36.364 25
+v 11.031 -36.364 0
+v -21.112 31.596 25
+v -21.112 31.596 0
+v 7.413 -37.27 25
+v 7.413 -37.27 0
+v -17.913 33.513 25
+v -17.913 33.513 0
+v 24.107 29.374 0
+v -38 0 0
+v 26.87 26.87 0
+v 35.107 -14.542 25
+v 29.374 24.107 0
+v 35.107 -14.542 0
+v 31.596 21.112 0
+v 3.725 -37.817 25
+v 37.27 7.413 25
+v 3.725 -37.817 0
+v 24.107 29.374 25
+v -24.107 -29.374 25
+v -14.542 35.107 25
+v -24.107 -29.374 0
+v -14.542 35.107 0
+v 37.27 7.413 0
+v 33.513 -17.913 25
+v 37.817 3.725 0
+v 33.513 -17.913 0
+v -11.031 36.364 25
+v -11.031 36.364 0
+v -7.413 37.27 25
+v 26.87 26.87 25
+v -7.413 37.27 0
+v -26.87 -26.87 25
+v -26.87 -26.87 0
+v 37.817 3.725 25
+v 0 -38 25
+v 31.596 -21.112 25
+v 0 -38 0
+v 31.596 -21.112 0
+v 29.374 24.107 25
+v -29.374 -24.107 25
+v -29.374 -24.107 0
+v 29.374 -24.107 25
+v 31.596 21.112 25
+v 29.374 -24.107 0
+v -3.725 -37.817 25
+v -31.596 -21.112 25
+v -3.725 -37.817 0
+v -31.596 -21.112 0
+v -37.817 3.725 25
+v -37.817 3.725 0
+v 26.87 -26.87 25
+v 26.87 -26.87 0
+v -37.27 7.413 0
+v -37.27 7.413 25
+v -7.413 -37.27 25
+v -33.513 -17.913 25
+v -33.513 -17.913 0
+v -7.413 -37.27 0
+v -36.364 11.031 25
+v -36.364 11.031 0
+v -35.107 -14.542 25
+v -35.107 -14.542 0
+v -35.107 14.542 25
+v -35.107 14.542 0
+# 128 vertices
+
+g group_0_2829873
+
+usemtl color_2829873
+s 0
+
+f 3 4 5
+f 3 5 6
+f 7 8 9
+f 7 9 10
+f 1 2 11
+f 1 11 12
+f 16 17 2
+f 16 2 1
+f 13 14 8
+f 13 8 7
+f 18 19 17
+f 18 17 16
+f 23 26 29
+f 23 29 32
+f 26 34 35
+f 26 35 29
+f 33 36 37
+f 33 37 38
+f 15 31 39
+f 15 39 40
+f 20 22 25
+f 20 25 28
+f 36 41 42
+f 36 42 37
+f 44 45 22
+f 44 22 20
+f 21 24 19
+f 21 19 18
+f 43 46 47
+f 43 47 49
+f 27 30 14
+f 27 14 13
+f 41 50 53
+f 41 53 42
+f 43 55 56
+f 43 56 46
+f 34 48 51
+f 34 51 35
+f 4 52 54
+f 4 54 5
+f 50 64 65
+f 50 65 53
+f 57 59 30
+f 57 30 27
+f 55 60 62
+f 55 62 56
+f 66 67 59
+f 66 59 57
+f 64 68 69
+f 64 69 65
+f 58 61 24
+f 58 24 21
+f 63 73 45
+f 63 45 44
+f 70 71 67
+f 70 67 66
+f 60 75 77
+f 60 77 62
+f 82 72 61
+f 82 61 58
+f 68 79 81
+f 68 81 69
+f 48 83 85
+f 48 85 51
+f 84 86 71
+f 84 71 70
+f 75 88 90
+f 75 90 77
+f 91 92 86
+f 91 86 84
+f 15 11 2
+f 15 2 17
+f 15 17 19
+f 15 19 24
+f 15 24 61
+f 15 61 72
+f 15 72 74
+f 15 74 76
+f 15 76 78
+f 15 78 6
+f 15 6 5
+f 15 5 54
+f 15 54 87
+f 15 87 89
+f 15 89 47
+f 52 80 87
+f 52 87 54
+f 94 74 72
+f 94 72 82
+f 93 95 92
+f 93 92 91
+f 83 96 97
+f 83 97 85
+f 80 98 89
+f 80 89 87
+f 79 99 101
+f 79 101 81
+f 88 100 102
+f 88 102 90
+f 94 103 76
+f 94 76 74
+f 39 31 95
+f 39 95 93
+f 104 105 97
+f 104 97 96
+f 98 49 47
+f 98 47 89
+f 100 106 108
+f 100 108 102
+f 103 107 78
+f 103 78 76
+f 101 99 109
+f 101 109 111
+f 110 112 105
+f 110 105 104
+f 113 114 73
+f 113 73 63
+f 106 115 116
+f 106 116 108
+f 107 3 6
+f 107 6 78
+f 118 117 114
+f 118 114 113
+f 120 121 112
+f 120 112 110
+f 109 119 122
+f 109 122 111
+f 123 124 117
+f 123 117 118
+f 125 126 121
+f 125 121 120
+f 127 128 124
+f 127 124 123
+f 115 33 38
+f 115 38 116
+f 10 9 128
+f 10 128 127
+f 28 25 126
+f 28 126 125
+f 119 23 32
+f 119 32 122
+f 73 114 117
+f 73 117 124
+f 73 124 128
+f 73 128 9
+f 73 9 8
+f 73 8 14
+f 73 14 30
+f 73 30 59
+f 73 59 67
+f 73 67 71
+f 73 71 86
+f 73 86 92
+f 73 92 95
+f 73 95 31
+f 73 31 15
+f 81 101 15
+f 47 46 15
+f 46 56 15
+f 56 62 15
+f 62 77 15
+f 77 90 15
+f 90 102 15
+f 102 108 15
+f 108 116 15
+f 116 38 15
+f 38 37 15
+f 37 42 15
+f 42 53 15
+f 53 65 15
+f 69 15 65
+f 81 15 69
+f 45 73 15
+f 101 111 15
+f 111 122 15
+f 122 32 15
+f 32 29 15
+f 29 35 15
+f 35 51 15
+f 51 85 15
+f 85 97 15
+f 97 105 15
+f 105 112 15
+f 112 121 15
+f 121 126 15
+f 126 25 15
+f 22 15 25
+f 45 15 22
+f 49 98 80
+f 49 80 52
+f 49 52 4
+f 49 4 3
+f 49 3 107
+f 49 107 103
+f 49 103 94
+f 49 94 82
+f 49 82 58
+f 49 58 21
+f 49 21 18
+f 49 18 16
+f 49 16 1
+f 49 1 12
+f 49 12 40
+f 40 39 93
+f 40 93 91
+f 40 91 84
+f 40 84 70
+f 40 70 66
+f 40 66 57
+f 40 57 27
+f 40 27 13
+f 40 13 7
+f 40 7 10
+f 40 10 127
+f 40 127 123
+f 40 123 118
+f 40 118 113
+f 63 40 113
+f 68 40 79
+f 64 40 68
+f 50 40 64
+f 41 40 50
+f 36 40 41
+f 33 40 36
+f 115 40 33
+f 106 40 115
+f 100 40 106
+f 88 40 100
+f 75 40 88
+f 60 40 75
+f 55 40 60
+f 43 40 55
+f 49 40 43
+f 99 79 40
+f 20 40 44
+f 28 40 20
+f 125 40 28
+f 120 40 125
+f 110 40 120
+f 104 40 110
+f 96 40 104
+f 83 40 96
+f 48 40 83
+f 34 40 48
+f 26 40 34
+f 23 40 26
+f 119 40 23
+f 109 40 119
+f 99 40 109
+f 63 44 40
+f 12 11 15
+f 12 15 40
+# 252 faces
+
+ #end of obj_0
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..d5779f3f9afc9637b2a04625ad9609798261639c
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder.urdf
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder_real.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder_real.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..b4bb8b7818a7740e334adc140dfaaa03a6ce7f7b
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder_real.urdf
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/head.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/head.obj
new file mode 100644
index 0000000000000000000000000000000000000000..49cefdd6d79e08c927efab45e16dd7a4f015f056
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/head.obj
@@ -0,0 +1,396 @@
+# Object Export From Tinkercad Server 2015
+
+mtllib obj.mtl
+
+o obj_0
+v 0.001 5.001 0
+v 0.492 4.977 0
+v 0.977 4.905 0
+v 1.453 4.786 0
+v 1.915 4.621 0
+v 2.358 4.411 0
+v 2.779 4.159 0
+v 3.173 3.866 0
+v 3.537 3.537 0
+v 3.866 3.173 0
+v 4.159 2.779 0
+v 4.411 2.358 0
+v 4.621 1.915 0
+v 4.786 1.453 0
+v 4.905 0.977 0
+v 4.977 0.492 0
+v 5.001 0.001 0
+v -2.776 4.159 0
+v 4.159 -2.776 30
+v -2.356 4.411 0
+v -1.912 4.621 0
+v -1.45 4.786 0
+v -0.974 4.905 0
+v -0.489 4.977 0
+v 4.411 -2.356 30
+v 4.621 -1.912 30
+v 4.786 -1.45 30
+v -4.999 0.001 0
+v -4.974 0.492 0
+v -4.903 0.977 0
+v -4.783 1.453 0
+v -4.618 1.915 0
+v -4.408 2.358 0
+v 4.905 -0.974 30
+v -4.156 2.779 0
+v -3.864 3.173 0
+v -3.534 3.537 0
+v -3.171 3.866 0
+v 4.977 -0.489 30
+v 4.977 -0.489 0
+v 4.905 -0.974 0
+v 4.786 -1.45 0
+v 4.621 -1.912 0
+v 4.411 -2.356 0
+v 4.159 -2.776 0
+v 0.001 -4.999 30
+v 3.866 -3.171 0
+v 3.537 -3.534 0
+v 3.173 -3.864 0
+v 2.779 -4.156 0
+v 2.358 -4.408 0
+v 1.915 -4.618 0
+v 0.492 -4.974 30
+v 1.453 -4.783 0
+v 0.977 -4.902 0
+v 0.492 -4.974 0
+v 0.001 -4.999 0
+v 0.977 -4.902 30
+v 1.453 -4.783 30
+v 1.915 -4.618 30
+v -3.171 -3.864 0
+v -3.534 -3.534 0
+v -3.864 -3.171 0
+v -4.156 -2.776 0
+v -4.408 -2.356 0
+v -4.618 -1.912 0
+v -4.783 -1.45 0
+v -4.903 -0.974 0
+v -4.974 -0.489 0
+v 2.358 -4.408 30
+v 2.779 -4.156 30
+v -0.489 -4.974 0
+v -0.974 -4.902 0
+v -1.45 -4.783 0
+v -1.912 -4.618 0
+v -2.356 -4.408 0
+v -2.776 -4.156 0
+v 3.173 -3.864 30
+v 3.537 -3.534 30
+v 0.001 5.001 30
+v 2.779 4.159 30
+v 2.358 4.411 30
+v 1.915 4.621 30
+v 3.866 -3.171 30
+v 1.453 4.786 30
+v 0.977 4.905 30
+v 0.492 4.977 30
+v 5.001 0.001 30
+v 4.977 0.492 30
+v 4.905 0.977 30
+v 4.786 1.453 30
+v 4.621 1.915 30
+v 4.411 2.358 30
+v 4.159 2.779 30
+v 3.866 3.173 30
+v 3.537 3.537 30
+v 3.173 3.866 30
+v -0.489 4.977 30
+v -0.974 4.905 30
+v -1.45 4.786 30
+v -1.912 4.621 30
+v -2.356 4.411 30
+v -2.776 4.159 30
+v -3.171 3.866 30
+v -3.534 3.537 30
+v -3.864 3.173 30
+v -4.156 2.779 30
+v -4.408 2.358 30
+v -4.618 1.915 30
+v -4.783 1.453 30
+v -4.903 0.977 30
+v -4.974 0.492 30
+v -4.999 0.001 30
+v -4.974 -0.489 30
+v -4.903 -0.974 30
+v -4.783 -1.45 30
+v -4.618 -1.912 30
+v -4.408 -2.356 30
+v -4.156 -2.776 30
+v -3.864 -3.171 30
+v -3.534 -3.534 30
+v -3.171 -3.864 30
+v -0.489 -4.974 30
+v -2.776 -4.156 30
+v -2.356 -4.408 30
+v -1.912 -4.618 30
+v -1.45 -4.783 30
+v -0.974 -4.902 30
+# 128 vertices
+
+g group_0_16089887
+
+usemtl color_16089887
+s 0
+
+f 2 3 1
+f 1 3 4
+f 1 4 5
+f 1 5 6
+f 1 6 7
+f 1 7 8
+f 9 10 8
+f 8 10 11
+f 8 11 12
+f 8 12 13
+f 8 13 14
+f 8 14 15
+f 8 15 16
+f 8 16 17
+f 8 17 40
+f 22 8 21
+f 20 21 8
+f 23 8 22
+f 24 8 23
+f 1 8 24
+f 18 20 8
+f 31 8 30
+f 32 8 31
+f 33 8 32
+f 35 8 33
+f 29 30 8
+f 36 8 35
+f 37 8 36
+f 38 8 37
+f 18 8 38
+f 28 29 8
+f 44 8 43
+f 42 43 8
+f 41 42 8
+f 45 8 44
+f 47 8 45
+f 50 8 49
+f 51 8 50
+f 48 49 8
+f 52 8 51
+f 54 8 52
+f 55 8 54
+f 56 8 55
+f 57 8 56
+f 72 8 57
+f 47 48 8
+f 40 41 8
+f 64 8 63
+f 65 8 64
+f 66 8 65
+f 62 63 8
+f 67 8 66
+f 68 8 67
+f 69 8 68
+f 28 8 69
+f 61 62 8
+f 72 73 8
+f 75 8 74
+f 73 74 8
+f 76 8 75
+f 77 8 76
+f 61 8 77
+f 98 82 80
+f 80 83 85
+f 80 85 86
+f 80 86 87
+f 19 45 44
+f 19 44 25
+f 43 26 44
+f 43 42 27
+f 42 41 27
+f 89 90 88
+f 88 90 91
+f 88 91 92
+f 88 92 93
+f 88 93 94
+f 88 94 95
+f 96 97 88
+f 97 81 88
+f 82 88 81
+f 95 96 88
+f 114 115 82
+f 25 44 26
+f 34 27 41
+f 43 27 26
+f 34 41 40
+f 99 82 98
+f 100 82 99
+f 101 82 100
+f 102 82 101
+f 103 82 102
+f 83 80 82
+f 55 58 53
+f 34 40 39
+f 58 55 54
+f 82 103 104
+f 82 104 105
+f 82 105 106
+f 82 106 107
+f 82 107 108
+f 82 108 109
+f 82 109 110
+f 82 110 111
+f 82 111 112
+f 82 112 113
+f 82 113 114
+f 56 53 57
+f 53 56 55
+f 39 88 82
+f 19 25 82
+f 25 26 82
+f 27 82 26
+f 34 82 27
+f 39 82 34
+f 57 53 46
+f 70 82 60
+f 59 60 82
+f 58 59 82
+f 71 82 70
+f 53 58 82
+f 78 82 71
+f 79 82 78
+f 84 82 79
+f 19 82 84
+f 46 53 82
+f 122 124 82
+f 71 50 78
+f 54 59 58
+f 5 83 82
+f 71 51 50
+f 48 78 49
+f 59 54 60
+f 52 60 54
+f 5 85 83
+f 49 78 50
+f 52 51 60
+f 70 60 51
+f 116 82 115
+f 117 82 116
+f 118 82 117
+f 119 82 118
+f 120 82 119
+f 121 82 120
+f 122 82 121
+f 85 4 3
+f 71 70 51
+f 46 82 123
+f 125 126 82
+f 123 82 128
+f 127 82 126
+f 128 82 127
+f 124 125 82
+f 48 79 78
+f 24 98 80
+f 48 47 79
+f 84 79 47
+f 16 89 88
+f 84 47 19
+f 16 90 89
+f 95 94 11
+f 14 91 90
+f 82 7 6
+f 92 91 13
+f 12 92 13
+f 80 1 24
+f 8 97 96
+f 76 75 125
+f 57 46 72
+f 126 75 127
+f 72 123 128
+f 46 123 72
+f 81 97 7
+f 7 82 81
+f 69 68 114
+f 85 3 86
+f 115 67 116
+f 117 116 66
+f 86 3 87
+f 2 87 3
+f 37 105 104
+f 116 67 66
+f 2 80 87
+f 121 120 62
+f 66 65 118
+f 66 118 117
+f 121 62 122
+f 61 122 62
+f 65 119 118
+f 120 119 63
+f 107 35 33
+f 115 114 68
+f 122 61 124
+f 120 63 62
+f 68 67 115
+f 14 13 91
+f 77 124 61
+f 113 29 28
+f 77 76 124
+f 125 124 76
+f 12 93 92
+f 126 125 75
+f 119 65 64
+f 113 28 114
+f 69 114 28
+f 127 75 74
+f 112 111 29
+f 94 93 12
+f 74 73 127
+f 64 63 119
+f 128 73 72
+f 112 29 113
+f 104 18 38
+f 104 38 37
+f 98 24 23
+f 8 7 97
+f 35 107 106
+f 40 88 39
+f 110 109 31
+f 101 21 20
+f 128 127 73
+f 98 23 99
+f 23 100 99
+f 6 5 82
+f 101 20 102
+f 85 5 4
+f 20 18 103
+f 20 103 102
+f 18 104 103
+f 1 80 2
+f 37 106 105
+f 107 33 108
+f 33 109 108
+f 88 17 16
+f 15 90 16
+f 15 14 90
+f 94 12 11
+f 10 95 11
+f 95 10 96
+f 9 96 10
+f 9 8 96
+f 40 17 88
+f 100 22 101
+f 109 33 32
+f 32 31 109
+f 111 110 30
+f 31 30 110
+f 111 30 29
+f 100 23 22
+f 21 101 22
+f 37 36 106
+f 36 35 106
+f 19 47 45
+# 252 faces
+
+ #end of obj_0
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/mid.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/mid.obj
new file mode 100644
index 0000000000000000000000000000000000000000..fdf238e4f38792d905ce57d96d1afe4281302472
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/mid.obj
@@ -0,0 +1,2134 @@
+# Object Export From Tinkercad Server 2015
+
+mtllib obj.mtl
+
+o obj_0
+v 24.107 -29.374 0
+v 21.112 -31.596 0
+v 30.297 8.013 24.448
+v 17.913 -33.513 0
+v 26.312 25.97 20.313
+v 14.542 -35.107 0
+v 31.366 0 24.448
+v 29 0 24.7348
+v -29.4178 -24.048 15.875
+v -29.3593 -24.1232 15.875
+v -17.841 30.499 22.151
+v 23.821 -23.511 23.561
+v -9.235 34.018 22.151
+v 34.5141 -15.7959 17
+v 35.107 -14.542 17
+v 35.4949 -13.4585 17
+v 35.0704 -14.4701 17
+v 35.107 -14.542 16.5646
+v 19.086 -32.629 18.172
+v 34.7754 -15.2433 16.808
+v 36.871 -9.751 18.172
+v 0 35.218 22.151
+v 23.0418 -29.8391 17
+v 19.2451 -32.7147 17
+v 21.112 -31.596 17
+v 24.107 -29.374 17
+v 0 33.25 23.561
+v 27.224 26.87 15.875
+v -27.224 -26.87 15.875
+v -20.1874 32.1501 16.1531
+v -20.4974 31.9643 15.875
+v 38.172 0 18.172
+v 18.605 -31.807 20.313
+v -9.3502 36.7849 15.875
+v 35.943 -9.506 20.313
+v -9.965 36.705 15.875
+v 37.21 0 20.313
+v 17.84 -30.499 22.151
+v -10.342 36.5365 15.9699
+v -10.4325 36.5139 15.875
+v 27.751 -8.418 24.75
+v 26.792 -11.098 24.75
+v 26.792 -11.098 24.6977
+v 27.751 -8.418 24.7164
+v 34.465 -9.115 22.151
+v 35.681 0 22.151
+v 25.576 -13.67 24.7131
+v 22.179 -21.891 24.448
+v 33.687 0 23.561
+v 27.0371 26.6856 14.025
+v -27.0371 -26.6856 14.025
+v 37.188 -9.835 15.875
+v -32.6246 -19.3954 13.578
+v 20.1874 -32.1501 16.1531
+v 20.4974 -31.9643 15.875
+v 26.87 26.87 14.3533
+v 38.5 0 15.875
+v -26.87 -26.87 14.3533
+v -31.596 -21.112 15.2913
+v -32.4905 -19.6194 13.8015
+v 25.576 -13.67 24.75
+v -15.683 -26.811 24.448
+v 25.1718 -14.3444 24.7268
+v 24.6473 -15.2195 24.7117
+v -19.1645 32.763 14.6776
+v -36.871 -9.751 18.172
+v 24.113 -16.111 24.6963
+v -38.172 0 18.172
+v -35.4943 -13.4603 17
+v -35.107 -14.542 17
+v -35.1653 -14.2444 17
+v -35.3852 -13.7649 16.8777
+v -13.1265 13.1265 35.747
+v -18.625 18.625 24.75
+v -13.1265 -13.1265 35.747
+v 25.23 24.903 22.151
+v -9.9461 36.6357 15.3646
+v -9.4809 36.7522 15.7634
+v -18.625 -18.625 24.75
+v 16.844 -28.795 23.561
+v -34.5149 -15.7941 17
+v -35.107 -14.542 16.5651
+v -13 -13 35.747
+v -13 -10 35.747
+v -13 10 35.747
+v -13 13 35.747
+v -13.671 25.576 24.75
+v -16.112 24.113 24.75
+v -18.397 22.417 24.75
+v -20.506 20.506 24.75
+v -22.417 18.397 24.75
+v -24.113 16.112 24.75
+v -25.576 13.671 24.75
+v -35.943 -9.506 20.313
+v -26.793 11.098 24.75
+v -27.751 8.418 24.75
+v -37.211 0 20.313
+v -28.443 5.658 24.75
+v -28.86 2.843 24.75
+v 24.113 -16.111 24.75
+v -29 0 24.75
+v 36.6961 -9.7048 13.1747
+v 36.523 -10.3962 13.4177
+v 36.4086 -10.8531 13.578
+v -8.719 32.117 23.561
+v -28.86 -2.842 24.75
+v -28.443 -5.658 24.75
+v -27.751 -8.418 24.75
+v -26.793 -11.098 24.75
+v -25.576 -13.67 24.75
+v -24.113 -16.111 24.75
+v -22.417 -18.397 24.75
+v -20.506 -20.506 24.75
+v 36.871 -9.751 13.578
+v 22.417 -18.397 24.6885
+v -34.465 -9.115 22.151
+v -18.397 -22.417 24.75
+v -16.112 -24.113 24.75
+v -13.671 -25.576 24.75
+v -35.681 0 22.151
+v 37.9544 -0.9279 13.3691
+v -27.9473 25.6813 17
+v 37.8998 -2.0405 13.578
+v 19.1645 -32.763 14.6776
+v -28.6531 24.9024 16.4377
+v -8.118 29.904 24.448
+v 37.0837 -8.1571 13.578
+v -33.688 0 23.561
+v 38.172 0 13.578
+v 0 30.959 24.448
+v 22.417 -18.397 24.75
+v -25.23 -24.903 22.151
+v 20.6333 -20.3655 24.7131
+v 20.506 -20.506 24.7087
+v 0 29 24.6886
+v 38 0 13.1954
+v -38.5 0 15.875
+v -37.188 -9.835 15.875
+v 13 13 35.747
+v 15.683 -26.811 24.448
+v 11.098 26.793 24.75
+v 8.418 27.751 24.75
+v 5.658 28.443 24.75
+v 35.3852 -13.7649 14.8723
+v 2.842 28.86 24.75
+v 35.107 -14.542 15.1849
+v 0 29 24.75
+v 17.913 -33.513 17
+v 19.107 -32.7975 17
+v 18.2144 -33.3324 15.875
+v -2.843 28.86 24.75
+v -5.658 28.443 24.75
+v -8.418 27.751 24.75
+v -11.098 26.793 24.75
+v 28.6531 24.9024 15.3123
+v 19.25 -32.909 15.875
+v 20.506 -20.506 24.75
+v 36.364 -11.031 13.7743
+v 2.2501 28.8892 24.6663
+v 19.5525 -21.3699 24.6948
+v 18.397 -22.417 24.6779
+v 2.842 28.86 24.6604
+v 37.817 -3.725 14.5724
+v 37.3366 -6.964 14.2345
+v 37.27 -7.413 14.1876
+v 37.2447 -7.5139 14.105
+v 5.658 28.443 24.6667
+v 18.397 -22.417 24.75
+v 16.111 -24.113 24.6813
+v 7.5897 27.9587 24.6954
+v 8.418 27.751 24.6804
+v 18.3829 -33.2314 15.6624
+v 15.683 26.811 24.448
+v 16.111 -24.113 24.75
+v 33.058 -18.838 18.172
+v 14.6252 -25.0035 24.7044
+v -28.6531 -24.9024 15.3123
+v 13.67 -25.576 24.6836
+v 9.5463 27.3477 24.6737
+v 33.342 -19 15.875
+v 11.098 26.793 24.6646
+v 13.67 -25.576 24.75
+v 14.4322 -34.7087 17
+v 14.542 -35.107 17
+v -23.821 -23.511 23.561
+v 13 10 78
+v 13 -10 78
+v 13 -10 35.747
+v 13 10 35.747
+v 13 13 78
+v -36.6961 -9.7048 13.1747
+v -36.9012 -8.8859 13.3881
+v -37.0837 -8.1571 13.578
+v 13 -13 78
+v 13 -13 35.747
+v -38 0 0
+v -37.817 3.725 0
+v -37.27 7.413 0
+v -36.364 11.031 0
+v -35.107 14.542 0
+v -33.513 17.913 0
+v -31.596 21.112 0
+v -29.374 24.107 0
+v 32.9839 -18.7959 13.3878
+v 32.787 -19.1245 13.492
+v -37.8998 -2.0405 13.578
+v -38 0 13.195
+v 33.058 -18.838 13.578
+v -37.817 -3.725 0
+v -36.871 -9.751 13.578
+v -29.374 -24.107 0
+v -31.596 -21.112 0
+v -33.513 -17.913 0
+v -35.107 -14.542 0
+v -36.364 -11.031 0
+v -37.27 -7.413 0
+v -12.9343 25.9244 24.6781
+v -11.098 26.793 24.6646
+v -29 0 24.7348
+v -28.9728 0.5514 24.7292
+v 23.821 23.511 23.561
+v -28.86 2.843 24.7058
+v -38.172 0 13.578
+v 33.29 -18.2852 13.578
+v -26.87 26.87 0
+v -8.418 27.751 24.6804
+v -26.4007 27.2953 17
+v -26.992 26.641 18.172
+v -7.5897 27.9587 24.6954
+v -5.658 28.443 24.6667
+v 33.513 -17.913 14.0141
+v -36.4086 -10.8531 13.578
+v 33.3822 -18.1313 13.7583
+v -26.312 25.97 20.313
+v -5.3976 28.4816 24.6661
+v -2.843 28.86 24.6604
+v 29.374 24.107 0
+v 31.596 21.112 0
+v 33.513 17.913 0
+v 35.107 14.542 0
+v 36.364 11.031 0
+v 37.27 7.413 0
+v 37.817 3.725 0
+v 38 0 0
+v -29.374 24.107 17
+v -31.596 21.112 17
+v -37.7626 -4.0915 14.5336
+v -30.0813 22.9362 17
+v -37.817 -3.725 14.5719
+v 30.297 -8.013 24.448
+v -37.8341 -3.3776 14.3669
+v -13.671 -25.576 24.6835
+v 37.817 -3.725 0
+v 37.27 -7.413 0
+v 36.364 -11.031 0
+v 35.107 -14.542 0
+v 33.513 -17.913 0
+v 31.596 -21.112 0
+v 29.374 -24.107 0
+v -13 13 78
+v -22.179 -21.891 24.448
+v 14.542 35.107 0
+v 17.913 33.513 0
+v 21.112 31.596 0
+v 24.107 29.374 0
+v -10 10 35.747
+v -10 -10 35.747
+v 10 -10 35.747
+v 10 10 35.747
+v -10 10 78
+v -10 -10 78
+v 26.87 26.87 0
+v -31.921 20.5697 17
+v 10 10 78
+v 10 -10 78
+v -31.596 21.112 16.4587
+v 22.179 21.891 24.448
+v -37.27 -7.413 14.1873
+v 32.54 -8.606 23.561
+v 11.031 -36.364 0
+v -25.0698 28.5015 15.875
+v 7.413 -37.27 0
+v -27.224 26.87 15.875
+v 3.725 -37.817 0
+v -32.225 18.364 20.313
+v 0 -38 0
+v -3.725 -37.817 0
+v -7.413 -37.27 0
+v -11.031 -36.364 0
+v -30.901 17.609 22.151
+v -34.7754 -15.2433 14.942
+v 13.67 25.576 24.6836
+v -35.107 -14.542 15.1854
+v 8.719 -32.117 23.561
+v -36.0809 -11.8217 14.0923
+v -36.364 -11.031 13.7744
+v 13.67 25.576 24.75
+v 13 10 0
+v 13 13 0
+v 26.87 -26.87 0
+v -16.112 -24.113 24.6813
+v 29.374 -24.107 17
+v 31.596 -21.112 17
+v 30.0813 -22.9362 17
+v -14.6261 -25.0036 24.7044
+v -29.174 16.625 23.561
+v 8.118 -29.904 24.448
+v -33.058 -18.838 18.172
+v 31.921 -20.5697 17
+v 0 -30.959 24.448
+v 31.596 -21.112 16.4587
+v -25.6086 28.0132 15.419
+v -33.342 -19 15.875
+v 13 -13 0
+v 13 -10 0
+v 0 -29 24.6886
+v -27.0371 26.6856 14.025
+v -26.87 26.87 14.3533
+v 14.6252 25.0035 24.7044
+v 32.225 -18.364 20.313
+v -29.4178 24.048 15.875
+v 0 -38 17
+v 3.725 -37.817 17
+v 7.413 -37.27 17
+v 4.8389 -37.2129 17
+v 16.111 24.113 24.6813
+v 0 -37.8413 17
+v -29.3593 24.1232 15.875
+v 30.901 -17.609 22.151
+v -27.164 15.479 24.448
+v 9.88 -36.393 18.172
+v 0 -37.676 18.172
+v 16.111 24.113 24.75
+v 9.9234 -36.5522 17
+v 9.9461 -36.6357 16.3854
+v -16.2025 -24.0458 24.6812
+v 29.174 -16.625 23.561
+v 11.031 -36.364 17
+v -18.397 -22.417 24.6779
+v 33.058 18.838 18.172
+v -33.1638 -18.4957 13.4995
+v 18.397 22.417 24.6779
+v -33.29 -18.2852 13.578
+v 16.2014 24.0459 24.6812
+v -33.058 -18.838 13.578
+v -28.443 5.658 24.7109
+v 33.342 19 15.875
+v -32.9839 -18.7959 13.3878
+v 9.631 -35.476 20.313
+v 18.397 22.417 24.75
+v -27.164 -15.479 24.448
+v 29.4178 -24.048 15.875
+v 29.3593 -24.1232 15.875
+v 0 -36.727 20.313
+v -33.513 -17.913 14.0143
+v -32.6246 19.3954 13.578
+v -25.23 24.903 22.151
+v 20.506 20.506 24.7087
+v 9.235 -34.018 22.151
+v -20.506 -20.506 24.7087
+v 27.164 15.479 24.448
+v 0 -35.218 22.151
+v -31.6197 21.0725 15.252
+v -31.596 21.112 15.2914
+v 33.1638 18.4957 13.4995
+v 33.29 18.2852 13.578
+v 33.058 18.838 13.578
+v -14.542 -35.107 0
+v -17.913 -33.513 0
+v -21.112 -31.596 0
+v -24.107 -29.374 0
+v 0 -33.25 23.561
+v 32.9839 18.7959 13.3878
+v 20.506 20.506 24.75
+v -21.5138 -19.3937 24.7009
+v -22.417 -18.397 24.6885
+v -20.6333 -20.3655 24.7131
+v 32.6246 -19.3954 13.578
+v 21.5138 19.3937 24.7009
+v 22.417 18.397 24.6885
+v 33.513 17.913 14.0143
+v 20.6333 20.3655 24.7131
+v -32.54 8.606 23.561
+v 9.3502 -36.7849 15.875
+v -24.113 -16.111 24.6964
+v 31.596 -21.112 15.2914
+v 31.6197 -21.0725 15.252
+v 9.965 -36.705 15.875
+v -24.107 -29.374 17
+v -21.112 -31.596 17
+v 10.342 -36.5365 15.9699
+v -23.2383 -29.6906 17
+v 10.4325 -36.5139 15.875
+v 22.417 18.397 24.75
+v -19.086 -32.629 18.172
+v 36.871 9.751 18.172
+v -19.2451 -32.7147 17
+v -25.172 -14.344 24.7269
+v 35.4943 13.4603 17
+v -25.576 -13.67 24.7132
+v 35.107 14.542 17
+v 35.1653 14.2444 17
+v -30.297 8.013 24.448
+v 35.3852 13.7649 16.8777
+v 24.113 16.112 24.6962
+v -31.366 0 24.448
+v -18.605 -31.807 20.313
+v 34.5149 15.7941 17
+v 24.113 16.112 24.75
+v 19.25 32.909 15.875
+v 35.107 14.542 16.5651
+v 25.1722 14.3447 24.7267
+v 25.576 13.671 24.7131
+v 19.107 32.7975 17
+v 17.913 33.513 17
+v 18.3829 33.2314 16.0876
+v -25.6732 -13.4645 24.712
+v -26.793 -11.098 24.6976
+v -17.841 -30.499 22.151
+v 35.943 9.506 20.313
+v 25.576 13.671 24.75
+v -33.058 18.838 18.172
+v 18.2144 33.3324 15.875
+v -33.342 19 15.875
+v -27.751 8.418 24.7164
+v -28.0047 7.4061 24.7357
+v -27.8234 8.1291 24.7219
+v 25.6751 13.4612 24.7118
+v -27.751 -8.418 24.7164
+v 34.465 9.115 22.151
+v 26.792 11.098 24.6977
+v -20.4974 -31.9643 15.875
+v -28.1502 -6.8258 24.7275
+v -28.443 -5.658 24.7109
+v -28.0047 -7.4061 24.7357
+v 26.792 11.098 24.75
+v 27.751 8.418 24.7164
+v 9.9461 -36.6357 15.3646
+v 9.4809 -36.7522 15.7634
+v 27.751 8.418 24.75
+v -28.86 -2.842 24.7059
+v 28.1502 6.8258 24.7275
+v 28.443 5.658 24.7109
+v -32.9839 18.7959 13.3878
+v -32.787 19.1245 13.492
+v 28.0047 7.4061 24.7357
+v -33.058 18.838 13.578
+v -13 10 78
+v 37.188 9.835 15.875
+v -13 -10 78
+v 28.443 5.658 24.75
+v 14.542 35.107 17
+v -23.821 23.511 23.561
+v 14.6609 34.6154 17
+v -13 -13 78
+v 28.86 2.843 24.7058
+v -8.719 -32.117 23.561
+v 28.86 2.843 24.75
+v -33.29 18.2852 13.578
+v 29 0 24.75
+v 0 38 0
+v 3.725 37.817 0
+v 7.413 37.27 0
+v 11.031 36.364 0
+v -3.725 37.817 0
+v -8.118 -29.904 24.448
+v 28.9728 -0.5513 24.7292
+v -11.031 36.364 0
+v -7.413 37.27 0
+v 28.86 -2.842 24.7059
+v -19.1645 -32.763 14.6776
+v -20.1874 -32.1501 15.5969
+v -33.513 17.913 14.0141
+v -33.3822 18.1313 13.7583
+v -26.793 11.098 24.6976
+v -34.5141 15.7959 17
+v -35.107 14.542 17
+v -35.4949 13.4585 17
+v -35.0704 14.4701 17
+v -35.107 14.542 16.5646
+v -34.7754 15.2433 16.808
+v -14.542 -35.107 17
+v -36.871 9.751 18.172
+v -14.6609 -34.6154 17
+v -13 13 0
+v 24.107 29.374 17
+v 21.112 31.596 17
+v 23.2383 29.6906 17
+v 10 10 0
+v 10 -10 0
+v -10 -10 0
+v -10 10 0
+v -13 -10 0
+v -13 10 0
+v -11.031 -36.364 17
+v 19.086 32.629 18.172
+v -9.88 -36.393 18.172
+v 19.2451 32.7147 17
+v 27.9473 -25.6813 17
+v -22.179 21.891 24.448
+v -9.9234 -36.5522 17
+v 28.6531 -24.9024 16.4377
+v -9.9461 -36.6357 16.3854
+v -35.943 9.506 20.313
+v -7.413 -37.27 17
+v -3.725 -37.817 17
+v -5.0844 -37.1806 17
+v 36.6961 9.7048 13.1747
+v 36.9011 8.8863 13.3882
+v 18.605 31.807 20.313
+v 37.0837 8.1571 13.578
+v 37.8998 2.0405 13.578
+v -25.576 13.671 24.7132
+v -34.465 9.115 22.151
+v 36.871 9.751 13.578
+v -13 -13 0
+v -19.25 -32.909 15.875
+v -9.631 -35.476 20.313
+v -19.107 -32.7975 17
+v -17.913 -33.513 17
+v 17.84 30.499 22.151
+v -16.844 28.795 23.561
+v -18.3829 -33.2314 16.0876
+v -9.235 -34.018 22.151
+v -18.2144 -33.3324 15.875
+v 11.031 36.364 17
+v -37.188 9.835 15.875
+v 20.4974 31.9643 15.875
+v 9.88 36.393 18.172
+v 36.4086 10.8531 13.578
+v 9.9234 36.5522 17
+v -10.4325 -36.5139 15.875
+v 9.9461 36.6357 16.3854
+v -24.113 16.112 24.6963
+v -9.965 -36.705 15.875
+v -25.1725 14.3443 24.7268
+v -24.6486 15.2183 24.7117
+v 37.7626 4.0915 14.5336
+v 7.413 37.27 17
+v 37.817 3.725 14.5719
+v 3.725 37.817 17
+v 0 38 17
+v 0 37.8413 17
+v 5.0844 37.1806 17
+v -9.4809 -36.7522 15.9866
+v -9.3502 -36.7849 15.875
+v 37.8341 3.3776 14.3669
+v 28.86 -2.842 24.75
+v 0 37.676 18.172
+v 28.443 -5.658 24.7109
+v 27.164 -15.479 24.448
+v 9.631 35.476 20.313
+v 37.27 7.413 14.1873
+v -24.107 29.374 0
+v -21.112 31.596 0
+v -17.913 33.513 0
+v -14.542 35.107 0
+v 0 36.727 20.313
+v 9.235 34.018 22.151
+v 19.1645 32.763 14.6776
+v 20.1874 32.1501 15.5969
+v -22.417 18.397 24.6885
+v 35.107 14.542 15.1854
+v 34.7754 15.2433 14.942
+v 36.0809 11.8217 14.0923
+v 36.364 11.031 13.7744
+v -15.683 26.811 24.448
+v 10.4325 36.5139 15.875
+v -36.6961 9.7048 13.1747
+v -36.523 10.3962 13.4177
+v -36.4086 10.8531 13.578
+v 9.965 36.705 15.875
+v 26.4007 -27.2953 17
+v -26.87 -26.87 0
+v -36.871 9.751 13.578
+v 26.992 -26.641 18.172
+v 9.4809 36.7522 15.9866
+v 9.3502 36.7849 15.875
+v 31.921 20.5696 17
+v 31.596 21.112 17
+v 29.374 24.107 17
+v 30.232 22.7423 17
+v -37.9544 0.9279 13.3691
+v 31.596 21.112 16.4586
+v -37.8998 2.0405 13.578
+v 31.6197 21.0725 16.498
+v 26.312 -25.97 20.313
+v -20.6333 20.3655 24.7131
+v -20.506 20.506 24.7087
+v -37.0837 8.1571 13.578
+v 13.1265 13.1265 35.747
+v 13.1265 -13.1265 35.747
+v 18.625 18.625 24.75
+v 18.625 -18.625 24.75
+v -9.9461 -36.6357 15.3646
+v -10.342 -36.5365 15.7801
+v 25.0698 -28.5015 15.875
+v 32.225 18.364 20.313
+v 27.224 -26.87 15.875
+v -19.5525 21.3699 24.6948
+v -18.397 22.417 24.6779
+v -35.3852 13.7649 14.8723
+v -35.107 14.542 15.1849
+v 30.901 17.609 22.151
+v -14.4322 34.7087 17
+v -14.542 35.107 17
+v 0 -29 24.75
+v 2.842 -28.86 24.75
+v 5.658 -28.443 24.75
+v 8.418 -27.751 24.75
+v 11.098 -26.792 24.75
+v -16.112 24.113 24.6813
+v 28.443 -5.658 24.75
+v -11.098 -26.792 24.75
+v -8.418 -27.751 24.75
+v -5.658 -28.443 24.75
+v -36.364 11.031 13.7743
+v -2.843 -28.86 24.75
+v 29.174 16.625 23.561
+v -14.6261 25.0036 24.7044
+v -13.671 25.576 24.6836
+v -37.817 3.725 14.5724
+v -37.3366 6.964 14.2345
+v -37.27 7.413 14.1876
+v 25.6086 -28.0132 15.419
+v -37.2447 7.5139 14.105
+v 27.0371 -26.6856 14.025
+v 26.87 -26.87 14.3533
+v 12.9343 -25.9238 24.6782
+v 11.098 -26.792 24.6647
+v 9.9461 36.6357 15.3646
+v 10.342 36.5365 15.7801
+v 29.3593 24.1232 15.875
+v 29.4178 24.048 15.875
+v 16.844 28.795 23.561
+v 8.418 -27.751 24.6804
+v 27.8234 -8.1291 24.7219
+v 28.0047 -7.4061 24.7357
+v -30.297 -8.013 24.448
+v 7.5897 -27.9587 24.6954
+v 5.658 -28.443 24.6667
+v 25.6086 28.0132 16.331
+v 8.719 32.117 23.561
+v 25.0698 28.5015 15.875
+v 5.3977 -28.4816 24.6661
+v 2.842 -28.86 24.6604
+v -17.913 33.513 17
+v -19.107 32.7975 17
+v -18.2144 33.3324 15.875
+v -16.844 -28.795 23.561
+v 8.118 29.904 24.448
+v -32.54 -8.606 23.561
+v -19.25 32.909 15.875
+v -2.2503 -28.8892 24.6663
+v -2.843 -28.86 24.6604
+v -30.232 -22.7423 17
+v -31.921 -20.5696 17
+v -31.596 -21.112 17
+v -29.374 -24.107 17
+v -5.658 -28.443 24.6667
+v -25.6086 -28.0132 16.331
+v -31.596 -21.112 16.4586
+v -31.6197 -21.0725 16.498
+v -3.725 37.817 17
+v -7.413 37.27 17
+v -4.8389 37.2129 17
+v -25.0698 -28.5015 15.875
+v -7.5897 -27.9587 24.6954
+v -8.418 -27.751 24.6804
+v 25.23 -24.903 22.151
+v -18.3829 33.2314 15.6624
+v -9.88 36.393 18.172
+v -9.9234 36.5522 17
+v -9.9461 36.6357 16.3854
+v 32.6246 19.3954 13.578
+v -9.5447 -27.3478 24.6738
+v 27.9472 25.6814 17
+v -11.098 -26.792 24.6647
+v -32.225 -18.364 20.313
+v 26.992 26.641 18.172
+v -11.031 36.364 17
+v 31.596 21.112 15.2913
+v 32.4905 19.6194 13.8015
+v -30.901 -17.609 22.151
+v 26.4004 27.2956 17
+v -9.631 35.476 20.313
+v -19.086 32.629 18.172
+v -29.174 -16.625 23.561
+v -23.0418 29.8391 17
+v -19.2451 32.7147 17
+v -21.112 31.596 17
+v -24.107 29.374 17
+v -27.9472 -25.6814 17
+v 32.54 8.606 23.561
+v -26.992 -26.641 18.172
+v -18.605 31.807 20.313
+v -26.4004 -27.2956 17
+v -26.312 -25.97 20.313
+# 698 vertices
+
+g group_0_40919
+
+usemtl color_40919
+s 0
+
+f 139 190 189
+f 186 189 190
+f 139 86 190
+f 260 190 86
+f 266 269 270
+f 274 270 269
+f 85 448 86
+f 260 86 448
+f 448 270 260
+f 190 260 270
+f 190 270 274
+f 190 274 186
+f 494 485 492
+f 489 492 485
+f 485 299 489
+f 298 489 299
+# 16 faces
+
+g group_0_4634441
+
+usemtl color_4634441
+s 0
+
+f 186 187 188
+f 188 189 186
+f 269 268 274
+f 275 274 268
+f 186 274 275
+f 186 275 187
+f 489 298 490
+f 315 490 298
+# 8 faces
+
+g group_0_8273816
+
+usemtl color_8273816
+s 0
+
+f 267 266 271
+f 270 271 266
+f 84 448 85
+f 450 448 84
+f 271 270 448
+f 271 448 450
+f 492 491 493
+f 492 493 494
+# 8 faces
+
+g group_0_15277357
+
+usemtl color_15277357
+s 0
+
+f 188 187 194
+f 194 195 188
+f 268 267 275
+f 271 275 267
+f 83 455 84
+f 450 84 455
+f 271 455 275
+f 450 455 271
+f 455 194 275
+f 187 275 194
+f 493 491 516
+f 314 516 491
+f 314 491 490
+f 314 490 315
+f 455 195 194
+f 83 195 455
+# 16 faces
+
+g group_0_16089887
+
+usemtl color_16089887
+s 0
+
+f 7 456 8
+f 16 17 15
+f 14 15 17
+f 15 18 16
+f 15 14 20
+f 15 20 18
+f 17 16 21
+f 24 25 23
+f 23 25 26
+f 18 52 16
+f 20 180 18
+f 52 18 180
+f 24 54 25
+f 691 30 31
+f 21 175 17
+f 14 17 175
+f 24 23 19
+f 180 20 14
+f 19 156 24
+f 21 52 32
+f 469 78 34
+f 175 21 35
+f 21 32 37
+f 21 37 35
+f 468 681 40
+f 40 681 39
+f 36 40 39
+f 43 44 42
+f 41 42 44
+f 40 77 468
+f 35 46 45
+f 9 59 211
+f 211 574 51
+f 35 37 46
+f 556 555 65
+f 46 49 45
+f 2 1 25
+f 25 54 55
+f 21 16 52
+f 59 60 212
+f 57 32 52
+f 26 25 1
+f 124 2 55
+f 212 211 59
+f 644 28 56
+f 50 56 28
+f 38 12 80
+f 51 58 29
+f 61 47 42
+f 59 313 60
+f 53 212 60
+f 47 43 42
+f 691 31 555
+f 68 137 66
+f 692 691 554
+f 69 70 71
+f 244 136 253
+f 65 555 31
+f 69 72 70
+f 69 71 66
+f 66 71 308
+f 76 598 5
+f 73 79 75
+f 64 67 48
+f 78 469 77
+f 66 138 69
+f 74 79 73
+f 70 81 71
+f 82 81 70
+f 78 77 36
+f 72 82 70
+f 73 75 84
+f 85 73 84
+f 86 73 85
+f 83 84 75
+f 71 81 308
+f 81 82 313
+f 81 313 308
+f 80 12 48
+f 90 74 89
+f 89 74 88
+f 91 74 90
+f 87 88 74
+f 34 78 36
+f 61 100 63
+f 63 47 61
+f 163 164 253
+f 469 468 77
+f 94 97 68
+f 95 79 93
+f 93 79 92
+f 96 79 95
+f 98 79 96
+f 99 79 98
+f 101 79 99
+f 79 74 92
+f 91 92 74
+f 77 40 36
+f 64 63 100
+f 67 64 100
+f 103 255 102
+f 104 255 103
+f 255 254 102
+f 115 48 67
+f 66 94 68
+f 146 256 144
+f 144 256 158
+f 22 13 105
+f 134 160 48
+f 133 134 48
+f 108 79 107
+f 106 107 79
+f 109 79 108
+f 110 79 109
+f 111 79 110
+f 112 79 111
+f 113 79 112
+f 117 79 113
+f 101 106 79
+f 103 102 114
+f 48 115 133
+f 22 105 27
+f 97 94 116
+f 127 102 254
+f 79 117 118
+f 79 118 119
+f 114 104 103
+f 116 120 97
+f 695 656 693
+f 124 4 2
+f 80 48 140
+f 163 253 123
+f 121 123 253
+f 105 126 27
+f 254 253 164
+f 165 166 254
+f 27 126 130
+f 100 131 67
+f 25 55 2
+f 115 67 131
+f 652 120 116
+f 123 121 129
+f 122 245 248
+f 102 127 114
+f 122 125 245
+f 422 228 248
+f 217 218 126
+f 218 226 126
+f 66 137 138
+f 72 69 138
+f 48 160 140
+f 591 74 73
+f 86 139 591
+f 86 591 73
+f 80 359 38
+f 121 136 129
+f 132 419 650
+f 140 294 80
+f 145 74 143
+f 142 143 74
+f 147 74 145
+f 141 142 74
+f 237 272 50
+f 148 4 150
+f 148 150 149
+f 153 74 152
+f 154 74 153
+f 87 74 154
+f 151 152 74
+f 147 151 74
+f 156 19 149
+f 183 149 19
+f 155 237 50
+f 146 144 52
+f 150 156 149
+f 313 82 138
+f 82 72 138
+f 52 180 146
+f 157 134 133
+f 115 131 133
+f 255 104 158
+f 157 133 131
+f 50 28 155
+f 651 130 159
+f 160 161 140
+f 159 162 651
+f 162 167 651
+f 172 156 150
+f 114 52 104
+f 158 104 52
+f 144 158 52
+f 145 162 159
+f 135 147 159
+f 145 159 147
+f 51 177 211
+f 55 54 156
+f 165 254 164
+f 156 124 55
+f 171 179 651
+f 127 254 166
+f 24 156 54
+f 170 651 167
+f 129 57 123
+f 161 160 168
+f 157 168 160
+f 163 123 57
+f 157 160 134
+f 164 163 57
+f 140 161 169
+f 114 127 166
+f 143 167 145
+f 145 167 162
+f 10 29 693
+f 184 6 4
+f 179 173 651
+f 169 176 140
+f 124 172 4
+f 150 4 172
+f 174 169 168
+f 57 52 164
+f 165 164 52
+f 114 166 52
+f 58 51 574
+f 161 168 169
+f 165 52 166
+f 142 171 170
+f 142 170 143
+f 143 170 167
+f 176 178 140
+f 216 215 191
+f 156 172 124
+f 179 181 173
+f 177 29 10
+f 141 181 179
+f 171 142 179
+f 141 179 142
+f 184 148 183
+f 149 183 148
+f 177 10 211
+f 182 178 176
+f 176 174 182
+f 319 326 173
+f 177 51 29
+f 14 175 180
+f 176 169 174
+f 141 292 181
+f 35 320 175
+f 181 292 173
+f 193 216 192
+f 191 192 216
+f 349 33 38
+f 216 278 209
+f 185 132 650
+f 198 494 197
+f 205 204 208
+f 196 209 207
+f 4 148 184
+f 224 204 257
+f 192 191 210
+f 204 224 208
+f 197 494 196
+f 493 516 214
+f 214 215 493
+f 216 493 215
+f 583 197 207
+f 222 346 403
+f 426 403 346
+f 154 218 217
+f 621 87 217
+f 154 217 87
+f 257 256 231
+f 99 222 220
+f 219 101 220
+f 99 220 101
+f 231 233 257
+f 193 192 210
+f 635 221 76
+f 223 207 206
+f 387 378 180
+f 233 231 180
+f 208 233 180
+f 153 226 154
+f 218 154 226
+f 689 227 228
+f 208 224 233
+f 283 227 281
+f 229 230 126
+f 235 126 230
+f 126 226 229
+f 191 215 232
+f 152 229 153
+f 293 295 214
+f 635 173 221
+f 152 230 229
+f 226 153 229
+f 62 261 185
+f 122 248 228
+f 277 173 344
+f 231 146 180
+f 224 257 233
+f 301 336 62
+f 130 235 236
+f 232 210 191
+f 130 126 235
+f 206 207 209
+f 151 236 235
+f 151 235 152
+f 319 173 292
+f 62 305 301
+f 235 230 152
+f 228 285 234
+f 246 248 245
+f 249 209 247
+f 228 422 285
+f 130 236 135
+f 239 299 238
+f 240 299 239
+f 241 298 240
+f 242 298 241
+f 253 315 244
+f 243 298 242
+f 244 298 243
+f 249 251 209
+f 62 252 305
+f 245 276 246
+f 135 236 151
+f 135 151 147
+f 125 328 245
+f 266 267 268
+f 266 268 269
+f 265 299 264
+f 263 264 299
+f 137 249 138
+f 246 273 248
+f 247 138 249
+f 237 238 299
+f 8 467 7
+f 7 467 250
+f 246 276 273
+f 249 137 251
+f 223 251 137
+f 467 470 250
+f 193 278 216
+f 248 273 422
+f 261 62 336
+f 247 209 278
+f 251 206 209
+f 221 173 277
+f 276 424 273
+f 228 227 283
+f 193 210 138
+f 193 138 278
+f 247 278 138
+f 344 342 277
+f 45 49 279
+f 125 122 283
+f 283 122 228
+f 326 344 173
+f 223 206 251
+f 279 250 337
+f 279 7 250
+f 279 49 7
+f 215 214 296
+f 295 296 214
+f 297 292 141
+f 296 232 215
+f 299 240 298
+f 254 315 253
+f 234 285 290
+f 255 315 254
+f 1 314 300
+f 257 314 256
+f 210 232 296
+f 210 296 295
+f 259 300 314
+f 303 304 302
+f 285 514 290
+f 2 314 1
+f 259 352 302
+f 303 302 311
+f 293 291 313
+f 138 293 313
+f 140 307 294
+f 314 2 4
+f 293 138 295
+f 210 295 138
+f 304 175 576
+f 303 309 304
+f 303 311 309
+f 312 554 318
+f 261 336 339
+f 304 309 175
+f 256 315 255
+f 258 259 314
+f 257 258 314
+f 315 256 314
+f 298 244 315
+f 645 646 310
+f 283 281 312
+f 311 180 309
+f 309 180 175
+f 640 641 307
+f 310 307 645
+f 318 225 317
+f 629 630 307
+f 688 185 261
+f 317 283 318
+f 312 318 283
+f 307 140 629
+f 261 351 688
+f 276 245 321
+f 324 325 323
+f 338 334 324
+f 325 327 323
+f 322 323 327
+f 305 118 301
+f 679 66 308
+f 119 118 305
+f 292 297 319
+f 119 305 252
+f 333 319 297
+f 424 276 321
+f 66 679 94
+f 331 332 325
+f 327 325 332
+f 121 253 136
+f 319 333 326
+f 325 324 334
+f 125 283 328
+f 320 35 45
+f 338 324 335
+f 255 158 256
+f 320 45 329
+f 146 231 256
+f 334 331 325
+f 212 348 213
+f 212 53 348
+f 379 361 277
+f 377 375 261
+f 279 337 329
+f 184 183 338
+f 334 338 183
+f 360 261 339
+f 329 45 279
+f 291 214 355
+f 213 355 214
+f 293 214 291
+f 343 213 341
+f 355 213 343
+f 335 391 338
+f 334 183 331
+f 19 331 183
+f 341 345 343
+f 222 99 346
+f 98 346 99
+f 339 336 117
+f 118 117 336
+f 118 336 301
+f 445 202 444
+f 332 331 349
+f 213 348 341
+f 344 350 342
+f 311 302 352
+f 348 345 341
+f 326 333 344
+f 350 344 333
+f 259 302 353
+f 349 354 332
+f 347 579 340
+f 364 203 202
+f 348 53 345
+f 328 317 203
+f 352 180 311
+f 379 380 361
+f 687 228 234
+f 33 349 331
+f 364 202 363
+f 382 379 277
+f 375 351 261
+f 331 19 33
+f 277 342 358
+f 277 358 382
+f 354 349 359
+f 345 313 343
+f 291 355 313
+f 343 313 355
+f 261 360 377
+f 345 60 313
+f 359 362 354
+f 345 53 60
+f 113 360 117
+f 234 290 357
+f 245 203 321
+f 364 321 203
+f 349 38 359
+f 117 360 339
+f 365 366 239
+f 381 239 366
+f 9 313 59
+f 225 203 317
+f 245 328 203
+f 294 362 359
+f 357 290 306
+f 257 204 258
+f 564 240 381
+f 372 362 294
+f 398 400 351
+f 367 366 365
+f 365 239 373
+f 204 205 258
+f 294 359 80
+f 321 364 424
+f 363 424 364
+f 342 350 358
+f 373 367 365
+f 375 376 351
+f 372 294 307
+f 374 358 350
+f 310 372 307
+f 283 317 328
+f 258 205 378
+f 112 376 375
+f 377 113 375
+f 112 375 113
+f 113 377 360
+f 383 306 290
+f 384 335 324
+f 208 378 205
+f 367 347 366
+f 381 366 347
+f 347 564 381
+f 453 357 306
+f 378 387 258
+f 386 258 387
+f 335 384 388
+f 352 259 386
+f 258 386 259
+f 330 453 306
+f 184 338 280
+f 400 417 351
+f 403 306 383
+f 338 391 393
+f 338 393 280
+f 390 392 389
+f 227 692 281
+f 351 376 385
+f 370 432 390
+f 388 393 391
+f 380 379 394
+f 374 394 379
+f 379 382 374
+f 390 389 370
+f 208 180 378
+f 306 403 330
+f 367 683 347
+f 111 385 112
+f 376 112 385
+f 374 382 358
+f 386 387 180
+f 398 351 385
+f 390 397 392
+f 388 391 335
+f 352 386 180
+f 417 418 639
+f 396 32 57
+f 361 412 413
+f 413 428 361
+f 639 351 417
+f 399 401 402
+f 397 395 392
+f 318 554 225
+f 432 517 397
+f 401 399 404
+f 110 398 111
+f 412 361 405
+f 184 280 6
+f 312 281 554
+f 399 402 396
+f 402 340 396
+f 380 405 361
+f 383 406 403
+f 110 400 398
+f 385 111 398
+f 396 449 399
+f 323 284 324
+f 401 408 402
+f 380 394 405
+f 409 405 394
+f 323 286 284
+f 411 408 401
+f 3 361 428
+f 401 404 411
+f 402 408 340
+f 281 692 554
+f 416 423 415
+f 414 416 415
+f 282 324 284
+f 428 431 3
+f 411 347 408
+f 408 347 340
+f 426 427 403
+f 330 403 427
+f 418 429 639
+f 529 496 454
+f 109 418 417
+f 109 417 110
+f 420 37 32
+f 219 220 406
+f 220 403 406
+f 536 537 330
+f 110 417 400
+f 421 413 412
+f 410 416 414
+f 220 222 403
+f 405 409 412
+f 396 420 32
+f 421 412 409
+f 439 384 282
+f 410 423 416
+f 425 475 330
+f 475 513 330
+f 273 424 422
+f 435 639 429
+f 37 420 430
+f 286 323 322
+f 418 109 429
+f 108 429 109
+f 330 427 425
+f 397 390 432
+f 476 422 424
+f 406 433 434
+f 430 46 37
+f 3 437 446
+f 406 639 433
+f 96 425 427
+f 96 427 426
+f 96 426 98
+f 431 428 436
+f 421 436 428
+f 421 428 413
+f 504 285 422
+f 439 282 438
+f 426 346 98
+f 514 285 504
+f 107 434 433
+f 433 435 107
+f 324 282 384
+f 107 435 108
+f 7 3 442
+f 439 438 388
+f 435 429 108
+f 436 440 437
+f 436 437 431
+f 434 441 406
+f 438 282 280
+f 106 441 434
+f 106 434 107
+f 696 234 357
+f 438 393 388
+f 7 442 443
+f 443 456 7
+f 384 439 388
+f 441 219 406
+f 201 444 202
+f 356 363 202
+f 356 202 445
+f 263 262 415
+f 357 453 11
+f 446 442 3
+f 447 445 444
+f 441 106 219
+f 101 219 106
+f 415 423 263
+f 396 57 449
+f 393 438 280
+f 451 443 442
+f 347 411 449
+f 440 451 446
+f 442 446 451
+f 415 452 454
+f 414 415 454
+f 57 547 540
+f 540 449 57
+f 446 437 440
+f 356 445 447
+f 456 443 458
+f 451 458 443
+f 404 399 449
+f 444 201 459
+f 473 474 201
+f 452 526 454
+f 458 460 8
+f 458 8 456
+f 237 299 272
+f 262 263 299
+f 272 299 265
+f 449 411 404
+f 444 459 447
+f 638 637 250
+f 363 356 424
+f 447 424 356
+f 457 310 466
+f 330 500 453
+f 457 372 310
+f 470 550 250
+f 638 250 550
+f 370 472 432
+f 463 299 462
+f 462 299 461
+f 389 371 370
+f 262 299 464
+f 512 136 243
+f 464 299 463
+f 485 461 299
+f 527 424 603
+f 447 474 424
+f 330 537 500
+f 473 424 474
+f 459 201 474
+f 459 474 447
+f 465 485 469
+f 536 330 513
+f 478 479 477
+f 476 477 479
+f 576 175 320
+f 477 480 478
+f 95 475 425
+f 95 425 96
+f 480 477 481
+f 476 481 477
+f 262 452 415
+f 11 696 357
+f 479 478 483
+f 316 654 310
+f 466 310 654
+f 461 485 465
+f 487 488 486
+f 587 320 329
+f 489 490 491
+f 489 491 492
+f 654 655 466
+f 264 528 487
+f 480 527 478
+f 505 501 495
+f 481 424 480
+f 527 480 424
+f 453 522 11
+f 482 495 484
+f 395 497 518
+f 483 422 479
+f 476 479 422
+f 286 314 284
+f 282 284 314
+f 6 280 314
+f 280 282 314
+f 487 498 488
+f 499 302 304
+f 395 518 407
+f 424 481 476
+f 501 484 495
+f 244 243 136
+f 499 502 302
+f 565 566 240
+f 505 495 503
+f 576 499 304
+f 410 496 498
+f 498 496 488
+f 4 6 314
+f 497 484 501
+f 414 454 496
+f 322 327 506
+f 505 506 507
+f 507 506 327
+f 501 505 507
+f 410 414 496
+f 287 516 286
+f 353 302 502
+f 422 483 504
+f 511 242 509
+f 508 509 242
+f 508 242 241
+f 483 68 97
+f 503 545 505
+f 545 546 505
+f 370 516 369
+f 507 327 332
+f 368 516 289
+f 483 97 504
+f 552 496 529
+f 332 497 507
+f 501 507 497
+f 504 120 514
+f 513 95 93
+f 504 97 120
+f 509 508 515
+f 288 289 516
+f 287 288 516
+f 314 286 516
+f 395 397 517
+f 496 552 510
+f 95 513 475
+f 484 519 520
+f 484 520 482
+f 530 508 241
+f 482 520 369
+f 520 519 523
+f 523 525 520
+f 497 354 518
+f 497 332 354
+f 497 395 484
+f 519 484 395
+f 517 519 395
+f 290 514 383
+f 136 512 129
+f 559 521 510
+f 514 120 128
+f 524 419 407
+f 599 502 499
+f 664 465 469
+f 517 523 519
+f 522 453 500
+f 514 128 383
+f 511 509 515
+f 518 524 407
+f 664 461 465
+f 128 406 383
+f 518 362 524
+f 369 520 525
+f 525 471 369
+f 539 531 526
+f 517 525 523
+f 518 354 362
+f 239 381 240
+f 599 353 502
+f 524 457 419
+f 483 478 527
+f 498 487 528
+f 563 565 240
+f 524 372 457
+f 527 68 483
+f 264 487 486
+f 524 362 372
+f 482 369 368
+f 137 68 527
+f 531 454 526
+f 495 289 532
+f 532 503 495
+f 515 508 530
+f 528 410 498
+f 353 627 259
+f 539 526 533
+f 495 482 289
+f 471 472 370
+f 370 369 471
+f 500 537 534
+f 503 532 535
+f 529 454 531
+f 199 617 200
+f 472 471 517
+f 603 473 200
+f 540 243 538
+f 242 553 243
+f 542 543 541
+f 539 541 544
+f 544 541 543
+f 531 539 544
+f 535 546 545
+f 540 547 243
+f 57 129 547
+f 548 470 467
+f 8 460 467
+f 548 467 460
+f 533 577 539
+f 92 534 537
+f 92 537 536
+f 92 536 93
+f 517 432 472
+f 544 543 549
+f 531 544 529
+f 93 536 513
+f 549 529 544
+f 535 545 503
+f 449 540 538
+f 471 525 517
+f 242 511 553
+f 538 243 553
+f 556 557 485
+f 555 556 485
+f 469 485 468
+f 547 512 243
+f 569 199 198
+f 529 558 552
+f 423 560 263
+f 337 250 551
+f 225 554 485
+f 529 549 558
+f 200 494 199
+f 511 515 449
+f 522 500 567
+f 553 511 449
+f 202 203 485
+f 538 553 449
+f 552 22 559
+f 201 200 473
+f 589 600 500
+f 588 589 500
+f 129 512 547
+f 552 558 22
+f 623 198 197
+f 554 555 485
+f 557 468 485
+f 528 264 561
+f 560 561 264
+f 560 264 263
+f 200 201 485
+f 563 240 564
+f 196 207 197
+f 534 562 500
+f 265 264 486
+f 559 27 643
+f 562 534 91
+f 241 240 566
+f 22 27 559
+f 622 623 197
+f 566 530 241
+f 92 91 534
+f 561 560 410
+f 595 289 288
+f 526 464 568
+f 526 568 533
+f 105 567 126
+f 515 530 566
+f 515 566 565
+f 482 368 289
+f 570 199 569
+f 571 199 570
+f 452 262 464
+f 526 452 464
+f 201 202 485
+f 203 225 485
+f 485 494 200
+f 493 196 494
+f 12 337 551
+f 602 200 617
+f 572 533 568
+f 286 322 287
+f 561 410 528
+f 211 516 574
+f 563 564 347
+f 449 563 347
+f 288 287 505
+f 570 569 575
+f 560 423 410
+f 573 597 599
+f 23 573 576
+f 505 546 288
+f 563 449 565
+f 515 565 449
+f 624 626 198
+f 577 578 539
+f 576 573 599
+f 572 578 577
+f 581 582 580
+f 579 580 582
+f 575 571 570
+f 371 574 516
+f 559 510 552
+f 581 580 584
+f 584 634 581
+f 622 197 585
+f 585 197 583
+f 600 601 567
+f 287 506 505
+f 580 579 586
+f 580 586 584
+f 572 631 578
+f 600 567 500
+f 572 577 533
+f 569 198 590
+f 626 590 198
+f 19 576 587
+f 500 562 588
+f 347 584 586
+f 506 287 322
+f 213 214 516
+f 493 216 209
+f 587 576 320
+f 541 463 462
+f 589 588 90
+f 562 91 588
+f 680 340 582
+f 579 582 340
+f 90 588 91
+f 594 593 592
+f 591 592 593
+f 199 494 198
+f 585 583 223
+f 596 289 595
+f 532 289 596
+f 593 74 591
+f 569 590 575
+f 586 579 347
+f 620 621 567
+f 79 594 75
+f 596 595 535
+f 396 340 598
+f 139 189 591
+f 592 591 189
+f 592 189 188
+f 592 188 195
+f 209 196 493
+f 212 213 516
+f 368 369 516
+f 370 371 516
+f 516 211 212
+f 598 420 396
+f 532 596 535
+f 583 207 223
+f 462 461 541
+f 499 576 599
+f 567 601 612
+f 199 571 617
+f 595 288 546
+f 440 593 451
+f 436 593 440
+f 458 451 593
+f 421 593 436
+f 409 593 421
+f 394 593 409
+f 374 593 394
+f 350 333 593
+f 460 458 593
+f 603 200 602
+f 89 601 600
+f 89 600 90
+f 595 546 535
+f 90 600 589
+f 539 463 541
+f 592 75 594
+f 606 647 605
+f 648 605 647
+f 83 75 195
+f 592 195 75
+f 74 593 333
+f 141 74 297
+f 333 297 74
+f 374 350 593
+f 603 602 527
+f 598 604 420
+f 607 608 79
+f 608 609 79
+f 79 609 610
+f 79 610 611
+f 79 611 182
+f 473 603 424
+f 601 89 612
+f 61 593 100
+f 42 593 61
+f 131 100 594
+f 41 593 42
+f 613 593 41
+f 548 593 613
+f 460 593 548
+f 594 100 593
+f 157 131 594
+f 616 79 615
+f 614 615 79
+f 607 79 618
+f 618 79 616
+f 119 614 79
+f 598 76 604
+f 88 612 89
+f 300 259 627
+f 621 217 567
+f 182 174 79
+f 168 594 174
+f 79 174 594
+f 594 168 157
+f 617 571 527
+f 527 571 575
+f 567 612 620
+f 602 617 527
+f 621 620 87
+f 619 604 221
+f 685 642 486
+f 624 198 623
+f 76 221 604
+f 87 620 88
+f 612 88 620
+f 496 680 488
+f 126 567 217
+f 223 137 585
+f 361 694 619
+f 625 599 597
+f 221 277 619
+f 622 585 137
+f 178 629 140
+f 300 627 628
+f 619 277 361
+f 623 622 137
+f 575 590 626
+f 627 599 628
+f 631 464 463
+f 539 578 463
+f 631 463 578
+f 630 636 307
+f 625 628 599
+f 632 464 631
+f 568 464 632
+f 419 457 650
+f 510 521 5
+f 611 630 629
+f 611 629 182
+f 137 527 623
+f 624 623 527
+f 575 626 527
+f 457 466 650
+f 353 599 627
+f 521 76 5
+f 632 631 572
+f 629 178 182
+f 624 527 626
+f 548 613 550
+f 548 550 470
+f 634 584 347
+f 640 307 636
+f 392 697 389
+f 521 635 76
+f 581 237 633
+f 568 632 572
+f 630 611 636
+f 681 468 557
+f 667 389 661
+f 661 389 697
+f 461 542 541
+f 610 636 611
+f 347 682 634
+f 697 392 695
+f 395 695 392
+f 645 307 641
+f 128 639 406
+f 695 395 407
+f 641 640 609
+f 610 609 640
+f 610 640 636
+f 559 643 521
+f 435 433 639
+f 637 44 551
+f 44 43 551
+f 606 681 557
+f 551 250 637
+f 644 642 28
+f 646 645 608
+f 609 608 645
+f 329 670 587
+f 609 645 641
+f 647 556 649
+f 647 649 648
+f 310 646 316
+f 329 337 670
+f 608 607 316
+f 608 316 646
+f 653 687 648
+f 652 128 120
+f 649 653 648
+f 56 272 265
+f 30 690 653
+f 64 48 551
+f 652 639 128
+f 643 130 651
+f 655 660 466
+f 671 653 649
+f 265 644 56
+f 351 639 652
+f 670 337 12
+f 618 655 654
+f 316 607 654
+f 618 654 607
+f 12 551 48
+f 643 27 130
+f 642 644 486
+f 31 30 653
+f 170 171 651
+f 675 373 238
+f 26 573 23
+f 653 65 31
+f 466 668 669
+f 597 573 26
+f 657 658 656
+f 658 659 656
+f 656 659 693
+f 239 238 373
+f 660 668 466
+f 135 159 130
+f 576 19 23
+f 658 662 659
+f 644 265 486
+f 662 658 663
+f 657 663 658
+f 587 33 19
+f 665 666 664
+f 681 673 665
+f 666 543 664
+f 542 664 543
+f 655 618 660
+f 616 660 618
+f 634 682 237
+f 469 34 665
+f 34 674 665
+f 313 662 663
+f 669 676 466
+f 676 62 466
+f 665 664 469
+f 556 647 557
+f 667 661 29
+f 635 521 643
+f 542 461 664
+f 33 670 38
+f 676 678 62
+f 657 308 313
+f 649 556 671
+f 65 671 556
+f 635 643 651
+f 672 549 666
+f 543 666 549
+f 389 667 371
+f 606 557 647
+f 695 308 656
+f 656 308 657
+f 669 668 615
+f 616 615 668
+f 651 173 635
+f 670 33 587
+f 666 665 673
+f 616 668 660
+f 663 657 313
+f 681 665 674
+f 674 39 681
+f 56 50 272
+f 683 675 238
+f 682 683 238
+f 653 671 65
+f 581 677 582
+f 633 677 581
+f 373 675 367
+f 673 672 666
+f 678 676 614
+f 615 614 676
+f 615 676 669
+f 698 679 308
+f 680 582 677
+f 677 28 680
+f 36 674 34
+f 633 237 155
+f 252 62 678
+f 667 58 371
+f 670 12 38
+f 606 605 681
+f 673 681 605
+f 678 614 252
+f 238 237 682
+f 581 634 237
+f 695 698 308
+f 633 28 677
+f 574 371 58
+f 119 252 614
+f 11 13 686
+f 633 155 28
+f 13 522 105
+f 58 667 29
+f 687 672 605
+f 673 605 672
+f 488 685 486
+f 11 522 13
+f 94 684 116
+f 36 39 674
+f 682 347 683
+f 567 105 522
+f 300 628 1
+f 679 684 94
+f 549 672 686
+f 605 648 687
+f 679 698 132
+f 62 185 650
+f 367 675 683
+f 679 132 684
+f 684 688 116
+f 625 1 628
+f 604 430 420
+f 488 680 685
+f 28 685 680
+f 686 558 549
+f 116 688 652
+f 62 650 466
+f 690 691 689
+f 691 692 689
+f 689 692 227
+f 185 688 684
+f 642 685 28
+f 132 185 684
+f 555 554 691
+f 351 652 688
+f 340 680 5
+f 597 26 1
+f 690 30 691
+f 29 695 693
+f 46 430 694
+f 510 5 680
+f 211 10 659
+f 693 659 10
+f 659 662 9
+f 659 9 211
+f 604 619 430
+f 558 686 13
+f 662 313 9
+f 340 5 598
+f 228 687 689
+f 690 689 687
+f 653 690 687
+f 694 430 619
+f 680 496 510
+f 29 697 695
+f 13 22 558
+f 672 696 686
+f 625 597 1
+f 637 638 41
+f 44 637 41
+f 696 672 687
+f 638 550 613
+f 613 41 638
+f 29 661 697
+f 63 64 551
+f 687 234 696
+f 47 63 551
+f 3 431 437
+f 47 551 43
+f 686 696 11
+f 695 407 698
+f 694 49 46
+f 698 407 419
+f 3 49 694
+f 419 132 698
+f 694 361 3
+f 49 3 7
+# 1344 faces
+
+ #end of obj_0
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-base.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-base.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..fa5abdef11d20a519910f943e9e8970130fb0965
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-base.urdf
@@ -0,0 +1,69 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head-long.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head-long.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..b743b89f7bc685fd073dcfc5f17d04944dec9fa8
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head-long.urdf
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..38e89cbf3fa73e21706e3ffba4a4e6a9064fc2a0
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head.urdf
@@ -0,0 +1,70 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/tip.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/tip.obj
new file mode 100644
index 0000000000000000000000000000000000000000..5c9b5e1bb053088edc361d78d21125904f5558c4
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/tip.obj
@@ -0,0 +1,4182 @@
+# Object Export From Tinkercad Server 2015
+
+mtllib obj.mtl
+
+o obj_0
+v -7.73 6.3469 14.497
+v -8.315 5.5589 14.497
+v -8.819 4.7169 14.497
+v -9.239 3.8299 14.497
+v -9.57 2.9059 14.497
+v -9.808 1.9539 14.497
+v -9.952 0.9829 14.497
+v -10 0.0029 14.497
+v -9.952 -0.9771 14.497
+v -9.808 -1.9481 14.497
+v -9.57 -2.9001 14.497
+v -9.239 -3.8241 14.497
+v -8.819 -4.7111 14.497
+v -8.315 -5.5531 14.497
+v -7.73 -6.3411 14.497
+v 5.752 -3.3181 8.031
+v 5.913 -3.4111 7.997
+v 1.6906 6.3125 9.8999
+v 4.696 4.6989 5.963
+v 1.6906 6.3125 9.8999
+v 3.321 -5.7491 4.031
+v -6.224 3.5959 9.863
+v 3.321 -5.7491 9.963
+v 3.414 -5.9101 3.997
+v 4.619 4.6219 5.8999
+v 0 -9.9971 14.497
+v 0.98 -9.9491 14.497
+v 1.951 -9.8051 14.497
+v 2.903 -9.5671 14.497
+v 3.827 -9.2361 14.497
+v 4.3254 -9.0001 14.497
+v -4.3254 -9.0001 14.497
+v -3.827 -9.2361 14.497
+v -2.903 -9.5671 14.497
+v 1.86 6.9439 9.863
+v -1.951 -9.8051 14.497
+v -0.98 -9.9491 14.497
+v -6.503 -7.583 14.497
+v 6.3422 -7.7249 14.495
+v 3.2663 -5.6544 4.0941
+v 5.5544 -8.3098 14.495
+v -6.352 3.6699 9.704
+v 3.2661 -5.6545 4.0941
+v 4.7127 -8.8136 14.495
+v 4.828 4.8309 5.997
+v 4.3187 -9.0001 14.495
+v -3.7724 5.2718 4.0942
+v 1.898 7.0879 9.704
+v 3.414 -5.9101 9.997
+v 5.6573 -3.2632 8.0941
+v 5.6572 -3.2634 8.0941
+v 4.959 4.9619 5.963
+v -6.451 3.7269 9.497
+v 3.506 -6.0711 9.963
+v 1.928 7.1979 9.497
+v 5.081 5.0839 5.863
+v 2.6786 -4.6364 6.997
+v 1.3864 -5.1721 6.997
+v 3.2661 -5.6545 9.8999
+v 3.2663 -5.6544 9.8999
+v 3.7879 -3.785 6.997
+v -4.3187 -9.0001 14.495
+v -4.7127 -8.8136 14.495
+v -5.5544 -8.3098 14.495
+v -6.3422 -7.7249 14.495
+v -6.503 -7.5792 14.495
+v 5.081 5.0839 4.131
+v 0 5.36 6.997
+v 1.3864 5.1771 6.997
+v 7.069 7.0718 14.495
+v 4.959 4.9619 4.031
+v 2.6748 -5.8996 5.8999
+v 0.975 4.9069 0.999
+v 1.451 4.7879 0.999
+v 1.913 4.6219 0.999
+v 2.357 4.4119 0.999
+v 2.778 4.1599 0.999
+v 0 5.0029 0.999
+v 0.49 4.9789 0.999
+v -1.3864 5.1771 6.997
+v 3.172 3.8679 0.999
+v 3.535 3.5379 0.999
+v 3.865 3.1749 0.999
+v 4.157 2.7809 0.999
+v 0.8324 3.1095 0.999
+v 0 3.2193 0.999
+v 5.1743 1.3893 6.997
+v -2.778 4.1599 0.999
+v -2.357 4.4119 0.999
+v -1.914 4.6219 0.999
+v -1.452 4.7879 0.999
+v 5.3571 0.0029 6.997
+v -0.976 4.9069 0.999
+v -0.49 4.9789 0.999
+v -3.172 3.8679 0.999
+v 4.6393 2.6814 6.997
+v 0.49 4.9789 2.7016
+v 0 5.0029 2.6637
+v 0.975 4.9069 2.6941
+v 1.2927 4.8274 2.6591
+v 4.828 4.8309 3.997
+v 2.2842 -6.0614 4.0939
+v -4.9704 1.5106 11.1312
+v -5.0256 1.3493 11.1413
+v 5.0251 1.3493 11.1412
+v -5.1562 0.5106 11.1223
+v -5.0863 1.0146 11.1257
+v 3.593 -6.2201 5.863
+v 0.5554 2.0757 0
+v 0 2.1489 0
+v 0 5.2078 11.139
+v 4.696 4.6989 4.031
+v -4.828 4.8309 7.997
+v 4.409 2.3599 0.999
+v 4.619 1.9159 0.999
+v 1.6082 2.7883 0.999
+v 1.451 4.7879 2.6816
+v 4.7851 1.9849 11.121
+v 1.913 4.6219 2.7036
+v 4.9698 1.5106 11.1311
+v 2.357 4.4119 2.6809
+v 2.4972 4.328 2.6584
+v 3.7731 -5.2654 8.0941
+v 4.1127 5.0106 4.0942
+v 4.619 4.6219 4.0941
+v -1.0117 5.0889 11.1255
+v -4.697 4.6989 8.031
+v 1.073 1.8614 0
+v -0.5077 5.1589 11.1222
+v -5.318 5.3209 4.738
+v 3.506 -6.0711 8.031
+v -5.335 5.3379 4.997
+v -4.6192 4.6217 8.0942
+v -4.6191 4.6219 8.0942
+v 3.414 -5.9101 7.997
+v 4.785 1.4539 0.999
+v 4.904 0.9779 0.999
+v 4.976 0.4929 0.999
+v 2.2742 2.2771 0.999
+v 3.772 -6.5311 4.997
+v 2.778 4.1599 2.6938
+v -5.267 5.2699 4.497
+v 2.8644 4.0959 2.6955
+v 3.172 3.8679 2.7016
+v 3.535 3.5379 2.6632
+v -5.187 5.1889 4.29
+v 6.415 1.7219 9.963
+v 3.321 -5.7491 8.031
+v 1.5174 1.5203 0
+v -5.0077 4.1155 8.0942
+v -5.187 5.1889 5.704
+v 3.593 6.2259 5.863
+v 3.2663 -5.6544 8.0941
+v 6.595 1.7699 9.997
+v 3.2661 -5.6545 8.0941
+v -5.267 5.2699 5.497
+v 3.772 6.5369 4.997
+v 6.774 1.8179 9.963
+v 3.667 6.3549 5.704
+v 5 0.0029 0.999
+v 2.7854 1.6111 0.999
+v 4.976 -0.4871 0.999
+v 4.904 -0.9731 0.999
+v -5.318 5.3209 5.256
+v 3.865 3.1749 2.7016
+v 3.667 -6.3491 5.704
+v 4.157 2.7809 2.6938
+v 4.3251 2.5 2.6584
+v 6.3933 1.0597 9.8999
+v 3.724 -6.4481 5.497
+v 6.3096 1.6935 9.8999
+v 6.3096 1.6935 9.8999
+v 2.6748 -5.8996 9.8999
+v 1.8585 1.0759 0
+v 6.941 1.8629 9.863
+v 3.76 -6.5101 5.256
+v -4.1127 5.0106 5.8998
+v 3.414 5.9159 7.997
+v 7.085 1.9009 9.704
+v 2.2842 -6.0614 8.0939
+v 3.724 6.4539 5.497
+v 3.1066 0.8353 0.999
+v 3.76 -6.5101 4.738
+v -4.697 4.6989 5.963
+v 4.785 -1.4491 0.999
+v 4.619 -1.9111 0.999
+v 3.321 5.7549 8.031
+v 4.409 2.3599 2.6809
+v 3.724 -6.4481 4.497
+v 4.619 1.9159 2.7036
+v 3.76 6.5159 5.256
+v -4.6191 4.6219 5.8998
+v 4.785 1.4539 2.6816
+v -4.6192 4.6217 5.8998
+v 4.8246 1.2956 2.6591
+v -4.828 4.8309 5.997
+v 3.772 -6.5311 8.997
+v 3.667 -6.3491 4.29
+v 3.593 -6.2201 9.863
+v 2.0728 0.5583 0
+v 3.2661 5.6602 8.0941
+v 3.2663 5.6601 8.0941
+v 3.593 -6.2201 4.131
+v -4.959 4.9619 5.963
+v 3.667 -6.3491 9.704
+v 3.76 6.5159 4.738
+v -5.082 5.0839 5.863
+v 3.724 -6.4481 9.497
+v 3.2164 0.0029 0.999
+v 4.409 -2.3541 0.999
+v 3.724 6.4539 4.497
+v 4.157 -2.7751 0.999
+v 3.865 -3.1691 0.999
+v -6.074 3.5089 8.031
+v 4.904 0.9779 2.6941
+v 4.976 0.4929 2.7016
+v 5 0.0029 2.6637
+v -5.082 5.0839 4.131
+v 3.76 -6.5101 9.256
+v 3.667 6.3549 4.29
+v -4.959 4.9619 4.031
+v 2.6746 5.9051 8.0941
+v -4.828 4.8309 3.997
+v 3.593 6.2259 4.131
+v 2.1461 0.0029 0
+v 3.76 -6.5101 8.738
+v 3.724 -6.4481 8.497
+v 3.216 -0.0001 0.999
+v 3.535 -3.5331 0.999
+v 3.172 -3.8621 0.999
+v -4.697 4.6989 4.031
+v 2.778 -4.1541 0.999
+v 3.1066 -0.8296 0.999
+v 4.9999 -0.0001 2.6639
+v 4.976 -0.4871 2.7016
+v 4.904 -0.9731 2.6942
+v 4.8248 -1.2899 2.6593
+v 3.667 -6.3491 8.29
+v 7.195 1.9309 9.497
+v 1.953 7.2909 8.997
+v 3.593 -6.2201 8.131
+v -4.6192 4.6217 4.0942
+v -4.6191 4.6219 4.0942
+v 2.0728 -0.5525 0
+v -6.0642 2.2871 9.9001
+v 2.357 -4.4071 0.999
+v 1.913 -4.6171 0.999
+v 2.7854 -1.6054 0.999
+v 4.785 -1.4491 2.682
+v 4.619 -1.9111 2.704
+v 4.409 -2.3541 2.6809
+v 1.946 7.2669 9.256
+v -6.534 3.7749 8.997
+v 4.3251 -2.4943 2.6584
+v 1.0536 -6.3912 5.9001
+v -5.0077 4.1155 4.0942
+v 1.6906 -6.3075 5.9001
+v 1.6907 -6.3074 5.9001
+v 1.719 -6.4131 5.963
+v 1.8585 -1.0702 0
+v -6.513 3.7629 9.256
+v 1.767 -6.5921 5.997
+v 1.946 7.2669 8.738
+v 0.49 -4.9731 0.999
+v 2.2742 -2.2714 0.999
+v 1.451 -4.7821 0.999
+v 0.975 -4.9011 0.999
+v 4.157 -2.7751 2.6938
+v 1.815 -6.7711 5.963
+v 4.093 -2.8615 2.6955
+v 1.928 7.1979 8.497
+v 3.865 -3.1691 2.7016
+v 3.5355 -3.5326 2.6639
+v 1.86 -6.9381 5.863
+v -6.513 3.7629 8.738
+v 1.953 -7.2851 4.997
+v 1.898 -7.0821 5.704
+v 1.5174 -1.5146 0
+v 1.898 7.0879 8.29
+v -6.451 3.7269 8.497
+v 1.898 7.0879 8.29
+v 1.898 7.0879 8.29
+v 1.928 -7.1921 5.497
+v 3.7746 5.2698 5.9
+v 1.86 6.9439 8.131
+v 1.898 7.0879 8.29
+v -6.352 3.6699 8.29
+v 0 -4.9971 0.999
+v 1.6082 -2.7826 0.999
+v 3.2663 5.6601 5.8999
+v 1.946 -7.2611 5.256
+v -0.49 -4.9731 0.999
+v -0.976 -4.9011 0.999
+v 3.2661 5.6602 5.8999
+v 3.321 5.7549 5.963
+v 3.535 -3.5331 2.664
+v 3.172 -3.8621 2.7016
+v 1.815 6.7769 8.031
+v 2.778 -4.1541 2.6938
+v 2.4975 -4.3227 2.6589
+v -6.224 3.5959 8.131
+v 1.719 -6.4131 9.963
+v 3.414 5.9159 5.997
+v 1.898 -7.0821 4.29
+v 1.767 -6.5921 9.997
+v 3.506 6.0759 5.963
+v 1.073 -1.8556 0
+v 1.86 -6.9381 4.131
+v 1.815 -6.7711 9.963
+v 0.8324 -3.1042 0.999
+v -5.2683 3.7759 5.8999
+v -1.452 -4.7821 0.999
+v -1.914 -4.6171 0.999
+v 1.815 -6.7711 4.031
+v 2.357 -4.4071 2.6817
+v 1.913 -4.6171 2.7042
+v 1.451 -4.7821 2.6811
+v 1.953 -7.2851 8.997
+v 1.2926 -4.8218 2.6585
+v 3.506 6.0759 4.031
+v -5.752 3.3239 5.963
+v 1.767 -6.5921 3.997
+v 0 6.6449 9.963
+v 1.0536 -6.3912 9.9001
+v 1.6906 -6.3075 9.9001
+v -5.6572 3.2692 5.8999
+v -5.6573 3.269 5.8999
+v 1.6907 -6.3074 9.9001
+v 0.5554 -2.0702 0
+v 3.414 5.9159 3.997
+v 1.719 -6.4131 4.031
+v 1.86 -6.9381 9.863
+v -5.913 3.4169 5.997
+v 0 6.8299 9.997
+v 1.898 -7.0821 9.704
+v -6.074 3.5089 5.963
+v 3.321 5.7549 4.031
+v 0 7.0159 9.963
+v 0 -3.2136 0.999
+v -2.357 -4.4071 0.999
+v -2.778 -4.1541 0.999
+v -3.172 -3.8621 0.999
+v 0.975 -4.9011 2.6936
+v 1.928 -7.1921 9.497
+v 0.49 -4.9731 2.7013
+v 0 -4.9971 2.6637
+v 0 7.1889 9.863
+v 3.2661 5.6602 4.0941
+v 3.2663 5.6601 4.0941
+v 1.6907 -6.3074 4.0939
+v 1.6906 -6.3075 4.0939
+v 1.946 -7.2611 9.256
+v -6.416 1.7219 9.963
+v 0 7.3379 9.704
+v 1.946 -7.2611 4.738
+v 0 -2.1432 0
+v 1.928 -7.1921 4.497
+v -6.595 1.7699 9.997
+v 0 7.4519 9.497
+v -0.003 -3.2132 0.999
+v -3.536 -3.5331 0.999
+v -3.865 -3.1691 0.999
+v -4.157 -2.7751 0.999
+v 1.898 -7.0821 8.29
+v -0.8324 -3.1042 0.999
+v -0.003 -4.997 2.6639
+v -6.774 1.8179 9.963
+v -0.49 -4.9731 2.7013
+v -0.976 -4.9011 2.6937
+v -1.2926 -4.822 2.6587
+v 0 -6.6391 5.963
+v 1.86 -6.9381 8.131
+v -6.074 3.5089 4.031
+v 0 -6.5298 5.8998
+v 2.2813 6.068 5.8999
+v -6.3103 1.6935 9.9001
+v -6.3103 1.6934 9.9001
+v -5.913 3.4169 3.997
+v 0 -6.8251 5.997
+v 0.6335 6.452 9.8999
+v -0.5554 -2.0702 0
+v 1.815 -6.7711 8.031
+v -6.941 1.8629 9.863
+v 0 -7.0101 5.963
+v -4.41 -2.3541 0.999
+v -4.62 -1.9111 0.999
+v -1.6082 -2.7826 0.999
+v 0 6.5356 9.8998
+v 1.767 -6.5921 7.997
+v -1.452 -4.7821 2.6815
+v 0 -7.1831 5.863
+v -1.914 -4.6171 2.7046
+v -7.085 1.9009 9.704
+v -2.357 -4.4071 2.6817
+v -5.752 3.3239 4.031
+v -2.4975 -4.3227 2.6589
+v 7.545 0.0029 8.997
+v 0 -7.5421 4.997
+v 1.719 -6.4131 8.031
+v -7.195 1.9309 9.497
+v 0 -7.3321 5.704
+v 7.52 0.0029 9.256
+v -5.6572 3.2692 4.0941
+v -5.6573 3.269 4.0941
+v -1.073 -1.8556 0
+v 0 -7.4461 5.497
+v -6.0642 2.2871 5.9001
+v 0 -7.5181 5.256
+v -4.976 -0.4871 0.999
+v -2.2742 -2.2714 0.999
+v -4.785 -1.4491 0.999
+v -4.904 -0.9731 0.999
+v -2.778 -4.1541 2.6938
+v -3.172 -3.8621 2.7016
+v -3.536 -3.5331 2.6645
+v 1.6907 -6.3074 8.0939
+v 1.6906 -6.3075 8.0939
+v 0 -7.0101 4.031
+v -5.9025 2.6776 4.0941
+v 1.946 -7.2611 8.738
+v 2.6746 5.9051 4.0941
+v -2.6786 4.6421 6.997
+v -6.224 3.5959 5.863
+v 0 -6.8251 3.997
+v -1.5174 -1.5146 0
+v 1.928 -7.1921 8.497
+v -6.534 3.7749 4.997
+v -5.205 0.0029 11.139
+v -6.352 3.6699 5.704
+v 3.7879 3.7907 6.997
+v -5 0.0029 0.999
+v -4.976 0.4929 0.999
+v -4.904 0.9779 0.999
+v 7.52 0.0029 8.738
+v 0 -6.6391 4.031
+v -2.7854 -1.6054 0.999
+v -6.451 3.7269 5.497
+v 0 -6.6391 9.963
+v 7.449 0.0029 8.497
+v -6.513 3.7629 5.256
+v -3.865 -3.1691 2.7016
+v -5.0863 -1.0089 11.1257
+v -4.157 -2.7751 2.6938
+v -5.1562 -0.5049 11.1223
+v 0 -6.8251 9.997
+v -4.3256 -2.4946 2.6589
+v -7.071 7.0739 14.497
+v 0 -6.5298 4.0942
+v 0.6333 -6.4466 4.0942
+v 0 -7.0101 9.963
+v -1.8585 -1.0702 0
+v -6.513 3.7629 4.738
+v 0 -7.5181 4.738
+v -6.451 3.7269 4.497
+v 0 -7.4461 4.497
+v 7.335 0.0029 8.29
+v 0 -6.5298 9.8998
+v -4.785 1.4539 0.999
+v -4.62 1.9159 0.999
+v -5.913 3.4169 7.997
+v -3.1071 -0.8296 0.999
+v -6.352 3.6699 4.29
+v 0 -7.3321 4.29
+v 7.186 0.0029 8.131
+v -6.224 3.5959 4.131
+v 0 -7.1831 4.131
+v -4.41 -2.3541 2.6817
+v 0 -7.1831 9.863
+v -4.62 -1.9111 2.7046
+v -4.785 -1.4491 2.6815
+v 7.013 0.0029 8.031
+v -4.8248 -1.2898 2.6587
+v -5.752 3.3239 8.031
+v 0 -7.1831 9.863
+v 0 -7.1831 9.863
+v 0 -7.3321 9.704
+v 0 -7.1831 9.863
+v -1.3465 5.0279 11.1412
+v -2.0731 -0.5525 0
+v 0 -7.4461 9.497
+v -5.6572 3.2692 8.0941
+v -5.6573 3.269 8.0941
+v -4.41 2.3599 0.999
+v -4.157 2.7809 0.999
+v -3.865 3.1749 0.999
+v -3.2164 0.0029 0.999
+v -3.216 -0.0001 0.999
+v -1.9821 4.7879 11.121
+v -4.9999 -0.0001 2.6639
+v -5 0.0029 2.6637
+v -1.5077 4.9727 11.1311
+v 0 -7.0101 8.031
+v -5.9025 2.6776 8.0941
+v -4.904 -0.9731 2.6937
+v -4.976 -0.4871 2.7013
+v 1.719 6.4179 5.963
+v -2.1461 0.0029 0
+v 0 -6.8251 7.997
+v -3.1071 0.8353 0.999
+v 1.767 6.5979 7.997
+v -3.536 3.5379 0.999
+v -4.976 0.4929 2.7013
+v 1.6906 6.3125 5.8999
+v -4.904 0.9779 2.6936
+v 1.6906 6.3125 5.8999
+v -4.8246 1.2954 2.6585
+v 1.767 6.5979 5.997
+v 6.595 -1.7641 7.997
+v 1.815 6.7769 5.963
+v 1.719 6.4179 8.031
+v -2.0731 0.5583 0
+v -6.4495 -0.6304 4.0942
+v 1.86 6.9439 5.863
+v -6.3103 1.6934 5.9001
+v -6.3103 1.6935 5.9001
+v 6.415 -1.7161 8.031
+v -6.416 1.7219 5.963
+v -2.7854 1.6111 0.999
+v -6.394 -1.0508 5.9001
+v -4.785 1.4539 2.6811
+v -4.62 1.9159 2.7042
+v -6.595 1.7699 5.997
+v 1.0569 6.3961 8.0941
+v 1.6906 6.3125 8.0941
+v -6.3103 -1.6877 5.9001
+v 1.6906 6.3125 8.0941
+v -4.41 2.3599 2.6817
+v -6.3103 -1.6878 5.9001
+v -6.416 -1.7161 5.963
+v -4.3256 2.5003 2.6589
+v 6.3933 -1.054 8.0941
+v 1.953 7.2909 4.997
+v 6.3096 -1.6877 8.0941
+v 6.3096 -1.6878 8.0941
+v -6.774 1.8179 5.963
+v 1.898 7.0879 5.704
+v 0 7.5479 8.997
+v -6.595 -1.7641 5.997
+v -1.8585 1.0759 0
+v -6.941 1.8629 5.863
+v 0 7.5229 9.256
+v 1.928 7.1979 5.497
+v -6.774 -1.8121 5.963
+v -7.288 1.9559 8.997
+v -7.288 1.9559 4.997
+v 1.946 7.2669 5.256
+v 4.6393 -2.6757 6.997
+v 0 7.5229 8.738
+v -6.941 -1.8571 5.863
+v -2.2742 2.2771 0.999
+v 0 7.4519 8.497
+v -7.085 1.9009 5.704
+v -4.157 2.7809 2.6938
+v -7.288 -1.9501 4.997
+v -3.865 3.1749 2.7016
+v -3.536 3.5379 2.664
+v -7.085 -1.8951 5.704
+v -3.5355 3.5383 2.6639
+v 0 7.3379 8.29
+v 1.946 7.2669 4.738
+v -7.195 1.9309 5.497
+v -1.5174 1.5203 0
+v -7.195 -1.9251 5.497
+v -7.264 1.9489 9.256
+v 0 7.1889 8.131
+v 1.928 7.1979 4.497
+v -7.264 1.9489 5.256
+v -7.264 -1.9441 5.256
+v -1.6082 2.7883 0.999
+v -2.778 4.1599 2.6938
+v -2.8644 4.0959 2.6955
+v -2.4972 4.328 2.6584
+v 0 7.0159 8.031
+v 1.898 7.0879 4.29
+v 4.5804 2.4512 11.1312
+v 4.5055 2.6042 11.1412
+v -7.085 1.9009 8.29
+v -3.172 3.8679 2.7016
+v -7.085 1.9009 4.29
+v 1.898 7.0879 4.29
+v 1.898 7.0879 4.29
+v -7.085 1.9009 8.29
+v -1.073 1.8614 0
+v -7.085 -1.8951 4.29
+v -7.085 1.9009 4.29
+v 0 -6.6391 8.031
+v -7.085 1.9009 4.29
+v -7.085 1.9009 8.29
+v -7.085 1.9009 8.29
+v -6.941 1.8629 4.131
+v -7.085 1.9009 4.29
+v 4.3118 2.884 11.1255
+v -6.941 1.8629 8.131
+v -7.085 1.9009 8.29
+v -6.941 -1.8571 4.131
+v 4.0048 3.2896 11.1222
+v -6.774 1.8179 4.031
+v -0.8324 3.1095 0.999
+v -2.357 4.4119 2.6809
+v -6.774 1.8179 8.031
+v 0 -6.5298 8.0942
+v -1.914 4.6219 2.704
+v 0.6333 -6.4466 8.0942
+v -1.452 4.7879 2.682
+v -6.774 -1.8121 4.031
+v -1.2928 4.8277 2.6593
+v -0.5554 2.0757 0
+v -6.595 1.7699 3.997
+v -6.595 -1.7641 3.997
+v 0 -7.4461 8.497
+v -0.003 3.2189 0.999
+v 6.223 3.5959 9.863
+v -0.003 5.0027 2.6639
+v 0 -7.3321 8.29
+v -6.416 1.7219 4.031
+v -0.976 4.9069 2.6942
+v -6.416 -1.7161 4.031
+v 6.352 3.6699 9.704
+v -0.49 4.9789 2.7016
+v 0 -7.1831 8.131
+v -6.394 1.0565 4.0939
+v 6.451 3.7269 9.497
+v -6.3103 1.6934 4.0939
+v -6.3103 1.6935 4.0939
+v 1.86 6.9439 4.131
+v 1.898 7.0879 4.29
+v 8.3126 5.5573 14.495
+v 8.8165 4.7155 14.495
+v -7.264 1.9489 8.738
+v 9.2364 3.8288 14.495
+v -6.3103 -1.6878 4.0939
+v -6.3103 -1.6877 4.0939
+v 9.5663 2.905 14.495
+v -7.264 1.9489 4.738
+v 9.8052 1.9533 14.495
+v 9.9492 0.9826 14.495
+v 9.9971 0.0029 14.495
+v -7.195 1.9309 8.497
+v -7.195 1.9309 4.497
+v -7.264 -1.9441 4.738
+v 7.7278 6.345 14.495
+v -7.195 -1.9251 4.497
+v -6.4495 0.6361 5.8998
+v -5.752 -3.3181 9.963
+v -5.9025 -2.6719 5.8999
+v -6.642 0.0029 9.963
+v 9.9492 -0.9769 14.495
+v 9.8052 -1.9476 14.495
+v 9.5663 -2.8993 14.495
+v 9.2364 -3.823 14.495
+v -6.642 0.0029 5.963
+v 8.8165 -4.7098 14.495
+v 8.3126 -5.5516 14.495
+v 7.7278 -6.3393 14.495
+v -5.913 -3.4111 9.997
+v -6.5327 0.0029 5.8998
+v -6.503 7.5887 14.497
+v 0 10 14.495
+v 0.9797 9.952 14.495
+v 1.9504 9.8081 14.495
+v -6.074 -3.5041 9.963
+v 2.9022 9.5691 14.495
+v -6.828 0.0029 9.997
+v 3.8259 9.2392 14.495
+v -1.719 6.4179 9.963
+v 4.7127 8.8193 14.495
+v -6.0642 -2.2814 4.0939
+v 5.5544 8.3155 14.495
+v -6.828 0.0029 5.997
+v -7.013 0.0029 9.963
+v -7.013 0.0029 5.963
+v -1.767 6.5979 9.997
+v 6.3422 7.7307 14.495
+v 1.815 6.7769 4.031
+v -7.186 0.0029 5.863
+v -5.6573 -3.2632 9.8999
+v -5.6572 -3.2634 9.8999
+v -6.503 7.5849 14.495
+v -6.3422 7.7307 14.495
+v -1.815 6.7769 9.963
+v -5.5544 8.3155 14.495
+v -7.545 0.0029 4.997
+v -6.4495 0.6361 9.8998
+v -4.7127 8.8193 14.495
+v -3.8259 9.2392 14.495
+v -2.9022 9.5691 14.495
+v -1.9504 9.8081 14.495
+v -7.335 0.0029 5.704
+v -0.9797 9.952 14.495
+v -6.5327 0.0029 9.8998
+v -1.0569 6.3961 9.8999
+v -5.752 -3.3181 5.963
+v -7.449 0.0029 5.497
+v 5.752 3.3239 9.963
+v -1.6906 6.3125 9.8999
+v -1.6906 6.3125 9.8999
+v -7.521 0.0029 5.256
+v -5.6573 -3.2632 5.8999
+v -5.6572 -3.2634 5.8999
+v -6.224 -3.5901 9.863
+v 5.913 3.4169 9.997
+v -1.86 6.9439 9.863
+v -5.913 -3.4111 5.997
+v -6.352 -3.6641 9.704
+v -1.899 7.0879 9.704
+v -7.186 0.0029 9.863
+v 6.073 3.5089 9.963
+v -6.074 -3.5041 5.963
+v -7.013 0.0029 4.031
+v -6.451 -3.7211 9.497
+v -7.186 0.0029 9.863
+v -7.186 0.0029 9.863
+v -1.928 7.1979 9.497
+v -7.335 0.0029 9.704
+v -7.186 0.0029 9.863
+v -6.828 0.0029 3.997
+v 5.6573 3.269 9.8999
+v 5.6572 3.2692 9.8999
+v -7.545 0.0029 8.997
+v -7.521 0.0029 9.256
+v -7.449 0.0029 9.497
+v -6.074 -3.5041 4.031
+v -6.642 0.0029 4.031
+v -5.913 -3.4111 3.997
+v -7.521 0.0029 8.738
+v -6.5327 0.0029 4.0942
+v 5.9022 2.6774 9.8999
+v -5.752 -3.3181 4.031
+v -7.521 0.0029 4.738
+v -0.6333 -6.4466 9.8998
+v -7.449 0.0029 4.497
+v 7.288 1.9559 8.997
+v -5.6572 -3.2634 4.0941
+v -5.6573 -3.2632 4.0941
+v -7.335 0.0029 4.29
+v 7.264 1.9489 9.256
+v -7.186 0.0029 4.131
+v 2.6786 4.6421 6.997
+v 1.767 6.5979 3.997
+v 7.264 1.9489 8.738
+v 7.195 1.9309 8.497
+v 6.4491 -0.6307 5.8999
+v -5.269 -3.7694 4.0942
+v 1.719 6.4179 4.031
+v 0 -7.5421 8.997
+v 7.085 1.9009 8.29
+v 0 -7.5181 9.256
+v -3.7879 3.7907 6.997
+v 6.941 1.8629 8.131
+v -5.0256 -1.3436 11.1413
+v 0 -7.5181 8.738
+v 6.774 1.8179 8.031
+v 1.0569 6.3961 4.0941
+v 1.6906 6.3125 4.0941
+v 1.6906 6.3125 4.0941
+v -2.4483 4.5832 11.1312
+v -2.6013 4.5083 11.1412
+v -4.9704 -1.5049 11.1312
+v 0 -5.3543 6.997
+v -3.2868 4.0077 11.1222
+v -2.8811 4.3147 11.1255
+v -7.071 -7.0681 14.497
+v 7.069 -7.0661 14.495
+v -6.224 -3.5901 5.863
+v -1.719 -6.4131 9.963
+v 0 6.8299 7.997
+v -6.595 1.7699 7.997
+v 1.0118 -5.0835 11.1257
+v 1.3464 -5.0228 11.1413
+v 0.5077 -5.1533 11.1223
+v 6.415 -1.7161 5.963
+v 0 -5.2021 11.139
+v 0.6335 6.452 5.8999
+v -1.767 -6.5921 9.997
+v 6.3096 -1.6878 5.8999
+v 6.3096 -1.6877 5.8999
+v 0 6.6449 8.031
+v 6.827 0.0029 7.997
+v -1.815 -6.7711 9.963
+v -6.416 1.7219 8.031
+v 0 6.5356 5.8998
+v 6.595 -1.7641 5.997
+v 0 6.6449 5.963
+v -1.953 -7.2851 8.997
+v 6.774 -1.8121 5.963
+v 0 6.5356 8.0942
+v 6.642 0.0029 8.031
+v 0 6.8299 5.997
+v -6.394 1.0565 8.0939
+v -1.6907 -6.3074 9.9001
+v -1.6906 -6.3075 9.9001
+v 6.941 -1.8571 5.863
+v -6.534 -3.7701 4.997
+v -6.3103 1.6934 8.0939
+v -1.86 -6.9381 9.863
+v -6.3103 1.6935 8.0939
+v 6.4491 0.6364 8.0941
+v 0 7.0159 5.963
+v 6.5327 0.0029 8.0942
+v 7.288 -1.9501 4.997
+v -1.899 -7.0821 9.704
+v 0 7.1889 5.863
+v 7.085 -1.8951 5.704
+v 0 7.5479 4.997
+v -1.928 -7.1921 9.497
+v 7.195 -1.9251 5.497
+v 0 7.3379 5.704
+v -7.013 0.0029 8.031
+v 7.264 -1.9441 5.256
+v -0.6335 6.452 8.0941
+v -1.947 -7.2611 9.256
+v -6.352 -3.6641 5.704
+v -1.953 7.2909 8.997
+v -6.451 -3.7211 5.497
+v 2.4484 -4.5775 11.1312
+v 5.1743 -1.3836 6.997
+v 2.6013 -4.5026 11.1412
+v 1.9822 -4.7824 11.1211
+v 7.264 -1.9441 4.738
+v 1.5077 -4.9675 11.1312
+v -1.899 -7.0821 8.29
+v -1.947 7.2669 9.256
+v -6.513 -3.7571 5.256
+v 7.195 -1.9251 4.497
+v -1.86 -6.9381 8.131
+v -6.513 -3.7571 4.738
+v -1.947 7.2669 8.738
+v -1.815 -6.7711 8.031
+v -6.451 -3.7211 4.497
+v 7.085 -1.8951 4.29
+v -7.449 0.0029 8.497
+v -1.928 7.1979 8.497
+v 7.085 -1.8952 4.29
+v 7.085 -1.8952 4.29
+v -6.352 -3.6641 4.29
+v 6.941 -1.8571 4.131
+v -1.767 -6.5921 7.997
+v 3.6803 3.6832 11.1389
+v 7.085 -1.8952 4.29
+v -7.335 0.0029 8.29
+v -6.224 -3.5901 4.131
+v 6.774 -1.8121 4.031
+v 3.6803 -3.6775 11.1389
+v -1.899 7.0879 8.29
+v 6.595 -1.7641 3.997
+v 3.2868 -4.002 11.1222
+v -7.186 0.0029 8.131
+v 2.8811 -4.309 11.1255
+v -1.719 -6.4131 8.031
+v 2.8811 4.3147 11.1255
+v -1.86 6.9439 8.131
+v -0.6333 -6.4466 5.8998
+v 3.2868 4.0077 11.1222
+v -1.0536 -6.3912 8.0939
+v 6.415 -1.7161 4.031
+v 0 7.4519 5.497
+v -1.6906 -6.3075 8.0939
+v -1.815 6.7769 8.031
+v -1.6907 -6.3074 8.0939
+v 0 7.5229 5.256
+v 6.3933 -1.054 4.0941
+v 6.3096 -1.6877 4.0941
+v 6.3096 -1.6878 4.0941
+v -1.947 -7.2611 8.738
+v -1.928 -7.1921 8.497
+v 4.3118 -2.8782 11.1255
+v 4.5055 -2.5985 11.1412
+v 4.0048 -3.2839 11.1222
+v 0 7.5229 4.738
+v 0 7.4519 4.497
+v 5.186 5.1889 9.704
+v 0 7.3379 4.29
+v 5.267 5.2699 9.497
+v 0 7.1889 4.131
+v 4.9698 -1.5049 11.1311
+v 5.0251 -1.3436 11.1412
+v 4.7851 -1.9792 11.121
+v 4.5804 -2.4455 11.1312
+v -2.2842 -6.0614 9.9001
+v 0 7.0159 4.031
+v 0 6.8299 3.997
+v 5.156 -0.5049 11.1222
+v 5.086 -1.0089 11.1255
+v -2.6767 -5.8989 8.0942
+v -3.321 -5.7491 9.963
+v -6.416 -1.7161 9.963
+v 6.223 -3.5901 5.863
+v -3.414 -5.9101 9.997
+v -6.595 -1.7641 9.997
+v 6.534 -3.7701 4.997
+v -3.507 -6.0711 9.963
+v 6.352 -3.6641 5.704
+v -6.774 -1.8121 9.963
+v 6.451 -3.7211 5.497
+v -3.7709 -5.2674 9.8997
+v 6.513 -3.7571 5.256
+v 0 6.6449 4.031
+v -3.2666 -5.6543 9.8998
+v -3.2664 -5.6545 9.8998
+v 4.696 4.6989 9.963
+v -6.394 -1.0508 9.9001
+v -6.3103 -1.6877 9.9001
+v -6.3103 -1.6878 9.9001
+v 6.513 -3.7571 4.738
+v 4.828 4.8309 9.997
+v 0 6.5356 4.0942
+v -6.941 -1.8571 9.863
+v 4.959 4.9619 9.963
+v 6.451 -3.7211 4.497
+v 6.223 3.5959 5.863
+v -3.593 6.2259 9.863
+v -7.085 -1.8951 9.704
+v 5.0077 4.1155 9.8998
+v 6.352 -3.6641 4.29
+v -3.668 6.3549 9.704
+v 4.619 4.6219 9.8999
+v 6.223 -3.5901 4.131
+v -3.725 6.4539 9.497
+v -7.195 -1.9251 9.497
+v 5.081 5.0839 9.863
+v -3.507 -6.0711 8.031
+v 6.534 3.7749 8.997
+v 6.0651 -2.2784 5.8999
+v -1.6906 -6.3075 5.9001
+v -1.6907 -6.3074 5.9001
+v -1.719 -6.4131 5.963
+v -3.414 -5.9101 7.997
+v 6.513 3.7629 9.256
+v -1.767 -6.5921 5.997
+v -4.5804 -2.4455 11.1312
+v -4.5055 -2.5985 11.1412
+v -4.7853 -1.9793 11.1211
+v 6.513 3.7629 8.738
+v 5.9022 -2.6717 4.0941
+v -1.815 -6.7711 5.963
+v -3.321 -5.7491 8.031
+v 6.451 3.7269 8.497
+v 5.2677 -3.7709 5.8999
+v 6.352 3.6699 8.29
+v 6.534 3.7749 4.997
+v -1.86 -6.9381 5.863
+v 5.6572 -3.2634 5.8999
+v -2.6765 5.9044 9.8998
+v 5.6573 -3.2632 5.8999
+v 5.752 -3.3181 5.963
+v -3.2664 -5.6545 8.0942
+v -3.2666 -5.6543 8.0942
+v 6.223 3.5959 8.131
+v -3.321 5.7549 9.963
+v -1.953 -7.2851 4.997
+v 5.913 -3.4111 5.997
+v -3.6803 -3.6775 11.1389
+v -1.899 -7.0821 5.704
+v -3.414 5.9159 9.997
+v -4.0048 -3.2839 11.1222
+v -4.3118 -2.8782 11.1255
+v 6.073 -3.5041 5.963
+v -1.928 -7.1921 5.497
+v -4.6393 2.6814 6.997
+v -3.507 6.0759 9.963
+v -6.828 0.0029 7.997
+v -1.947 -7.2611 5.256
+v 6.073 -3.5041 4.031
+v 5.913 -3.4111 3.997
+v -2.8811 -4.309 11.1255
+v -3.2664 5.6602 9.8998
+v -2.6013 -4.5026 11.1412
+v -3.2666 5.66 9.8998
+v -3.2868 -4.002 11.1222
+v -6.642 0.0029 8.031
+v -1.899 -7.0821 4.29
+v 5.752 -3.3181 4.031
+v -1.86 -6.9381 4.131
+v -6.5327 0.0029 8.0942
+v -1.815 -6.7711 4.031
+v -1.5077 -4.9675 11.1312
+v -1.3464 -5.0228 11.1413
+v -1.9822 -4.7824 11.1211
+v -2.4484 -4.5775 11.1312
+v 5.6573 -3.2632 4.0941
+v 5.6572 -3.2634 4.0941
+v -1.767 -6.5921 3.997
+v -0.5077 -5.1533 11.1223
+v -1.0118 -5.0835 11.1257
+v 6.073 3.5089 8.031
+v -1.719 -6.4131 4.031
+v -3.773 -6.5311 8.997
+v -3.593 -6.2201 9.863
+v -3.6803 3.6832 11.1389
+v -6.4495 -0.6304 8.0942
+v -3.668 -6.3491 9.704
+v -1.0536 -6.3912 4.0939
+v -1.6906 -6.3075 4.0939
+v -1.6907 -6.3074 4.0939
+v -4.3118 2.884 11.1255
+v -7.288 -1.9501 8.997
+v -4.0048 3.2896 11.1222
+v 6.352 3.6699 5.704
+v -1.947 -7.2611 4.738
+v -1.928 -7.1921 4.497
+v 6.451 3.7269 5.497
+v 6.415 -1.7161 9.963
+v -7.264 -1.9441 9.256
+v 6.0651 2.2841 8.0941
+v -1.767 6.5979 7.997
+v 6.595 -1.7641 9.997
+v 6.513 3.7629 5.256
+v 6.774 -1.8121 9.963
+v -7.085 -1.8951 8.29
+v 6.595 1.7699 7.997
+v 6.513 3.7629 4.738
+v -2.2842 -6.0614 5.9001
+v -1.719 6.4179 8.031
+v 6.451 3.7269 4.497
+v 6.3096 -1.6878 9.8999
+v 6.3096 -1.6877 9.8999
+v -6.941 -1.8571 8.131
+v 6.352 3.6699 4.29
+v 6.415 1.7219 8.031
+v 6.941 -1.8571 9.863
+v 6.223 3.5959 4.131
+v 7.085 -1.8951 9.704
+v -1.6906 6.3125 8.0941
+v -6.774 -1.8121 8.031
+v -1.6906 6.3125 8.0941
+v -2.6767 -5.8989 4.0942
+v 6.3096 1.6935 8.0941
+v 6.3096 1.6935 8.0941
+v 7.195 -1.9251 9.497
+v -3.7709 -5.2674 5.8997
+v -3.321 -5.7491 5.963
+v -3.2664 -5.6545 5.8998
+v 4.1127 -5.0049 5.8998
+v -3.2666 -5.6543 5.8998
+v -3.414 -5.9101 5.997
+v 4.696 -4.6941 5.963
+v -3.507 -6.0711 5.963
+v 4.6189 -4.6164 5.8998
+v 4.6191 -4.6162 5.8998
+v -3.773 6.5369 8.997
+v -3.725 -6.4481 9.497
+v 4.828 -4.8251 5.997
+v 2.6013 4.5083 11.1412
+v -7.264 -1.9441 8.738
+v -3.76 -6.5101 9.256
+v -3.76 6.5159 9.256
+v 4.959 -4.9561 5.963
+v -7.195 -1.9251 8.497
+v 5.081 -5.0781 5.863
+v 1.9821 4.7879 11.121
+v 2.4483 4.5832 11.1312
+v -3.76 6.5159 8.738
+v -3.725 6.4539 8.497
+v -3.76 -6.5101 8.738
+v 6.534 -3.7701 8.997
+v -3.725 -6.4481 8.497
+v 6.223 -3.5901 9.863
+v -3.668 6.3549 8.29
+v 5.0077 -4.1098 4.0942
+v 6.352 -3.6641 9.704
+v -3.668 -6.3491 8.29
+v 5.318 5.3209 8.738
+v -3.507 -6.0711 4.031
+v 5.081 -5.0781 4.131
+v -5.9025 -2.6719 9.8999
+v 5.335 5.3379 8.997
+v 6.451 -3.7211 9.497
+v 4.959 -4.9561 4.031
+v -3.593 -6.2201 8.131
+v -3.593 6.2259 8.131
+v -3.414 -5.9101 3.997
+v 5.267 5.2699 8.497
+v 6.513 -3.7571 9.256
+v 4.828 -4.8251 3.997
+v 5.186 5.1889 8.29
+v -4.697 -4.6941 9.963
+v -3.321 -5.7491 4.031
+v 4.696 -4.6941 4.031
+v 6.513 -3.7571 8.738
+v 5.318 5.3209 9.256
+v -4.828 -4.8251 9.997
+v -3.2664 -5.6545 4.0942
+v -2.2813 6.068 8.0941
+v -3.2666 -5.6543 4.0942
+v 6.451 -3.7211 8.497
+v 4.6191 -4.6162 4.0942
+v 4.6189 -4.6164 4.0942
+v -4.959 -4.9561 9.963
+v 6.352 -3.6641 8.29
+v 5.318 -5.3151 4.738
+v -5.0077 -4.1098 9.8998
+v 5.335 -5.3321 4.997
+v 6.223 -3.5901 8.131
+v 5.267 -5.2641 4.497
+v -4.6191 -4.6163 9.8997
+v -5.082 -5.0781 9.863
+v 5.186 -5.1841 4.29
+v -5.082 -5.0781 8.131
+v 5.186 -5.1841 5.704
+v 6.0651 -2.2784 9.8999
+v -4.959 -4.9561 8.031
+v 5.752 -3.3181 9.963
+v 5.267 -5.2641 5.497
+v 5.318 -5.3151 5.256
+v -3.507 6.0759 8.031
+v 5.913 -3.4111 9.997
+v 6.073 -3.5041 9.963
+v -3.593 -6.2201 5.863
+v 5.2677 -3.7709 9.8999
+v -3.773 -6.5311 4.997
+v -5.175 1.3893 6.997
+v -3.668 -6.3491 5.704
+v 5.6572 -3.2634 9.8999
+v 5.6573 -3.2632 9.8999
+v -6.595 -1.7641 7.997
+v -3.725 -6.4481 5.497
+v 5.6572 3.2692 5.8999
+v 5.6573 3.269 5.8999
+v -3.76 -6.5101 5.256
+v 5.752 3.3239 5.963
+v 6.073 -3.5041 8.031
+v -6.416 -1.7161 8.031
+v 5.913 3.4169 5.997
+v 5.081 5.0839 8.131
+v 4.959 4.9619 8.031
+v -3.76 -6.5101 4.738
+v 6.073 3.5089 5.963
+v -3.725 -6.4481 4.497
+v -6.3103 -1.6878 8.0939
+v -6.3103 -1.6877 8.0939
+v -3.668 -6.3491 4.29
+v -3.593 -6.2201 4.131
+v -6.0642 -2.2814 8.0939
+v -5.0077 -4.1098 5.8998
+v 5.267 3.7775 4.094
+v -4.6191 -4.6163 5.8997
+v 3.2663 -5.6544 5.8999
+v 3.2661 -5.6545 5.8999
+v -4.697 -4.6941 5.963
+v 3.593 6.2259 9.863
+v 3.321 -5.7491 5.963
+v -4.1127 -5.0049 8.0942
+v -6.074 -3.5041 8.031
+v -5.187 5.1889 9.704
+v -4.828 -4.8251 5.997
+v 3.414 -5.9101 5.997
+v 3.667 6.3549 9.704
+v -5.267 5.2699 9.497
+v 3.724 6.4539 9.497
+v 3.506 -6.0711 5.963
+v -4.959 -4.9561 5.963
+v 6.073 3.5089 4.031
+v -5.082 -5.0781 5.863
+v 5.913 3.4169 3.997
+v -4.697 4.6989 9.963
+v 5.752 3.3239 4.031
+v 3.321 5.7549 9.963
+v -4.828 4.8309 9.997
+v 3.7731 -5.2654 4.0941
+v 5.6572 3.2692 4.0941
+v 5.6573 3.269 4.0941
+v 3.414 5.9159 9.997
+v -4.959 4.9619 9.963
+v -5.082 -5.0781 4.131
+v 3.506 -6.0711 4.031
+v 3.506 6.0759 9.963
+v -4.959 -4.9561 4.031
+v -4.1127 5.0106 9.8998
+v 5.9022 2.6774 5.8999
+v -4.828 -4.8251 3.997
+v 3.7746 5.2698 9.9
+v -6.534 -3.7701 8.997
+v -4.6191 4.6219 9.8998
+v 3.2663 5.6601 9.8999
+v -4.6192 4.6217 9.8998
+v 3.2661 5.6602 9.8999
+v -6.513 -3.7571 9.256
+v -4.697 -4.6941 4.031
+v -5.082 5.0839 9.863
+v -4.1127 -5.0049 4.0942
+v -6.513 -3.7571 8.738
+v -4.6191 -4.6163 4.0943
+v -6.451 -3.7211 8.497
+v 4.696 -4.6941 9.963
+v 5.267 3.7775 8.094
+v 5.913 3.4169 7.997
+v -6.352 -3.6641 8.29
+v 4.828 -4.8251 9.997
+v 5.752 3.3239 8.031
+v 4.959 -4.9561 9.963
+v -6.224 -3.5901 8.131
+v 5.6572 3.2692 8.0941
+v 4.1127 -5.0049 9.8998
+v 5.6573 3.269 8.0941
+v 4.6189 -4.6164 9.8998
+v 4.6191 -4.6162 9.8998
+v 5.081 -5.0781 9.863
+v -4.5055 2.6042 11.1412
+v 1.5077 4.9727 11.1311
+v 1.3465 5.0279 11.1412
+v -4.7853 1.985 11.1211
+v 0.5077 5.1589 11.1222
+v 1.0117 5.0889 11.1255
+v -4.5804 2.4512 11.1312
+v 5.0077 -4.1098 8.0942
+v 5.081 -5.0781 8.131
+v 6.0651 2.2841 4.0941
+v -5.3571 0.0029 6.997
+v 4.959 -4.9561 8.031
+v 6.3933 1.0597 5.8999
+v 4.828 -4.8251 7.997
+v -3.414 5.9159 7.997
+v 6.415 1.7219 5.963
+v -5.913 -3.4111 7.997
+v 6.3096 1.6935 5.8999
+v 6.3096 1.6935 5.8999
+v -5.318 -5.3151 8.738
+v 4.696 -4.6941 8.031
+v -3.321 5.7549 8.031
+v -5.335 -5.3321 8.997
+v 6.595 1.7699 5.997
+v 4.828 4.8309 7.997
+v -5.267 -5.2641 8.497
+v -5.752 -3.3181 8.031
+v 6.774 1.8179 5.963
+v -3.2664 5.6602 8.0942
+v 4.6191 -4.6162 8.0942
+v -3.2666 5.66 8.0942
+v 4.6189 -4.6164 8.0942
+v -5.187 -5.1841 8.29
+v 6.941 1.8629 5.863
+v 5.318 -5.3151 8.738
+v 4.696 4.6989 8.031
+v -5.6572 -3.2634 8.0941
+v 5.335 -5.3321 8.997
+v -5.187 -5.1841 9.704
+v -5.6573 -3.2632 8.0941
+v 7.288 1.9559 4.997
+v -5.318 -5.3151 4.738
+v 5.267 -5.2641 8.497
+v 7.085 1.9009 5.704
+v 4.1127 5.0106 8.0942
+v -5.335 -5.3321 4.997
+v 4.619 4.6219 8.0941
+v -5.267 -5.2641 9.497
+v -5.267 -5.2641 4.497
+v -3.7724 5.2718 8.0942
+v 5.186 -5.1841 8.29
+v 7.195 1.9309 5.497
+v -5.318 -5.3151 9.256
+v -5.187 -5.1841 4.29
+v 5.186 -5.1841 9.704
+v 7.264 1.9489 5.256
+v 3.772 6.5369 8.997
+v -5.187 -5.1841 5.704
+v -5.269 -3.7694 8.0942
+v 5.267 -5.2641 9.497
+v 7.264 1.9489 4.738
+v -5.267 -5.2641 5.497
+v 7.195 1.9309 4.497
+v 5.318 -5.3151 9.256
+v 3.76 6.5159 9.256
+v -5.318 -5.3151 5.256
+v 7.085 1.9009 4.29
+v 6.941 1.8629 4.131
+v 3.76 6.5159 8.738
+v -5.318 5.3209 8.738
+v 6.774 1.8179 4.031
+v -5.335 5.3379 8.997
+v 3.724 6.4539 8.497
+v -5.267 5.2699 8.497
+v 6.595 1.7699 3.997
+v 3.667 6.3549 8.29
+v -5.187 5.1889 8.29
+v 3.593 6.2259 8.131
+v 6.415 1.7219 4.031
+v 5.205 0.0029 11.139
+v -5.318 5.3209 9.256
+v 5.086 1.0146 11.1255
+v 5.156 0.5106 11.1222
+v -5.175 -1.3836 6.997
+v 6.3096 1.6935 4.0941
+v 6.3096 1.6935 4.0941
+v 6.642 0.0029 9.963
+v 6.827 0.0029 9.997
+v 7.013 0.0029 9.963
+v 7.186 0.0029 9.863
+v -4.828 -4.8251 7.997
+v 7.335 0.0029 9.704
+v 6.5327 0.0029 5.8998
+v -5.082 5.0839 8.131
+v 6.642 0.0029 5.963
+v 7.449 0.0029 9.497
+v -4.697 -4.6941 8.031
+v -4.959 4.9619 8.031
+v 6.5327 0.0029 9.8998
+v 6.827 0.0029 5.997
+v -4.6191 -4.6163 8.0943
+v -4.6393 -2.6757 6.997
+v 7.013 0.0029 5.963
+v 6.4491 -0.6307 9.8999
+v -3.7879 -3.785 6.997
+v 7.186 0.0029 5.863
+v 3.506 6.0759 8.031
+v -2.6786 -4.6364 6.997
+v 7.545 0.0029 4.997
+v -1.3864 -5.1721 6.997
+v 7.335 0.0029 5.704
+v 7.288 -1.9501 8.997
+v 7.449 0.0029 5.497
+v 5.318 5.3209 4.738
+v 7.52 0.0029 5.256
+v 5.335 5.3379 4.997
+v 7.264 -1.9441 9.256
+v 5.267 5.2699 4.497
+v 5.186 5.1889 4.29
+v 7.264 -1.9441 8.738
+v 7.52 0.0029 4.738
+v 5.186 5.1889 5.704
+v 7.195 -1.9251 8.497
+v 5.267 5.2699 5.497
+v 7.449 0.0029 4.497
+v 5.318 5.3209 5.256
+v 7.085 -1.8951 8.29
+v 7.085 -1.8952 8.29
+v 7.085 -1.8952 8.29
+v 6.941 -1.8571 8.131
+v 7.085 -1.8952 8.29
+v 7.335 0.0029 4.29
+v 2.2813 6.068 9.8999
+v 6.774 -1.8121 8.031
+v 7.186 0.0029 4.131
+v 1.719 6.4179 9.963
+v 7.013 0.0029 4.031
+v 1.767 6.5979 9.997
+v 6.827 0.0029 3.997
+v 1.815 6.7769 9.963
+v 6.642 0.0029 4.031
+v 5.9022 -2.6717 8.0941
+v 6.4491 0.6364 4.0941
+v 6.5327 0.0029 4.0942
+v 5.0077 4.1155 5.8998
+v -5.752 3.3239 9.963
+v -5.913 3.4169 9.997
+v -6.074 3.5089 9.963
+v -0.6335 6.452 4.0941
+v -1.0569 6.3961 5.8999
+v -1.6906 6.3125 5.8999
+v -1.6906 6.3125 5.8999
+v -1.719 6.4179 5.963
+v -5.2683 3.7759 9.8999
+v -1.767 6.5979 5.997
+v -1.815 6.7769 5.963
+v -1.86 6.9439 5.863
+v -5.6572 3.2692 9.8999
+v -5.6573 3.269 9.8999
+v -1.953 7.2909 4.997
+v -1.899 7.0879 5.704
+v -1.928 7.1979 5.497
+v -1.947 7.2669 5.256
+v -1.947 7.2669 4.738
+v -1.928 7.1979 4.497
+v -1.899 7.0879 4.29
+v -1.86 6.9439 4.131
+v -1.815 6.7769 4.031
+v -1.767 6.5979 3.997
+v -1.719 6.4179 4.031
+v -1.6906 6.3125 4.0941
+v -1.6906 6.3125 4.0941
+v -3.593 6.2259 5.863
+v -3.773 6.5369 4.997
+v -3.668 6.3549 5.704
+v -3.725 6.4539 5.497
+v -3.76 6.5159 5.256
+v -3.76 6.5159 4.738
+v -3.725 6.4539 4.497
+v -3.668 6.3549 4.29
+v -3.593 6.2259 4.131
+v -2.6765 5.9044 5.8998
+v -2.2813 6.068 4.0941
+v -3.321 5.7549 5.963
+v -3.2664 5.6602 5.8998
+v -3.2666 5.66 5.8998
+v -3.414 5.9159 5.997
+v -3.507 6.0759 5.963
+v -3.507 6.0759 4.031
+v -3.414 5.9159 3.997
+v -3.321 5.7549 4.031
+v -3.2664 5.6602 4.0942
+v -3.2666 5.66 4.0942
+# 1390 vertices
+
+g group_0_undefined
+
+usemtl color_undefined
+s 0
+
+f 3 657 2
+f 4 657 3
+f 5 657 4
+f 6 657 5
+f 7 657 6
+f 10 657 9
+f 11 657 10
+f 27 28 26
+f 26 28 29
+f 26 29 30
+f 26 30 31
+f 31 32 33
+f 31 33 34
+f 31 34 36
+f 31 36 37
+f 31 37 26
+f 46 31 30
+f 30 818 46
+f 29 820 30
+f 50 16 51
+f 772 27 26
+f 50 1339 16
+f 1127 52 45
+f 46 62 31
+f 772 26 37
+f 59 173 23
+f 33 32 62
+f 59 23 60
+f 63 979 62
+f 979 978 62
+f 31 62 32
+f 21 43 40
+f 40 1159 21
+f 63 62 678
+f 46 678 62
+f 257 258 58
+f 1137 57 1138
+f 23 1193 60
+f 75 85 74
+f 76 116 75
+f 77 116 76
+f 74 85 73
+f 73 85 79
+f 259 1141 72
+f 81 116 77
+f 82 139 81
+f 83 139 82
+f 84 161 83
+f 86 611 78
+f 94 78 611
+f 611 598 94
+f 93 94 598
+f 78 79 86
+f 85 86 79
+f 91 93 598
+f 90 91 598
+f 95 88 569
+f 569 550 95
+f 78 98 79
+f 97 79 98
+f 99 73 79
+f 99 79 97
+f 99 100 73
+f 74 73 100
+f 76 122 77
+f 81 77 143
+f 141 143 77
+f 21 102 43
+f 592 627 576
+f 93 616 94
+f 619 613 94
+f 570 88 571
+f 85 109 110
+f 85 110 86
+f 97 753 99
+f 1279 171 105
+f 115 161 114
+f 114 161 84
+f 85 75 116
+f 117 74 100
+f 74 117 75
+f 119 75 117
+f 121 76 75
+f 121 75 119
+f 141 77 122
+f 121 122 76
+f 145 82 81
+f 145 83 82
+f 167 84 83
+f 172 120 105
+f 167 168 84
+f 109 85 128
+f 116 128 85
+f 112 125 124
+f 129 111 388
+f 100 755 117
+f 119 421 121
+f 137 182 136
+f 138 182 137
+f 136 182 115
+f 116 81 139
+f 126 129 691
+f 133 127 134
+f 81 143 144
+f 133 150 127
+f 81 144 145
+f 150 133 748
+f 172 105 171
+f 1275 1304 285
+f 1135 125 112
+f 139 149 128
+f 139 128 116
+f 134 748 133
+f 145 144 125
+f 147 1285 154
+f 141 349 143
+f 148 155 153
+f 153 123 148
+f 143 124 144
+f 160 209 138
+f 139 83 161
+f 162 228 160
+f 145 165 83
+f 165 167 83
+f 188 114 168
+f 114 84 168
+f 190 115 188
+f 156 1374 164
+f 193 195 136
+f 216 138 215
+f 171 169 147
+f 60 817 59
+f 235 236 162
+f 302 23 173
+f 171 147 172
+f 149 139 174
+f 161 174 139
+f 127 150 473
+f 166 283 170
+f 285 1304 298
+f 1160 167 1135
+f 170 291 176
+f 145 125 165
+f 148 135 399
+f 170 283 291
+f 168 167 1160
+f 1304 178 298
+f 58 57 180
+f 276 140 176
+f 291 276 176
+f 155 148 180
+f 161 115 182
+f 177 184 192
+f 210 248 186
+f 186 233 185
+f 163 233 162
+f 114 188 115
+f 180 148 399
+f 136 115 193
+f 190 193 115
+f 194 192 184
+f 215 137 195
+f 137 136 195
+f 137 215 138
+f 160 138 217
+f 160 234 162
+f 163 162 236
+f 236 237 163
+f 200 174 182
+f 161 182 174
+f 1289 179 175
+f 190 188 1161
+f 202 201 187
+f 187 201 222
+f 188 168 1161
+f 199 332 335
+f 204 196 1385
+f 1160 1161 168
+f 204 1385 1370
+f 204 1370 207
+f 182 138 209
+f 212 248 210
+f 213 248 212
+f 202 187 1242
+f 335 344 205
+f 216 217 138
+f 208 205 344
+f 1150 269 274
+f 185 249 186
+f 250 251 186
+f 221 218 1386
+f 183 357 189
+f 182 209 225
+f 182 225 200
+f 198 189 304
+f 1202 1203 380
+f 1340 215 1283
+f 357 304 189
+f 195 1283 215
+f 215 1340 216
+f 209 160 228
+f 232 289 230
+f 230 265 229
+f 229 265 213
+f 228 162 233
+f 163 185 233
+f 160 217 234
+f 235 162 234
+f 185 163 237
+f 250 186 249
+f 210 254 212
+f 21 331 102
+f 227 364 238
+f 272 273 213
+f 233 244 228
+f 244 225 228
+f 209 228 225
+f 242 231 243
+f 47 243 231
+f 242 256 231
+f 241 382 131
+f 103 1201 245
+f 235 861 236
+f 236 862 237
+f 373 221 223
+f 247 289 246
+f 246 289 232
+f 233 186 248
+f 237 249 185
+f 206 532 157
+f 173 328 302
+f 206 211 560
+f 210 186 251
+f 268 212 254
+f 251 254 210
+f 23 305 49
+f 212 270 213
+f 257 58 255
+f 239 402 736
+f 309 54 49
+f 299 300 232
+f 72 58 258
+f 371 259 255
+f 257 255 259
+f 233 248 260
+f 233 260 244
+f 197 219 318
+f 258 257 259
+f 321 184 196
+f 227 226 426
+f 196 204 333
+f 258 259 72
+f 863 250 249
+f 204 207 336
+f 224 626 220
+f 863 249 237
+f 224 320 625
+f 261 400 53
+f 426 364 227
+f 320 674 625
+f 863 934 250
+f 248 213 265
+f 247 266 310
+f 267 310 266
+f 264 310 267
+f 212 268 270
+f 262 384 269
+f 272 213 270
+f 296 229 273
+f 229 213 273
+f 146 454 142
+f 297 230 296
+f 299 232 230
+f 221 465 218
+f 416 180 399
+f 1273 281 271
+f 278 260 265
+f 248 265 260
+f 277 166 108
+f 108 274 277
+f 332 199 54
+f 191 542 181
+f 231 256 395
+f 205 199 335
+f 1271 280 275
+f 279 281 282
+f 283 166 277
+f 277 406 283
+f 279 271 281
+f 254 981 268
+f 268 981 270
+f 272 270 1059
+f 279 282 286
+f 279 286 285
+f 282 281 286
+f 1086 273 272
+f 19 45 295
+f 277 401 406
+f 265 230 289
+f 288 339 264
+f 156 164 437
+f 293 312 365
+f 375 738 294
+f 284 295 290
+f 294 290 295
+f 229 296 230
+f 297 299 230
+f 315 246 300
+f 246 232 300
+f 142 452 130
+f 246 315 247
+f 295 375 294
+f 266 319 267
+f 291 398 276
+f 267 343 264
+f 345 264 343
+f 287 301 594
+f 345 346 264
+f 287 1291 301
+f 306 507 303
+f 265 289 307
+f 265 307 278
+f 500 178 187
+f 198 304 308
+f 1295 214 301
+f 299 297 1159
+f 1087 1159 297
+f 500 187 510
+f 301 1291 1295
+f 203 198 308
+f 959 748 311
+f 1086 296 273
+f 1086 1087 296
+f 302 305 23
+f 297 296 1087
+f 49 305 309
+f 510 187 222
+f 305 450 309
+f 466 308 304
+f 152 1319 159
+f 310 264 339
+f 289 247 310
+f 304 463 466
+f 340 387 313
+f 292 293 365
+f 387 365 313
+f 314 203 308
+f 184 321 311
+f 316 247 315
+f 311 194 184
+f 220 211 1316
+f 266 247 317
+f 316 317 247
+f 220 67 224
+f 343 267 319
+f 317 319 266
+f 1345 22 367
+f 1316 67 220
+f 314 308 418
+f 275 629 544
+f 275 544 253
+f 275 280 629
+f 1335 334 323
+f 173 820 328
+f 327 326 321
+f 311 321 326
+f 328 325 302
+f 325 324 302
+f 102 350 317
+f 319 317 351
+f 317 316 102
+f 289 310 329
+f 289 329 307
+f 315 43 316
+f 691 323 665
+f 196 333 321
+f 54 309 332
+f 674 320 330
+f 333 204 336
+f 336 522 333
+f 665 323 334
+f 53 393 42
+f 341 387 340
+f 361 410 342
+f 342 387 341
+f 365 360 292
+f 336 207 423
+f 24 322 331
+f 207 151 423
+f 331 21 24
+f 288 264 346
+f 368 292 366
+f 346 366 288
+f 288 366 292
+f 240 263 537
+f 368 369 292
+f 337 124 349
+f 369 370 293
+f 335 480 344
+f 392 313 390
+f 245 376 103
+f 103 376 104
+f 351 317 350
+f 349 348 337
+f 208 344 352
+f 104 377 107
+f 351 350 331
+f 337 348 421
+f 376 245 353
+f 352 219 208
+f 350 102 331
+f 276 355 140
+f 276 453 355
+f 449 448 345
+f 449 343 351
+f 140 355 183
+f 356 329 339
+f 310 339 329
+f 48 354 347
+f 378 373 223
+f 183 355 357
+f 318 219 352
+f 421 348 121
+f 319 351 343
+f 358 1345 367
+f 343 449 345
+f 318 352 745
+f 339 288 360
+f 455 463 357
+f 330 112 337
+f 304 357 463
+f 48 55 354
+f 363 436 362
+f 362 410 361
+f 288 292 360
+f 354 55 359
+f 293 292 369
+f 390 312 370
+f 312 293 370
+f 124 143 349
+f 364 372 238
+f 337 112 124
+f 340 396 341
+f 341 413 342
+f 414 342 413
+f 372 241 238
+f 55 252 359
+f 1343 353 245
+f 414 415 342
+f 252 240 541
+f 255 374 371
+f 377 104 376
+f 929 379 371
+f 371 379 259
+f 1202 380 111
+f 365 381 360
+f 381 356 360
+f 339 360 356
+f 372 382 241
+f 376 353 377
+f 147 169 1284
+f 366 346 448
+f 380 323 388
+f 259 379 262
+f 388 111 380
+f 366 448 368
+f 1285 147 1284
+f 448 992 368
+f 262 379 384
+f 372 492 382
+f 1286 158 154
+f 386 436 385
+f 385 436 363
+f 312 313 365
+f 383 367 22
+f 154 1285 1286
+f 131 389 135
+f 269 391 274
+f 312 390 313
+f 378 223 231
+f 269 384 391
+f 340 313 394
+f 392 394 313
+f 389 131 382
+f 323 691 388
+f 384 941 391
+f 413 341 396
+f 394 396 340
+f 498 389 382
+f 361 342 415
+f 382 492 498
+f 22 42 383
+f 338 35 347
+f 415 362 361
+f 398 453 276
+f 443 363 362
+f 378 231 395
+f 35 48 347
+f 42 393 383
+f 399 389 586
+f 443 446 363
+f 263 548 537
+f 274 391 401
+f 401 277 274
+f 263 271 548
+f 404 395 403
+f 405 381 387
+f 365 387 381
+f 403 395 256
+f 239 1293 402
+f 393 53 400
+f 399 586 603
+f 370 993 390
+f 1111 959 407
+f 397 736 402
+f 390 994 392
+f 408 291 283
+f 389 399 135
+f 261 564 400
+f 171 1279 169
+f 283 406 408
+f 321 407 327
+f 672 338 680
+f 387 342 410
+f 386 411 461
+f 412 461 411
+f 409 461 412
+f 338 672 334
+f 680 338 347
+f 417 603 58
+f 398 291 408
+f 180 416 58
+f 385 363 446
+f 608 395 615
+f 399 603 417
+f 417 416 399
+f 469 386 467
+f 378 395 608
+f 318 420 197
+f 347 354 702
+f 404 419 395
+f 615 395 419
+f 197 420 226
+f 122 121 348
+f 748 959 150
+f 744 421 755
+f 349 122 348
+f 424 322 314
+f 425 405 410
+f 387 410 405
+f 314 418 424
+f 418 975 982
+f 426 226 420
+f 396 1084 413
+f 541 713 359
+f 10 9 442
+f 414 413 1180
+f 427 132 130
+f 364 426 614
+f 346 345 448
+f 201 738 222
+f 369 368 992
+f 433 499 432
+f 432 486 431
+f 429 423 151
+f 431 487 409
+f 410 362 436
+f 732 397 434
+f 151 156 429
+f 429 156 437
+f 106 683 428
+f 377 683 107
+f 440 567 561
+f 415 441 362
+f 441 443 362
+f 331 322 435
+f 440 561 437
+f 421 744 337
+f 467 385 446
+f 438 445 302
+f 385 467 386
+f 1 657 447
+f 122 349 141
+f 124 125 144
+f 411 386 470
+f 437 164 440
+f 322 424 435
+f 411 472 412
+f 445 305 302
+f 427 567 440
+f 494 409 412
+f 445 450 305
+f 290 738 284
+f 164 132 440
+f 448 449 435
+f 435 449 331
+f 440 132 427
+f 410 436 451
+f 410 451 425
+f 351 331 449
+f 25 19 284
+f 503 69 375
+f 69 773 68
+f 503 505 69
+f 324 770 457
+f 733 443 743
+f 901 444 690
+f 295 496 375
+f 130 452 427
+f 446 443 733
+f 634 452 454
+f 355 453 455
+f 324 438 302
+f 453 1000 455
+f 441 743 443
+f 1324 456 439
+f 455 357 355
+f 459 499 458
+f 458 499 433
+f 324 457 438
+f 452 142 454
+f 461 409 487
+f 436 386 461
+f 453 999 1000
+f 468 332 309
+f 431 490 432
+f 462 454 146
+f 502 504 432
+f 433 506 458
+f 491 686 478
+f 113 460 214
+f 521 459 520
+f 751 318 745
+f 585 462 591
+f 462 146 218
+f 469 470 386
+f 127 460 113
+f 466 418 308
+f 450 468 309
+f 494 412 472
+f 470 472 411
+f 468 474 475
+f 761 681 757
+f 756 757 684
+f 462 218 465
+f 460 127 473
+f 684 685 756
+f 450 474 468
+f 685 491 488
+f 332 468 476
+f 477 468 475
+f 477 476 468
+f 469 667 470
+f 467 446 734
+f 475 474 477
+f 465 221 373
+f 436 461 479
+f 436 479 451
+f 332 476 335
+f 733 734 446
+f 469 467 734
+f 480 335 476
+f 522 336 535
+f 388 691 129
+f 481 959 482
+f 336 423 535
+f 473 150 481
+f 482 473 481
+f 434 397 1317
+f 484 518 483
+f 485 518 484
+f 483 518 459
+f 487 431 486
+f 482 493 473
+f 488 756 685
+f 434 1320 439
+f 489 490 431
+f 502 432 490
+f 462 639 454
+f 458 520 459
+f 456 1327 464
+f 639 462 585
+f 521 527 459
+f 493 482 959
+f 756 488 943
+f 465 591 462
+f 478 696 491
+f 505 773 69
+f 409 494 495
+f 503 375 496
+f 431 409 489
+f 495 489 409
+f 496 295 303
+f 202 738 201
+f 486 497 487
+f 497 479 487
+f 461 487 479
+f 588 287 594
+f 472 632 494
+f 495 494 512
+f 512 726 495
+f 486 432 499
+f 499 459 518
+f 501 550 485
+f 95 550 501
+f 433 432 504
+f 505 503 496
+f 520 458 506
+f 504 506 433
+f 1324 1327 456
+f 178 500 298
+f 505 496 773
+f 483 530 484
+f 593 594 301
+f 303 507 496
+f 484 553 485
+f 555 485 553
+f 489 495 726
+f 69 68 523
+f 507 783 496
+f 555 556 485
+f 407 515 1111
+f 526 222 69
+f 593 301 214
+f 403 530 404
+f 507 306 509
+f 486 499 511
+f 486 511 497
+f 514 1111 515
+f 502 621 504
+f 321 517 407
+f 798 509 513
+f 623 506 504
+f 490 726 502
+f 623 504 621
+f 513 509 152
+f 515 407 517
+f 321 333 517
+f 333 522 517
+f 509 306 152
+f 95 571 88
+f 525 519 529
+f 515 517 514
+f 651 529 519
+f 643 514 517
+f 510 766 500
+f 517 651 643
+f 526 69 524
+f 523 524 69
+f 483 459 527
+f 513 152 159
+f 553 484 530
+f 527 530 483
+f 528 525 529
+f 524 510 526
+f 222 526 510
+f 651 517 522
+f 510 524 523
+f 533 516 534
+f 527 404 530
+f 624 419 521
+f 516 533 531
+f 683 353 646
+f 539 511 518
+f 499 518 511
+f 353 358 646
+f 159 536 513
+f 423 540 535
+f 536 181 542
+f 541 359 252
+f 521 520 624
+f 623 520 506
+f 529 538 692
+f 623 624 520
+f 423 429 540
+f 536 159 181
+f 534 816 533
+f 533 816 531
+f 541 240 537
+f 419 404 527
+f 538 671 543
+f 518 485 550
+f 543 675 549
+f 671 675 543
+f 683 377 353
+f 570 572 88
+f 546 542 191
+f 452 634 545
+f 89 599 90
+f 427 452 545
+f 501 485 556
+f 548 271 551
+f 540 429 552
+f 191 157 546
+f 578 95 558
+f 556 558 501
+f 501 558 95
+f 393 711 383
+f 546 157 532
+f 571 95 578
+f 714 393 400
+f 256 242 555
+f 256 553 403
+f 552 429 437
+f 562 539 550
+f 518 550 539
+f 549 688 557
+f 551 279 559
+f 714 400 721
+f 243 556 242
+f 530 403 553
+f 279 551 271
+f 206 560 532
+f 564 721 400
+f 437 561 552
+f 279 285 559
+f 553 256 555
+f 559 285 565
+f 596 641 592
+f 627 628 576
+f 555 242 556
+f 243 558 556
+f 285 298 565
+f 261 253 564
+f 568 563 697
+f 89 88 572
+f 599 89 572
+f 564 253 544
+f 560 211 566
+f 602 90 599
+f 427 545 567
+f 565 298 573
+f 602 604 90
+f 634 454 639
+f 211 220 566
+f 574 580 581
+f 831 582 577
+f 550 569 583
+f 550 583 562
+f 1389 572 1390
+f 579 585 587
+f 582 588 589
+f 582 589 577
+f 1390 570 47
+f 243 47 578
+f 579 639 585
+f 582 638 588
+f 571 47 570
+f 591 579 587
+f 579 591 590
+f 587 585 591
+f 577 589 594
+f 577 594 593
+f 500 766 573
+f 570 1390 572
+f 589 588 594
+f 573 298 500
+f 465 590 591
+f 578 47 571
+f 243 578 558
+f 88 89 569
+f 576 718 592
+f 389 498 586
+f 601 759 603
+f 590 465 373
+f 510 777 766
+f 91 90 604
+f 616 93 606
+f 606 93 91
+f 604 606 91
+f 595 709 605
+f 373 597 590
+f 601 603 586
+f 607 583 598
+f 569 598 583
+f 510 523 777
+f 597 378 608
+f 604 602 1380
+f 602 599 1389
+f 318 751 420
+f 572 1389 599
+f 593 214 600
+f 597 373 378
+f 1389 1380 602
+f 610 426 420
+f 600 214 460
+f 89 90 569
+f 598 569 90
+f 426 610 614
+f 613 98 78
+f 619 94 616
+f 460 767 600
+f 78 94 613
+f 723 617 609
+f 86 110 611
+f 110 607 611
+f 598 611 607
+f 364 620 372
+f 606 1369 616
+f 364 614 620
+f 716 608 615
+f 460 473 767
+f 179 618 612
+f 723 512 617
+f 537 548 813
+f 620 492 372
+f 612 175 179
+f 620 828 492
+f 619 616 1346
+f 780 473 493
+f 493 1111 796
+f 574 566 580
+f 220 580 566
+f 527 521 419
+f 872 574 625
+f 512 494 632
+f 626 574 581
+f 128 149 109
+f 149 174 109
+f 174 200 109
+f 110 109 200
+f 110 200 225
+f 110 225 244
+f 110 244 260
+f 110 260 278
+f 110 278 307
+f 110 307 329
+f 110 329 356
+f 110 356 381
+f 110 381 405
+f 425 607 405
+f 451 607 425
+f 479 607 451
+f 497 607 479
+f 511 607 497
+f 539 607 511
+f 562 607 539
+f 583 607 562
+f 110 405 607
+f 624 615 419
+f 624 623 615
+f 623 621 615
+f 472 470 632
+f 512 632 617
+f 632 631 617
+f 631 667 617
+f 636 633 637
+f 635 633 636
+f 630 649 633
+f 928 622 736
+f 596 838 641
+f 627 592 641
+f 280 638 629
+f 574 626 625
+f 673 649 70
+f 731 639 579
+f 581 580 626
+f 582 831 638
+f 280 287 638
+f 287 588 638
+f 690 428 683
+f 147 154 694
+f 559 832 551
+f 656 1208 643
+f 158 701 154
+f 656 519 1208
+f 158 175 707
+f 522 669 651
+f 627 649 628
+f 648 633 649
+f 647 633 648
+f 637 633 647
+f 656 643 651
+f 358 663 646
+f 519 656 651
+f 667 631 470
+f 663 358 367
+f 659 658 39
+f 658 689 39
+f 664 39 666
+f 669 522 535
+f 677 931 676
+f 651 669 529
+f 529 669 538
+f 663 367 670
+f 669 671 538
+f 669 535 671
+f 224 625 626
+f 334 672 665
+f 850 1043 668
+f 580 220 626
+f 668 673 850
+f 175 612 707
+f 535 540 671
+f 367 383 670
+f 665 672 949
+f 671 540 675
+f 668 649 673
+f 676 644 677
+f 989 678 760
+f 679 760 678
+f 106 107 683
+f 657 678 447
+f 239 618 179
+f 540 552 675
+f 681 761 679
+f 622 618 239
+f 682 545 634
+f 491 685 686
+f 239 736 622
+f 549 675 688
+f 645 698 1299
+f 822 537 813
+f 675 552 688
+f 696 478 695
+f 478 126 695
+f 691 695 126
+f 645 692 698
+f 685 684 39
+f 689 687 39
+f 687 686 39
+f 686 685 39
+f 684 681 39
+f 679 39 681
+f 678 39 679
+f 693 563 557
+f 690 683 646
+f 695 691 665
+f 557 688 693
+f 552 561 688
+f 12 657 11
+f 8 657 7
+f 688 561 693
+f 706 893 670
+f 699 1299 698
+f 699 1134 1299
+f 8 7 428
+f 695 665 696
+f 697 563 693
+f 711 670 383
+f 561 567 693
+f 698 692 699
+f 393 715 711
+f 693 567 697
+f 568 697 682
+f 680 347 702
+f 568 682 554
+f 697 545 682
+f 444 9 428
+f 567 545 697
+f 706 711 712
+f 354 705 702
+f 670 711 706
+f 1236 704 710
+f 715 706 712
+f 605 716 609
+f 706 715 714
+f 8 428 9
+f 718 576 717
+f 716 605 709
+f 712 711 715
+f 608 709 597
+f 717 694 718
+f 705 359 713
+f 709 608 716
+f 715 393 714
+f 354 359 705
+f 490 489 726
+f 621 502 726
+f 541 822 713
+f 920 612 618
+f 723 716 615
+f 541 537 822
+f 730 765 438
+f 721 564 720
+f 615 621 723
+f 438 774 445
+f 694 154 701
+f 438 765 774
+f 158 707 701
+f 544 720 564
+f 716 723 609
+f 779 450 445
+f 445 774 779
+f 575 118 727
+f 172 727 120
+f 779 795 450
+f 667 469 734
+f 680 954 672
+f 723 621 726
+f 576 575 717
+f 726 512 723
+f 727 694 717
+f 147 694 727
+f 554 729 640
+f 727 717 575
+f 172 147 727
+f 729 554 682
+f 772 730 457
+f 1003 720 719
+f 682 634 729
+f 457 730 438
+f 729 731 640
+f 831 1044 725
+f 731 642 640
+f 450 795 474
+f 639 731 729
+f 634 639 729
+f 735 642 731
+f 795 801 474
+f 731 579 735
+f 728 667 734
+f 743 728 733
+f 818 815 46
+f 735 584 642
+f 734 733 728
+f 28 27 768
+f 69 222 738
+f 737 595 584
+f 202 1242 738
+f 928 736 732
+f 690 646 901
+f 27 772 770
+f 584 735 737
+f 736 397 732
+f 579 590 735
+f 1 2 657
+f 735 590 737
+f 709 595 737
+f 737 597 709
+f 1167 722 724
+f 984 37 36
+f 737 590 597
+f 739 674 330
+f 740 732 434
+f 976 977 34
+f 8 9 657
+f 976 34 33
+f 64 967 63
+f 740 439 741
+f 100 99 754
+f 434 439 740
+f 762 952 66
+f 38 762 66
+f 969 66 952
+f 724 728 1170
+f 997 995 1
+f 447 989 1
+f 755 100 754
+f 529 645 528
+f 344 747 352
+f 529 692 645
+f 344 480 747
+f 1111 493 959
+f 538 703 692
+f 476 805 480
+f 481 150 959
+f 1201 4 1204
+f 538 543 703
+f 747 745 352
+f 2 1198 3
+f 997 1 989
+f 640 793 554
+f 447 678 989
+f 760 679 761
+f 739 330 337
+f 739 337 744
+f 745 864 751
+f 610 420 751
+f 744 881 739
+f 66 678 38
+f 751 865 610
+f 428 690 444
+f 464 471 749
+f 584 595 835
+f 902 750 442
+f 471 752 749
+f 724 722 609
+f 865 751 864
+f 614 825 620
+f 57 123 153
+f 722 605 609
+f 724 609 617
+f 155 180 57
+f 417 58 416
+f 759 58 603
+f 617 728 724
+f 631 632 470
+f 235 234 1341
+f 728 617 667
+f 902 903 750
+f 38 657 762
+f 684 757 681
+f 557 563 812
+f 759 601 854
+f 968 1168 761
+f 843 654 763
+f 601 586 854
+f 843 39 846
+f 763 39 843
+f 554 823 568
+f 757 968 761
+f 543 708 703
+f 846 39 848
+f 41 848 39
+f 44 817 41
+f 764 708 549
+f 543 549 708
+f 741 439 456
+f 818 30 820
+f 754 744 755
+f 767 808 600
+f 29 28 769
+f 820 29 769
+f 768 769 28
+f 741 456 746
+f 770 768 27
+f 744 754 753
+f 746 456 464
+f 749 746 464
+f 781 68 773
+f 653 666 654
+f 496 783 773
+f 923 771 775
+f 776 775 771
+f 34 977 36
+f 783 507 788
+f 742 776 771
+f 767 473 780
+f 523 786 777
+f 778 752 471
+f 773 783 781
+f 979 63 967
+f 969 965 65
+f 65 66 969
+f 788 507 509
+f 784 864 745
+f 780 961 767
+f 782 957 785
+f 799 92 797
+f 549 557 764
+f 790 765 791
+f 787 797 1019
+f 785 957 887
+f 791 765 730
+f 509 798 788
+f 778 516 787
+f 769 768 325
+f 1350 783 788
+f 493 796 780
+f 324 325 768
+f 801 476 477
+f 796 794 780
+f 794 789 780
+f 799 797 787
+f 772 457 770
+f 766 858 573
+f 477 474 801
+f 798 513 802
+f 768 770 324
+f 787 531 799
+f 795 991 801
+f 719 544 629
+f 719 720 544
+f 476 801 805
+f 792 892 803
+f 118 120 727
+f 778 471 508
+f 804 532 560
+f 92 799 531
+f 847 840 593
+f 802 536 807
+f 508 516 778
+f 787 516 531
+f 480 805 811
+f 786 810 777
+f 593 600 847
+f 805 1045 811
+f 640 826 793
+f 480 811 747
+f 809 806 896
+f 764 557 812
+f 745 747 784
+f 747 811 784
+f 92 531 816
+f 815 817 44
+f 848 41 817
+f 534 1339 816
+f 44 46 815
+f 794 1111 789
+f 630 628 649
+f 652 666 653
+f 814 812 563
+f 173 59 815
+f 800 904 819
+f 814 568 823
+f 769 328 820
+f 325 328 769
+f 563 568 814
+f 1050 1199 664
+f 666 1051 664
+f 554 793 823
+f 815 59 817
+f 70 838 673
+f 818 173 815
+f 818 820 173
+f 70 641 838
+f 725 719 629
+f 828 620 825
+f 827 813 548
+f 831 725 638
+f 830 833 834
+f 829 826 642
+f 827 551 832
+f 640 642 826
+f 827 548 551
+f 830 824 833
+f 492 828 837
+f 839 830 834
+f 830 839 836
+f 834 833 839
+f 629 638 725
+f 584 835 829
+f 882 1277 647
+f 492 837 498
+f 642 584 829
+f 648 883 647
+f 875 876 649
+f 1251 835 841
+f 649 650 875
+f 877 875 650
+f 841 835 595
+f 877 650 878
+f 652 878 650
+f 866 867 653
+f 878 652 867
+f 577 840 831
+f 868 866 654
+f 841 595 605
+f 759 854 1307
+f 841 605 722
+f 857 859 1307
+f 577 593 840
+f 832 559 844
+f 600 808 847
+f 850 673 853
+f 565 851 844
+f 673 838 853
+f 863 237 862
+f 846 848 1193
+f 374 852 371
+f 498 837 849
+f 926 371 852
+f 498 849 586
+f 857 1307 854
+f 371 926 929
+f 886 901 646
+f 60 1193 848
+f 817 60 848
+f 802 513 536
+f 559 565 844
+f 886 646 663
+f 858 1070 851
+f 838 916 853
+f 536 542 807
+f 851 573 858
+f 807 542 856
+f 855 1336 845
+f 853 1171 850
+f 854 586 849
+f 859 857 849
+f 857 854 849
+f 861 862 236
+f 565 573 851
+f 856 546 860
+f 862 855 863
+f 542 546 856
+f 855 862 861
+f 653 867 652
+f 653 654 866
+f 546 532 860
+f 442 444 901
+f 654 843 868
+f 860 532 804
+f 650 666 652
+f 934 980 251
+f 610 821 614
+f 821 610 865
+f 821 825 614
+f 804 560 869
+f 945 951 771
+f 1302 1305 1142
+f 560 566 869
+f 792 785 887
+f 869 566 870
+f 872 1362 870
+f 870 574 872
+f 766 777 1005
+f 637 647 1277
+f 574 870 566
+f 800 890 904
+f 777 1013 1005
+f 721 720 919
+f 882 647 883
+f 883 648 876
+f 649 876 648
+f 824 819 909
+f 1003 719 996
+f 909 914 824
+f 674 880 874
+f 839 917 836
+f 625 874 872
+f 833 914 839
+f 777 810 1013
+f 842 836 963
+f 877 1100 875
+f 877 878 1100
+f 867 1114 878
+f 625 674 874
+f 946 1305 884
+f 665 949 943
+f 884 1307 859
+f 863 855 934
+f 849 936 884
+f 827 1040 813
+f 657 38 678
+f 406 962 408
+f 806 894 896
+f 12 13 657
+f 13 14 657
+f 14 15 657
+f 800 809 890
+f 15 762 657
+f 899 898 885
+f 879 899 885
+f 444 442 9
+f 463 455 971
+f 1000 971 455
+f 889 663 670
+f 463 973 466
+f 886 663 889
+f 418 466 975
+f 887 892 792
+f 758 12 11
+f 488 491 943
+f 881 880 739
+f 894 806 803
+f 889 670 893
+f 10 750 11
+f 894 803 892
+f 965 898 967
+f 873 928 1080
+f 674 739 880
+f 98 906 97
+f 893 706 907
+f 899 967 898
+f 1346 906 619
+f 744 897 881
+f 911 702 705
+f 898 895 885
+f 753 754 99
+f 902 442 901
+f 744 753 897
+f 930 931 13
+f 809 896 890
+f 901 886 902
+f 932 930 12
+f 753 97 906
+f 960 954 680
+f 701 905 900
+f 903 902 886
+f 613 619 906
+f 897 753 906
+f 442 750 10
+f 680 702 960
+f 707 908 905
+f 904 890 1090
+f 819 904 909
+f 117 755 119
+f 421 119 755
+f 827 1052 1040
+f 466 973 975
+f 956 14 931
+f 914 833 824
+f 706 714 907
+f 424 418 982
+f 838 913 916
+f 758 11 750
+f 911 705 915
+f 907 714 912
+f 66 65 678
+f 917 839 914
+f 986 424 982
+f 916 913 900
+f 435 424 986
+f 912 721 919
+f 836 917 963
+f 705 713 915
+f 915 713 918
+f 912 714 721
+f 922 732 740
+f 1046 918 822
+f 945 771 923
+f 64 678 65
+f 918 713 822
+f 622 873 871
+f 928 873 622
+f 925 926 924
+f 852 924 926
+f 952 762 15
+f 251 250 934
+f 956 15 14
+f 1080 922 1066
+f 1080 928 922
+f 922 928 732
+f 696 943 491
+f 757 756 966
+f 943 966 756
+f 13 12 930
+f 845 972 855
+f 1066 922 933
+f 12 758 932
+f 996 725 1044
+f 719 725 996
+f 922 740 933
+f 927 1294 936
+f 379 935 384
+f 855 972 934
+f 936 1294 1142
+f 379 929 935
+f 941 384 935
+f 932 1065 930
+f 696 665 943
+f 937 746 939
+f 944 942 945
+f 927 936 837
+f 1142 1305 947
+f 950 999 398
+f 758 750 903
+f 945 923 944
+f 907 912 700
+f 966 943 949
+f 936 1142 947
+f 946 884 936
+f 672 954 949
+f 746 749 939
+f 947 946 936
+f 790 879 765
+f 782 771 951
+f 885 765 879
+f 885 888 765
+f 953 401 391
+f 888 774 765
+f 955 952 15
+f 779 774 891
+f 955 15 956
+f 957 782 951
+f 1383 177 422
+f 1382 1383 422
+f 953 391 941
+f 13 931 14
+f 774 888 891
+f 422 177 748
+f 957 951 1047
+f 1112 953 941
+f 933 741 937
+f 401 953 958
+f 740 741 933
+f 957 1047 1049
+f 784 1054 864
+f 952 955 1091
+f 957 1049 887
+f 955 956 1091
+f 949 954 1155
+f 794 796 1111
+f 401 958 406
+f 741 746 937
+f 1056 865 864
+f 948 939 749
+f 930 676 931
+f 677 1091 956
+f 1099 1103 892
+f 962 406 958
+f 821 865 1061
+f 702 911 960
+f 931 677 956
+f 961 808 767
+f 842 964 845
+f 398 408 950
+f 408 962 950
+f 964 842 963
+f 965 967 64
+f 1110 950 962
+f 64 65 965
+f 837 828 927
+f 961 780 970
+f 1208 990 974
+f 968 966 949
+f 974 789 1208
+f 837 936 849
+f 895 969 1095
+f 913 694 900
+f 970 780 789
+f 969 895 965
+f 952 1095 969
+f 701 900 694
+f 701 707 905
+f 463 971 973
+f 895 898 965
+f 859 849 884
+f 972 845 964
+f 718 913 592
+f 879 790 976
+f 973 971 1132
+f 1132 971 1131
+f 694 913 718
+f 738 430 284
+f 1059 270 981
+f 984 36 977
+f 974 970 789
+f 908 612 920
+f 978 976 33
+f 908 707 612
+f 903 1065 758
+f 33 62 978
+f 981 254 980
+f 254 251 980
+f 618 871 920
+f 980 972 981
+f 981 972 1059
+f 1048 1044 831
+f 972 980 934
+f 791 977 790
+f 976 978 879
+f 978 979 879
+f 976 790 977
+f 805 801 1041
+f 618 622 871
+f 899 879 979
+f 899 979 967
+f 805 1041 1045
+f 840 847 1009
+f 1204 3 1198
+f 808 1017 847
+f 1068 963 917
+f 772 37 983
+f 37 984 983
+f 992 993 369
+f 1024 808 961
+f 994 390 993
+f 987 1054 784
+f 772 983 730
+f 779 891 988
+f 779 988 795
+f 988 991 795
+f 1086 272 1059
+f 370 369 993
+f 974 990 970
+f 991 988 1236
+f 942 547 938
+f 983 984 730
+f 791 730 984
+f 61 938 547
+f 791 984 977
+f 913 596 592
+f 435 992 448
+f 838 596 913
+f 995 1198 2
+f 2 1 995
+f 994 993 986
+f 993 992 986
+f 435 986 992
+f 890 1092 1090
+f 904 1090 1094
+f 909 904 1094
+f 654 666 763
+f 649 666 650
+f 627 641 649
+f 41 678 44
+f 44 678 46
+f 39 678 41
+f 999 453 398
+f 997 1351 995
+f 1103 894 892
+f 1000 999 1128
+f 942 938 945
+f 1016 1015 1002
+f 919 720 1003
+f 1128 1131 1000
+f 1000 1131 971
+f 948 752 985
+f 766 1005 858
+f 948 749 752
+f 985 752 1010
+f 1007 1238 940
+f 1025 810 80
+f 778 1010 752
+f 924 1307 925
+f 1097 914 909
+f 1011 940 1258
+f 876 875 1015
+f 1015 1016 876
+f 1301 1016 1002
+f 787 1010 778
+f 787 1019 1010
+f 1009 847 1017
+f 92 87 797
+f 1068 1074 963
+f 994 1026 392
+f 1026 1082 394
+f 396 394 1082
+f 964 963 1074
+f 786 68 810
+f 1013 810 1025
+f 892 887 1099
+f 394 392 1026
+f 1060 1022 1020
+f 797 87 1028
+f 1013 1025 1023
+f 1067 1029 1022
+f 1019 797 1028
+f 1017 808 1024
+f 1022 1060 1067
+f 1027 1004 1019
+f 1028 1027 1019
+f 896 894 1104
+f 775 816 923
+f 923 547 944
+f 961 1115 1024
+f 547 923 816
+f 942 944 547
+f 1092 890 896
+f 96 1004 87
+f 1027 87 1004
+f 1027 1028 87
+f 1030 1031 1034
+f 1015 1100 1002
+f 858 1105 1070
+f 1012 1032 1031
+f 1002 1106 1006
+f 1034 1031 1032
+f 1002 1102 1106
+f 1033 1038 61
+f 1039 938 61
+f 1202 111 659
+f 1122 970 990
+f 1036 945 938
+f 1023 1083 1013
+f 1039 61 1038
+f 666 668 1043
+f 1037 929 1035
+f 1151 1037 1035
+f 1038 1033 1036
+f 1038 1036 1039
+f 1039 1036 938
+f 1050 664 1051
+f 801 991 1041
+f 1236 1245 991
+f 1051 666 1043
+f 1172 996 1044
+f 1102 1002 1100
+f 39 763 666
+f 1036 1042 945
+f 1008 1107 1057
+f 991 1245 1041
+f 1171 853 916
+f 945 1042 951
+f 1047 951 1042
+f 1174 1043 850
+f 1203 660 1200
+f 1046 813 1040
+f 662 664 1199
+f 822 813 1046
+f 987 811 1045
+f 831 840 1048
+f 1048 840 1009
+f 811 987 784
+f 1107 1008 1006
+f 1050 1051 1330
+f 1180 413 1084
+f 1174 1176 1043
+f 64 63 678
+f 1052 827 832
+f 655 644 889
+f 1052 832 1053
+f 1020 1008 1057
+f 1056 864 1054
+f 1053 844 1058
+f 932 758 1065
+f 1057 1060 1020
+f 676 930 1065
+f 1061 865 1056
+f 972 1078 1059
+f 1097 1064 914
+f 914 1064 917
+f 903 886 1065
+f 676 1065 644
+f 821 1069 825
+f 1053 832 844
+f 821 1061 1069
+f 933 1062 1066
+f 1064 1068 917
+f 1058 851 1070
+f 644 1065 886
+f 825 921 828
+f 1058 844 851
+f 1069 921 825
+f 1067 1073 1029
+f 644 886 889
+f 927 828 921
+f 1062 933 937
+f 893 655 889
+f 1091 1095 952
+f 655 893 661
+f 1105 858 1005
+f 907 661 893
+f 1076 655 1081
+f 1083 1023 80
+f 1082 1084 396
+f 964 1078 972
+f 1077 1180 1084
+f 1082 1026 1077
+f 1084 1082 1077
+f 964 1074 1078
+f 1219 1013 1083
+f 925 1012 926
+f 1031 926 1012
+f 1031 1035 926
+f 1035 929 926
+f 1078 1087 1086
+f 1078 1086 1059
+f 1037 935 929
+f 935 1108 941
+f 907 700 661
+f 1091 677 644
+f 1162 908 1166
+f 644 1076 1091
+f 905 908 1162
+f 1095 1091 1076
+f 895 1095 1076
+f 999 1126 1128
+f 710 704 919
+f 966 968 757
+f 909 1094 1097
+f 1266 1062 1072
+f 1003 1177 710
+f 1071 982 975
+f 887 1049 1099
+f 1177 996 1172
+f 986 982 1077
+f 1166 908 920
+f 1100 878 1114
+f 1113 1114 867
+f 1077 982 1071
+f 704 700 912
+f 1077 1026 986
+f 871 1147 1140
+f 1026 994 986
+f 1147 873 1149
+f 704 912 919
+f 871 873 1147
+f 873 1080 1149
+f 1103 1104 894
+f 919 1003 710
+f 958 1119 962
+f 896 1104 1092
+f 1003 996 1177
+f 1006 1106 1107
+f 1119 1110 962
+f 1062 937 1072
+f 326 959 311
+f 935 1037 1108
+f 1075 1072 939
+f 194 311 748
+f 1146 1036 1141
+f 937 939 1072
+f 998 910 1241
+f 1146 1042 1036
+f 192 194 748
+f 1075 939 948
+f 1047 1042 1150
+f 1109 1113 866
+f 760 761 1168
+f 867 866 1113
+f 1112 941 1108
+f 1100 1015 875
+f 1109 1102 1113
+f 1114 1113 1102
+f 1094 1090 189
+f 1116 958 953
+f 1102 1100 1114
+f 1269 1040 1052
+f 1116 953 1112
+f 198 1094 189
+f 1024 1115 1143
+f 1117 96 1118
+f 1097 203 1064
+f 1119 958 1116
+f 1118 1120 1117
+f 1157 900 905
+f 1119 1116 1263
+f 1115 970 1122
+f 1115 961 970
+f 24 1074 1068
+f 948 1124 1075
+f 950 1126 999
+f 1110 1126 950
+f 1127 1123 1225
+f 1103 1099 170
+f 911 1163 960
+f 1127 1225 1231
+f 1127 1231 910
+f 61 547 1205
+f 990 1130 1122
+f 183 1092 140
+f 1104 140 1092
+f 1129 1122 1130
+f 1092 183 1090
+f 189 1090 183
+f 170 176 1103
+f 1133 1129 1281
+f 176 1104 1103
+f 176 140 1104
+f 973 1063 975
+f 1159 40 299
+f 1132 1063 973
+f 1159 1087 1078
+f 1130 1281 1129
+f 1063 1071 975
+f 915 1144 1179
+f 61 57 1033
+f 1179 911 915
+f 58 72 57
+f 1148 1144 918
+f 1136 1302 1134
+f 1137 1033 57
+f 1134 699 692
+f 72 1138 57
+f 1138 72 1141
+f 918 1046 1148
+f 1136 1134 1139
+f 259 262 1141
+f 1205 51 16
+f 1138 1141 1137
+f 1137 1141 1033
+f 1030 1136 1139
+f 866 868 1109
+f 1141 1036 1033
+f 1139 1031 1030
+f 1109 868 1196
+f 644 655 1076
+f 1141 262 1146
+f 703 708 1145
+f 1146 262 269
+f 661 1088 1081
+f 703 1145 1139
+f 1232 1079 1055
+f 1079 1232 1240
+f 1240 1085 1079
+f 1042 1146 1150
+f 915 918 1144
+f 1146 269 1150
+f 1049 1047 108
+f 1150 108 1047
+f 1150 274 108
+f 1049 108 166
+f 1099 1049 166
+f 812 1255 1153
+f 166 170 1099
+f 1173 1175 989
+f 1107 1106 1190
+f 415 1182 441
+f 102 316 43
+f 300 43 315
+f 1074 21 1078
+f 21 1074 24
+f 1157 905 1162
+f 300 299 40
+f 1158 954 960
+f 40 43 300
+f 1207 190 1161
+f 1158 1155 954
+f 743 441 1182
+f 1078 21 1159
+f 1158 960 1163
+f 1156 1161 1160
+f 1160 1135 1156
+f 1097 1094 198
+f 167 165 1135
+f 125 1135 165
+f 1206 1093 1089
+f 198 203 1097
+f 112 1156 1135
+f 1064 203 1165
+f 25 430 1342
+f 1165 1068 1064
+f 314 1165 203
+f 1342 1117 1120
+f 1165 24 1068
+f 1121 1211 17
+f 96 87 1169
+f 1165 314 322
+f 1118 96 1169
+f 1115 1122 1214
+f 1118 1169 1120
+f 949 1155 1168
+f 1214 1122 1224
+f 989 760 1168
+f 1168 968 949
+f 24 1165 322
+f 1174 850 1171
+f 1168 1173 989
+f 1122 1129 1133
+f 1122 1133 1224
+f 1120 1221 1123
+f 1168 1155 1173
+f 1170 1063 1167
+f 1057 1107 1197
+f 1123 1221 1225
+f 1154 1152 1272
+f 1330 1051 1176
+f 1176 1174 1157
+f 1174 1171 1157
+f 1176 1157 1330
+f 1175 1173 1155
+f 661 1081 655
+f 920 1140 1166
+f 700 1088 661
+f 920 871 1140
+f 1060 1252 1257
+f 1088 700 1096
+f 1067 1060 1257
+f 1261 1073 1067
+f 1163 911 1179
+f 1257 1261 1067
+f 1180 1182 414
+f 1171 900 1157
+f 1235 1055 1073
+f 916 900 1171
+f 1193 843 846
+f 1172 1044 1181
+f 843 1196 868
+f 415 414 1182
+f 948 985 1124
+f 985 1125 1124
+f 743 1182 1178
+f 1182 1180 1178
+f 1139 1134 692
+f 1189 1233 1222
+f 703 1139 692
+f 1181 1048 1183
+f 1109 1184 1102
+f 1044 1048 1181
+f 1192 1185 1189
+f 1145 708 1151
+f 1109 1196 1184
+f 1184 49 1188
+f 985 1186 1125
+f 1151 708 764
+f 700 704 1096
+f 1144 1148 42
+f 1186 985 1010
+f 1151 764 1153
+f 1102 1184 1188
+f 704 1236 1096
+f 1187 1183 1009
+f 1186 1019 1189
+f 764 812 1153
+f 1102 1188 1106
+f 1186 1010 1019
+f 69 738 375
+f 290 294 738
+f 1190 1106 1188
+f 1048 1009 1183
+f 1019 1004 1189
+f 786 523 68
+f 1222 1186 1189
+f 1197 1107 1190
+f 1187 1009 1017
+f 422 1083 80
+f 1189 1004 1194
+f 1191 1187 1017
+f 1196 843 1195
+f 80 810 68
+f 1023 1025 80
+f 1189 1194 1192
+f 885 895 1076
+f 1192 96 1185
+f 1193 1195 843
+f 1191 1017 1024
+f 885 1081 888
+f 1194 1004 96
+f 1191 1024 1143
+f 1076 1081 885
+f 1251 841 1164
+f 106 428 7
+f 1214 1143 1115
+f 1088 888 1081
+f 1290 92 742
+f 92 816 742
+f 742 816 776
+f 6 107 7
+f 106 7 107
+f 891 888 1088
+f 1167 1164 722
+f 775 776 816
+f 107 6 104
+f 891 1096 988
+f 430 1185 96
+f 1244 1185 430
+f 1195 1184 1196
+f 1088 1096 891
+f 1192 1194 96
+f 1164 841 722
+f 1167 724 1170
+f 5 104 6
+f 1201 103 4
+f 660 1203 659
+f 3 1204 4
+f 1178 1170 728
+f 126 478 687
+f 687 478 686
+f 687 689 126
+f 129 126 689
+f 1172 1181 1220
+f 728 743 1178
+f 660 659 39
+f 658 111 689
+f 129 689 111
+f 658 659 111
+f 1202 659 1203
+f 1199 1200 662
+f 662 1200 660
+f 61 1227 1229
+f 1229 123 61
+f 662 660 39
+f 1069 1061 1098
+f 1050 1330 1199
+f 103 104 5
+f 1098 1101 1069
+f 5 4 103
+f 327 407 959
+f 1101 921 1069
+f 1227 61 1205
+f 1255 812 814
+f 326 327 959
+f 18 20 1200
+f 1356 245 1204
+f 16 1218 1205
+f 1200 20 1203
+f 921 1288 927
+f 1288 921 1101
+f 1234 1237 1299
+f 1355 1356 1198
+f 193 190 1207
+f 1130 990 1281
+f 1208 1281 990
+f 1208 789 1111
+f 1226 1083 422
+f 1035 1031 1145
+f 1139 1145 1031
+f 1209 1121 1093
+f 1145 1151 1035
+f 1156 1207 1161
+f 1083 1226 1219
+f 1093 1206 1209
+f 1037 1153 1108
+f 92 1210 87
+f 1151 1153 1037
+f 1072 1270 1266
+f 1096 1236 988
+f 1270 1072 1075
+f 1211 1121 1209
+f 1216 87 1215
+f 87 1216 1169
+f 1210 1213 1215
+f 1131 1128 1251
+f 1169 1213 1120
+f 1005 1212 1105
+f 1132 1131 1164
+f 1212 1013 1219
+f 1164 1167 1132
+f 1212 1005 1013
+f 1216 1213 1169
+f 1215 1213 1216
+f 1063 1132 1167
+f 1213 1297 1221
+f 1170 1071 1063
+f 1275 1124 1125
+f 1181 1217 1220
+f 1213 1221 1120
+f 1054 1217 1223
+f 17 1218 16
+f 1170 1178 1071
+f 1211 148 1218
+f 1178 1077 1071
+f 1223 1056 1054
+f 1222 1125 1186
+f 17 1211 1218
+f 1178 1180 1077
+f 1183 1223 1217
+f 57 61 123
+f 1181 1183 1217
+f 1061 1056 1230
+f 1228 1219 1226
+f 1218 123 1229
+f 1223 1230 1056
+f 1225 1303 1231
+f 1227 1205 1218
+f 1229 1227 1218
+f 1112 1259 1116
+f 1228 1247 1219
+f 1230 1098 1061
+f 1232 1055 1235
+f 1295 1105 1212
+f 1231 1303 1308
+f 1133 1299 1237
+f 1247 1228 422
+f 1237 1224 1133
+f 1234 1256 1224
+f 1237 1234 1224
+f 1241 910 1231
+f 1187 1191 1230
+f 1233 1244 1242
+f 1126 1239 1246
+f 1245 1236 710
+f 1308 1241 1231
+f 1226 422 1228
+f 1248 1089 1085
+f 998 1241 1249
+f 1085 1240 1248
+f 1098 1191 1143
+f 829 1246 1239
+f 1041 1250 1045
+f 1101 1143 1214
+f 1249 1001 998
+f 1248 1206 1089
+f 1242 1244 430
+f 422 748 1247
+f 1041 1245 1250
+f 1128 1246 1251
+f 710 1177 1245
+f 829 835 1246
+f 1233 1185 1244
+f 1246 835 1251
+f 1060 1057 1252
+f 1245 1177 1250
+f 1253 1001 1249
+f 1185 1233 1189
+f 1131 1251 1164
+f 1249 1312 1253
+f 1045 1250 1220
+f 1252 1057 1197
+f 1253 1007 1001
+f 1288 1214 1224
+f 1254 1062 1266
+f 1112 1108 1255
+f 1254 1066 1062
+f 1238 1007 1253
+f 1108 1153 1255
+f 1267 1052 1053
+f 1140 1337 1166
+f 940 1238 1258
+f 1271 1058 1274
+f 1259 1112 1255
+f 1140 1147 35
+f 1255 814 1259
+f 48 1147 1149
+f 1274 1058 1070
+f 1172 1220 1250
+f 1172 1250 1177
+f 987 1217 1054
+f 55 1262 252
+f 1220 1217 987
+f 55 1149 1262
+f 1070 1105 1291
+f 1116 1259 1263
+f 1011 1260 1014
+f 1262 1149 1080
+f 1011 1258 1260
+f 1235 1073 1261
+f 1261 197 1235
+f 1264 1018 1014
+f 1259 823 1263
+f 1254 252 1262
+f 1264 1014 1260
+f 1045 1220 987
+f 1080 1066 1262
+f 1265 1018 1264
+f 1101 1098 1143
+f 1184 1195 1193
+f 1066 1254 1262
+f 23 1184 1193
+f 58 759 255
+f 1265 1021 1018
+f 23 49 1184
+f 793 826 1243
+f 374 255 759
+f 1188 54 1190
+f 1239 1243 826
+f 51 547 50
+f 1188 49 54
+f 51 1205 547
+f 826 829 1239
+f 153 155 57
+f 54 199 1190
+f 1268 1152 1021
+f 253 1269 1267
+f 1021 1265 1268
+f 1259 814 823
+f 793 1243 1263
+f 1052 1267 1269
+f 793 1263 823
+f 1110 1239 1126
+f 1187 1223 1183
+f 1272 1152 1268
+f 1239 1110 1243
+f 238 1240 227
+f 1187 1230 1223
+f 635 105 633
+f 1240 238 1248
+f 1126 1246 1128
+f 1230 1191 1098
+f 1271 1267 1053
+f 637 1277 636
+f 1248 241 1206
+f 238 241 1248
+f 641 70 649
+f 1263 1243 1119
+f 1206 241 131
+f 575 576 628
+f 1119 1243 1110
+f 1209 1206 131
+f 628 630 575
+f 118 575 630
+f 1053 1058 1271
+f 633 120 630
+f 118 630 120
+f 195 193 1282
+f 1209 131 135
+f 1282 1283 195
+f 1101 1214 1288
+f 286 1275 285
+f 135 1211 1209
+f 1279 105 635
+f 120 633 105
+f 286 281 1273
+f 286 1273 1275
+f 1280 1279 636
+f 1274 1070 1291
+f 636 1277 1280
+f 1211 135 148
+f 666 649 668
+f 1218 148 123
+f 514 643 1111
+f 1197 1190 199
+f 1208 1111 643
+f 1278 53 1148
+f 1154 1276 1156
+f 882 1301 1277
+f 876 1016 883
+f 882 883 1301
+f 199 205 1197
+f 635 636 1279
+f 1148 1046 1278
+f 1234 1299 1256
+f 1252 1197 205
+f 261 1278 1269
+f 1277 1296 1280
+f 1299 1133 1281
+f 1257 1252 208
+f 1154 1272 1276
+f 1280 169 1279
+f 1207 1282 193
+f 662 39 664
+f 205 208 1252
+f 1256 1302 1298
+f 208 219 1257
+f 1288 1224 1294
+f 1278 1040 1269
+f 1261 1257 219
+f 1046 1040 1278
+f 1276 1340 1283
+f 1156 1276 1207
+f 1282 1207 1276
+f 997 989 1175
+f 1351 1355 995
+f 1283 1282 1276
+f 219 197 1261
+f 1266 263 240
+f 226 1235 197
+f 92 1290 1210
+f 1266 240 1254
+f 1235 226 1232
+f 1266 1270 263
+f 1210 1215 87
+f 1240 1232 227
+f 1270 271 263
+f 1290 1292 1210
+f 226 227 1232
+f 1213 1210 1292
+f 1270 1273 271
+f 1351 997 1175
+f 1292 1297 1213
+f 1293 239 179
+f 35 1337 1140
+f 179 1289 1293
+f 1294 1224 1256
+f 1288 1294 927
+f 1300 1221 1297
+f 48 35 1147
+f 1290 742 1292
+f 1284 169 1296
+f 771 1292 742
+f 169 1280 1296
+f 48 1149 55
+f 1291 1105 1295
+f 1298 1302 1142
+f 113 214 1295
+f 1300 1225 1221
+f 158 1287 175
+f 1294 1256 1298
+f 1287 158 1286
+f 1287 1289 175
+f 1298 1142 1294
+f 1016 1301 883
+f 1292 771 782
+f 1296 1301 1284
+f 1297 1292 782
+f 1284 1301 1002
+f 525 1281 519
+f 1208 519 1281
+f 1006 1285 1284
+f 1006 1008 1285
+f 1297 782 785
+f 1302 1256 1299
+f 252 1254 240
+f 1300 1297 785
+f 113 1295 1212
+f 1299 1281 645
+f 528 645 1281
+f 1273 1270 1075
+f 525 528 1281
+f 1296 1277 1301
+f 1273 1124 1275
+f 1300 785 792
+f 134 1247 748
+f 946 947 1305
+f 1300 792 1303
+f 1124 1273 1075
+f 113 1212 1219
+f 1286 1020 1287
+f 1303 1225 1300
+f 1299 1134 1302
+f 113 1219 127
+f 1307 884 1305
+f 1308 1303 803
+f 792 803 1303
+f 1030 1034 1305
+f 1032 1305 1034
+f 1289 1029 1293
+f 1302 1030 1305
+f 1136 1030 1302
+f 1314 402 1293
+f 1318 1306 819
+f 925 1307 1012
+f 1309 397 402
+f 1306 800 819
+f 1307 1305 1012
+f 1032 1012 1305
+f 1318 1238 1306
+f 1307 852 759
+f 374 759 852
+f 1317 1320 434
+f 1219 1247 127
+f 924 852 1307
+f 1238 1253 1306
+f 1253 1312 1306
+f 206 157 1311
+f 1222 178 1304
+f 1311 1315 206
+f 1247 134 127
+f 1284 1002 1006
+f 1285 1008 1286
+f 1304 1275 1125
+f 1020 1286 1008
+f 1313 157 191
+f 803 806 1308
+f 1125 1222 1304
+f 1308 806 1310
+f 1241 1310 1249
+f 1001 1007 1321
+f 1287 1022 1289
+f 1323 1007 940
+f 1287 1020 1022
+f 1289 1022 1029
+f 1310 1241 1308
+f 1312 1310 809
+f 1344 1158 1163
+f 1314 1293 1029
+f 1233 187 178
+f 1310 806 809
+f 1029 1073 1314
+f 402 1314 1309
+f 1233 178 1222
+f 1249 1310 1312
+f 1073 1055 1314
+f 809 800 1312
+f 1055 1309 1314
+f 1312 800 1306
+f 430 738 1242
+f 1317 397 1309
+f 1018 1021 1316
+f 253 1267 275
+f 1233 1242 187
+f 1267 1271 275
+f 998 1001 1319
+f 1318 1258 1238
+f 1324 439 1320
+f 819 824 1318
+f 1001 1321 1319
+f 1318 824 1322
+f 1322 1260 1258
+f 287 280 1274
+f 1324 1325 1326
+f 1274 280 1271
+f 1324 1320 1325
+f 1089 1325 1320
+f 1162 1333 1157
+f 1318 1322 1258
+f 1324 1326 1328
+f 1324 1328 1327
+f 1329 1322 830
+f 1326 1325 1328
+f 1274 1291 287
+f 295 284 19
+f 1051 1043 1176
+f 464 1331 471
+f 295 45 303
+f 464 1327 1331
+f 306 303 52
+f 824 830 1322
+f 1157 1333 1330
+f 52 303 45
+f 1329 1260 1322
+f 1331 508 471
+f 306 52 56
+f 1264 1260 1329
+f 56 152 306
+f 830 836 1329
+f 1329 836 1332
+f 157 1313 1311
+f 1200 1199 18
+f 206 1315 211
+f 1332 1264 1329
+f 1315 1316 211
+f 508 16 516
+f 380 1203 20
+f 836 842 1332
+f 1179 1144 22
+f 16 1339 516
+f 224 71 320
+f 380 1333 323
+f 71 224 67
+f 101 320 71
+f 1264 1332 1265
+f 320 101 330
+f 1148 53 42
+f 18 1330 1333
+f 20 18 1333
+f 1317 1309 1079
+f 330 101 112
+f 1055 1079 1309
+f 1333 380 20
+f 1079 1085 1317
+f 152 56 1319
+f 1334 1332 842
+f 1085 1320 1317
+f 1265 1332 1334
+f 159 1321 181
+f 1085 1089 1320
+f 1335 1333 1162
+f 1321 159 1319
+f 191 181 1323
+f 1335 1162 1166
+f 1334 1268 1265
+f 1321 1323 181
+f 1323 1313 191
+f 323 1333 1335
+f 1334 845 1336
+f 19 1123 45
+f 842 845 1334
+f 19 1120 1123
+f 45 1123 1127
+f 1268 1334 1336
+f 1337 334 1335
+f 56 52 910
+f 910 52 1127
+f 1339 534 516
+f 1337 338 334
+f 998 56 910
+f 1336 1272 1268
+f 1011 1313 940
+f 1313 1323 940
+f 1337 1335 1166
+f 261 53 1278
+f 1011 1311 1313
+f 35 338 1337
+f 1328 1093 1327
+f 1315 1311 1014
+f 1093 1328 1089
+f 1325 1089 1328
+f 1311 1011 1014
+f 1327 1121 1331
+f 1316 1315 1018
+f 1093 1121 1327
+f 17 508 1331
+f 1315 1014 1018
+f 1330 18 1199
+f 1338 1272 1336
+f 67 1316 1021
+f 547 816 1339
+f 1276 1272 1338
+f 1269 253 261
+f 508 17 16
+f 217 216 1341
+f 1340 1341 216
+f 1021 1152 67
+f 217 1341 234
+f 71 67 1152
+f 1339 50 547
+f 1276 1338 1340
+f 101 71 1154
+f 855 1338 1336
+f 1152 1154 71
+f 1156 101 1154
+f 112 101 1156
+f 861 235 1341
+f 1121 17 1331
+f 1341 1340 1338
+f 998 1319 56
+f 855 861 1338
+f 1341 1338 861
+f 1155 1351 1175
+f 1321 1007 1323
+f 781 1347 68
+f 96 1342 430
+f 25 284 430
+f 1117 1342 96
+f 1155 1343 1351
+f 19 1342 1120
+f 25 1342 19
+f 802 807 1354
+f 804 869 1357
+f 1155 1158 1343
+f 1359 856 860
+f 1343 1158 1344
+f 880 1364 874
+f 353 1343 1344
+f 353 1344 358
+f 358 1344 1345
+f 1345 1344 1163
+f 98 613 906
+f 1163 1179 1345
+f 906 1346 897
+f 80 68 1347
+f 1349 80 1348
+f 1347 1348 80
+f 1347 781 783
+f 783 1350 1347
+f 1348 1347 1350
+f 1352 1350 788
+f 1198 995 1355
+f 1348 1350 1349
+f 788 798 1352
+f 1352 798 1353
+f 1353 802 1354
+f 1353 798 802
+f 1201 1204 245
+f 1351 1343 1355
+f 1357 869 1361
+f 1356 1355 1343
+f 1354 807 1358
+f 1179 22 1345
+f 856 1359 1358
+f 856 1358 807
+f 22 1144 42
+f 1360 1359 860
+f 1374 1360 1357
+f 1360 804 1357
+f 860 804 1360
+f 1204 1198 1356
+f 870 1362 1361
+f 869 870 1361
+f 1356 1343 245
+f 1362 872 1363
+f 874 1364 1363
+f 872 874 1363
+f 880 1365 1364
+f 1365 881 1366
+f 1365 880 881
+f 1366 881 897
+f 1346 1369 1367
+f 897 1346 1367
+f 1366 897 1367
+f 1368 1369 606
+f 1346 616 1369
+f 1369 1368 1367
+f 1349 1379 80
+f 1350 1352 1381
+f 1365 1378 1364
+f 1368 1380 1367
+f 1354 1358 1370
+f 1371 132 164
+f 1361 1371 1357
+f 1371 1361 1375
+f 1358 1372 1370
+f 1358 1359 1372
+f 1372 1359 1373
+f 1374 156 1373
+f 1373 1360 1374
+f 1373 1359 1360
+f 164 1374 1371
+f 1374 1357 1371
+f 1371 1375 132
+f 1375 130 132
+f 1376 142 130
+f 1376 130 1375
+f 1361 1362 1375
+f 1375 1362 1376
+f 1377 142 1376
+f 1376 1363 1377
+f 1362 1363 1376
+f 146 1377 1378
+f 1377 1364 1378
+f 1377 1363 1364
+f 422 80 1379
+f 1382 422 1379
+f 1379 1349 1350
+f 1350 1381 1379
+f 1387 1366 1367
+f 1380 1368 604
+f 606 604 1368
+f 1380 1389 1388
+f 1367 1380 1388
+f 192 748 177
+f 177 1381 184
+f 1379 1381 1382
+f 1383 1382 1381
+f 1383 1381 177
+f 1384 1352 1353
+f 1384 196 184
+f 1381 1352 1384
+f 184 1381 1384
+f 196 1384 1385
+f 1384 1353 1385
+f 1385 1354 1370
+f 1385 1353 1354
+f 1372 207 1370
+f 151 207 1372
+f 151 1373 156
+f 1373 151 1372
+f 142 1377 146
+f 1378 218 146
+f 1378 1386 218
+f 1378 1365 1386
+f 1386 1387 221
+f 1365 1366 1386
+f 1386 1366 1387
+f 1387 1367 1388
+f 223 1387 1388
+f 1388 231 223
+f 1390 1388 1389
+f 231 1388 47
+f 1390 47 1388
+f 1387 223 221
+# 2776 faces
+
+ #end of obj_0
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..0c888dc0f9c9fa8038d79c58114047d35b8ec768
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace_real.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace_real.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..2cc2d1d212670aedd3d8cd4c453e03bd4055898a
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace_real.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.obj b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.obj
new file mode 100644
index 0000000000000000000000000000000000000000..b9855504d8ff40e0452aca924091d155da70b284
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.obj
@@ -0,0 +1,64 @@
+# Object Export From Tinkercad Server 2015
+
+mtllib obj.mtl
+
+o obj_0
+v 10 -10 20
+v 10 -10 0
+v 10 10 0
+v 10 10 20
+v 9.002 9.003 20
+v 9.002 -9.002 20
+v -10 10 0
+v -10 10 20
+v -9.003 9.003 20
+v -9.003 9.003 0
+v 9.002 9.003 0
+v 9.002 -9.002 0
+v -9.003 -9.002 0
+v -9.003 -9.002 20
+v -10 -10 0
+v -10 -10 20
+# 16 vertices
+
+g group_0_15277357
+
+usemtl color_15277357
+s 0
+
+f 1 2 3
+f 1 3 4
+f 4 5 6
+f 4 6 1
+f 9 10 11
+f 9 11 5
+f 6 12 13
+f 6 13 14
+f 10 9 14
+f 10 14 13
+f 7 10 13
+f 7 13 15
+f 4 8 5
+f 9 5 8
+f 8 7 15
+f 8 15 16
+f 10 7 11
+f 3 11 7
+f 11 3 12
+f 2 12 3
+f 14 16 6
+f 1 6 16
+f 16 15 2
+f 16 2 1
+f 9 8 14
+f 16 14 8
+f 7 8 3
+f 4 3 8
+f 2 15 12
+f 13 12 15
+f 12 6 5
+f 12 5 11
+# 32 faces
+
+ #end of obj_0
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..7cc2395e7b0752b23ab8b476d70be658e9092a13
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.urdf
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone2.urdf b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone2.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..672edafb7a446d200f8e12184411b6c7be7a7c71
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone2.urdf
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing.py
new file mode 100644
index 0000000000000000000000000000000000000000..ed2310facab8d67a25496086d61c37937fb865e0
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing.py
@@ -0,0 +1,1092 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Simple block environments for the XArm."""
+
+import collections
+import enum
+import math
+import time
+from typing import Dict, List, Optional, Tuple, Union
+
+import gym
+from gym import spaces
+from gym.envs import registration
+from diffusion_policy.env.block_pushing.utils import utils_pybullet
+from diffusion_policy.env.block_pushing.utils import xarm_sim_robot
+from diffusion_policy.env.block_pushing.utils.pose3d import Pose3d
+from diffusion_policy.env.block_pushing.utils.utils_pybullet import ObjState
+from diffusion_policy.env.block_pushing.utils.utils_pybullet import XarmState
+import numpy as np
+from scipy.spatial import transform
+import pybullet
+import pybullet_utils.bullet_client as bullet_client
+
+import matplotlib.pyplot as plt
+
+BLOCK_URDF_PATH = "third_party/py/envs/assets/block.urdf"
+PLANE_URDF_PATH = "third_party/bullet/examples/pybullet/gym/pybullet_data/" "plane.urdf"
+WORKSPACE_URDF_PATH = "third_party/py/envs/assets/workspace.urdf"
+ZONE_URDF_PATH = "third_party/py/envs/assets/zone.urdf"
+INSERT_URDF_PATH = "third_party/py/envs/assets/insert.urdf"
+
+EFFECTOR_HEIGHT = 0.06
+EFFECTOR_DOWN_ROTATION = transform.Rotation.from_rotvec([0, math.pi, 0])
+
+WORKSPACE_BOUNDS = np.array(((0.15, -0.5), (0.7, 0.5)))
+
+# Min/max bounds calculated from oracle data using:
+# ibc/environments/board2d_dataset_statistics.ipynb
+# to calculate [mean - 3 * std, mean + 3 * std] using the oracle data.
+# pylint: disable=line-too-long
+ACTION_MIN = np.array([-0.02547718, -0.02090043], np.float32)
+ACTION_MAX = np.array([0.02869084, 0.04272365], np.float32)
+EFFECTOR_TARGET_TRANSLATION_MIN = np.array(
+ [0.1774151772260666, -0.6287994794547558], np.float32
+)
+EFFECTOR_TARGET_TRANSLATION_MAX = np.array(
+ [0.5654461532831192, 0.5441607423126698], np.float32
+)
+EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MIN = np.array(
+ [-0.07369826920330524, -0.11395704373717308], np.float32
+)
+EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MAX = np.array(
+ [0.10131562314927578, 0.19391131028532982], np.float32
+)
+EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MIN = np.array(
+ [-0.17813862301409245, -0.3309651017189026], np.float32
+)
+EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MAX = np.array(
+ [0.23726161383092403, 0.8404090404510498], np.float32
+)
+BLOCK_ORIENTATION_COS_SIN_MIN = np.array(
+ [-2.0649861991405487, -0.6154364347457886], np.float32
+)
+BLOCK_ORIENTATION_COS_SIN_MAX = np.array(
+ [1.6590178310871124, 1.8811014890670776], np.float32
+)
+TARGET_ORIENTATION_COS_SIN_MIN = np.array(
+ [-1.0761439241468906, -0.8846937336493284], np.float32
+)
+TARGET_ORIENTATION_COS_SIN_MAX = np.array(
+ [-0.8344330154359341, 0.8786859593819827], np.float32
+)
+
+# Hardcoded Pose joints to make sure we don't have surprises from using the
+# IK solver on reset. The joint poses correspond to the Pose with:
+# rotation = rotation3.Rotation3.from_axis_angle([0, 1, 0], math.pi)
+# translation = np.array([0.3, -0.4, 0.07])
+INITIAL_JOINT_POSITIONS = np.array(
+ [
+ -0.9254632489674508,
+ 0.6990770671568564,
+ -1.106629064060494,
+ 0.0006653351931553931,
+ 0.3987969742311386,
+ -4.063402065624296,
+ ]
+)
+
+DEFAULT_CAMERA_POSE = (1.0, 0, 0.75)
+DEFAULT_CAMERA_ORIENTATION = (np.pi / 4, np.pi, -np.pi / 2)
+IMAGE_WIDTH = 320
+IMAGE_HEIGHT = 240
+CAMERA_INTRINSICS = (
+ 0.803 * IMAGE_WIDTH, # fx
+ 0,
+ IMAGE_WIDTH / 2.0, # cx
+ 0,
+ 0.803 * IMAGE_WIDTH, # fy
+ IMAGE_HEIGHT / 2.0, # cy
+ 0,
+ 0,
+ 1,
+)
+
+# "Realistic" visuals.
+X_MIN_REAL = 0.15
+X_MAX_REAL = 0.6
+Y_MIN_REAL = -0.3048
+Y_MAX_REAL = 0.3048
+WORKSPACE_BOUNDS_REAL = np.array(((X_MIN_REAL, Y_MIN_REAL), (X_MAX_REAL, Y_MAX_REAL)))
+WORKSPACE_URDF_PATH_REAL = "third_party/py/ibc/environments/assets/workspace_real.urdf"
+CAMERA_POSE_REAL = (0.75, 0, 0.5)
+CAMERA_ORIENTATION_REAL = (np.pi / 5, np.pi, -np.pi / 2)
+
+IMAGE_WIDTH_REAL = 320
+IMAGE_HEIGHT_REAL = 180
+CAMERA_INTRINSICS_REAL = (
+ 0.803 * IMAGE_WIDTH_REAL, # fx
+ 0,
+ IMAGE_WIDTH_REAL / 2.0, # cx
+ 0,
+ 0.803 * IMAGE_WIDTH_REAL, # fy
+ IMAGE_HEIGHT_REAL / 2.0, # cy
+ 0,
+ 0,
+ 1,
+)
+# pylint: enable=line-too-long
+
+
+def build_env_name(task, shared_memory, use_image_obs, use_normalized_env=False):
+ """Construct the env name from parameters."""
+ if isinstance(task, str):
+ task = BlockTaskVariant[task]
+ env_name = "Block" + task.value
+
+ if use_image_obs:
+ env_name = env_name + "Rgb"
+ if use_normalized_env:
+ env_name = env_name + "Normalized"
+ if shared_memory:
+ env_name = "Shared" + env_name
+
+ env_name = env_name + "-v0"
+
+ return env_name
+
+
+class BlockTaskVariant(enum.Enum):
+ REACH = "Reach"
+ REACH_NORMALIZED = "ReachNormalized"
+ PUSH = "Push"
+ PUSH_NORMALIZED = "PushNormalized"
+ INSERT = "Insert"
+
+
+def sleep_spin(sleep_time_sec):
+ """Spin wait sleep. Avoids time.sleep accuracy issues on Windows."""
+ if sleep_time_sec <= 0:
+ return
+ t0 = time.perf_counter()
+ while time.perf_counter() - t0 < sleep_time_sec:
+ pass
+
+
+class BlockPush(gym.Env):
+ """Simple XArm environment for block pushing."""
+
+ def __init__(
+ self,
+ control_frequency=10.0,
+ task=BlockTaskVariant.PUSH,
+ image_size=None,
+ shared_memory=False,
+ seed=None,
+ goal_dist_tolerance=0.01,
+ effector_height=None,
+ visuals_mode="default",
+ abs_action=False
+ ):
+ """Creates an env instance.
+
+ Args:
+ control_frequency: Control frequency for the arm. Each env step will
+ advance the simulation by 1/control_frequency seconds.
+ task: enum for which task, see BlockTaskVariant enum.
+ image_size: Optional image size (height, width). If None, no image
+ observations will be used.
+ shared_memory: If True `pybullet.SHARED_MEMORY` is used to connect to
+ pybullet. Useful to debug.
+ seed: Optional seed for the environment.
+ goal_dist_tolerance: float, how far away from the goal to terminate.
+ effector_height: float, custom height for end effector.
+ visuals_mode: 'default' or 'real'.
+ """
+ # pybullet.connect(pybullet.GUI)
+ # pybullet.resetDebugVisualizerCamera(
+ # cameraDistance=1.5,
+ # cameraYaw=0,
+ # cameraPitch=-40,
+ # cameraTargetPosition=[0.55, -0.35, 0.2],
+ # )
+ if visuals_mode != "default" and visuals_mode != "real":
+ raise ValueError("visuals_mode must be `real` or `default`.")
+ self._task = task
+ self._connection_mode = pybullet.DIRECT
+ if shared_memory:
+ self._connection_mode = pybullet.SHARED_MEMORY
+
+ self.goal_dist_tolerance = goal_dist_tolerance
+
+ self.effector_height = effector_height or EFFECTOR_HEIGHT
+
+ self._visuals_mode = visuals_mode
+ if visuals_mode == "default":
+ self._camera_pose = DEFAULT_CAMERA_POSE
+ self._camera_orientation = DEFAULT_CAMERA_ORIENTATION
+ self.workspace_bounds = WORKSPACE_BOUNDS
+ self._image_size = image_size
+ self._camera_instrinsics = CAMERA_INTRINSICS
+ self._workspace_urdf_path = WORKSPACE_URDF_PATH
+ else:
+ self._camera_pose = CAMERA_POSE_REAL
+ self._camera_orientation = CAMERA_ORIENTATION_REAL
+ self.workspace_bounds = WORKSPACE_BOUNDS_REAL
+ self._image_size = image_size
+ self._camera_instrinsics = CAMERA_INTRINSICS_REAL
+ self._workspace_urdf_path = WORKSPACE_URDF_PATH_REAL
+
+ self.action_space = spaces.Box(low=-0.1, high=0.1, shape=(2,)) # x, y
+ self.observation_space = self._create_observation_space(image_size)
+
+ self._rng = np.random.RandomState(seed=seed)
+ self._block_ids = None
+ self._previous_state = None
+ self._robot = None
+ self._workspace_uid = None
+ self._target_id = None
+ self._target_pose = None
+ self._target_effector_pose = None
+ self._pybullet_client = None
+ self.reach_target_translation = None
+ self._setup_pybullet_scene()
+ self._saved_state = None
+
+ assert isinstance(self._pybullet_client, bullet_client.BulletClient)
+ self._control_frequency = control_frequency
+ self._step_frequency = (
+ 1 / self._pybullet_client.getPhysicsEngineParameters()["fixedTimeStep"]
+ )
+
+ self._last_loop_time = None
+ self._last_loop_frame_sleep_time = None
+ if self._step_frequency % self._control_frequency != 0:
+ raise ValueError(
+ "Control frequency should be a multiple of the "
+ "configured Bullet TimeStep."
+ )
+ self._sim_steps_per_step = int(self._step_frequency / self._control_frequency)
+
+ self.rendered_img = None
+ self._abs_action = abs_action
+
+ # Use saved_state and restore to make reset safe as no simulation state has
+ # been updated at this state, but the assets are now loaded.
+ self.save_state()
+ self.reset()
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def workspace_uid(self):
+ return self._workspace_uid
+
+ @property
+ def target_effector_pose(self):
+ return self._target_effector_pose
+
+ @property
+ def target_pose(self):
+ return self._target_pose
+
+ @property
+ def control_frequency(self):
+ return self._control_frequency
+
+ @property
+ def connection_mode(self):
+ return self._connection_mode
+
+ def save_state(self):
+ self._saved_state = self._pybullet_client.saveState()
+
+ def set_goal_dist_tolerance(self, val):
+ self.goal_dist_tolerance = val
+
+ def get_control_frequency(self):
+ return self._control_frequency
+
+ def compute_state(self):
+ return self._compute_state()
+
+ def get_goal_translation(self):
+ """Return the translation component of the goal (2D)."""
+ if self._task == BlockTaskVariant.REACH:
+ return np.concatenate([self.reach_target_translation, [0]])
+ else:
+ return self._target_pose.translation if self._target_pose else None
+
+ def get_obj_ids(self):
+ return self._block_ids
+
+ def _setup_workspace_and_robot(self, end_effector="suction"):
+ self._pybullet_client.resetSimulation()
+ self._pybullet_client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+ self._pybullet_client.setPhysicsEngineParameter(enableFileCaching=0)
+ self._pybullet_client.setGravity(0, 0, -9.8)
+
+ utils_pybullet.load_urdf(
+ self._pybullet_client, PLANE_URDF_PATH, basePosition=[0, 0, -0.001]
+ )
+ self._workspace_uid = utils_pybullet.load_urdf(
+ self._pybullet_client,
+ self._workspace_urdf_path,
+ basePosition=[0.35, 0, 0.0],
+ )
+
+ self._robot = xarm_sim_robot.XArmSimRobot(
+ self._pybullet_client,
+ initial_joint_positions=INITIAL_JOINT_POSITIONS,
+ end_effector=end_effector,
+ color="white" if self._visuals_mode == "real" else "default",
+ )
+
+ def _setup_pybullet_scene(self):
+ self._pybullet_client = bullet_client.BulletClient(self._connection_mode)
+
+ # Temporarily disable rendering to speed up loading URDFs.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
+
+ self._setup_workspace_and_robot()
+
+ if self._task == BlockTaskVariant.INSERT:
+ target_urdf_path = INSERT_URDF_PATH
+ else:
+ target_urdf_path = ZONE_URDF_PATH
+
+ self._target_id = utils_pybullet.load_urdf(
+ self._pybullet_client, target_urdf_path, useFixedBase=True
+ )
+ self._block_ids = [
+ utils_pybullet.load_urdf(
+ self._pybullet_client, BLOCK_URDF_PATH, useFixedBase=False
+ )
+ ]
+
+ # Re-enable rendering.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
+
+ self.step_simulation_to_stabilize()
+
+ def step_simulation_to_stabilize(self, nsteps=100):
+ for _ in range(nsteps):
+ self._pybullet_client.stepSimulation()
+
+ def seed(self, seed=None):
+ self._rng = np.random.RandomState(seed=seed)
+
+ def _set_robot_target_effector_pose(self, pose):
+ self._target_effector_pose = pose
+ self._robot.set_target_effector_pose(pose)
+
+ def reset(self, reset_poses=True):
+ workspace_center_x = 0.4
+
+ if reset_poses:
+ self._pybullet_client.restoreState(self._saved_state)
+
+ rotation = transform.Rotation.from_rotvec([0, math.pi, 0])
+ translation = np.array([0.3, -0.4, self.effector_height])
+ starting_pose = Pose3d(rotation=rotation, translation=translation)
+ self._set_robot_target_effector_pose(starting_pose)
+
+ # Reset block pose.
+ block_x = workspace_center_x + self._rng.uniform(low=-0.1, high=0.1)
+ block_y = -0.2 + self._rng.uniform(low=-0.15, high=0.15)
+ block_translation = np.array([block_x, block_y, 0])
+ block_sampled_angle = self._rng.uniform(math.pi)
+ block_rotation = transform.Rotation.from_rotvec([0, 0, block_sampled_angle])
+
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._block_ids[0],
+ block_translation.tolist(),
+ block_rotation.as_quat().tolist(),
+ )
+
+ # Reset target pose.
+ target_x = workspace_center_x + self._rng.uniform(low=-0.10, high=0.10)
+ target_y = 0.2 + self._rng.uniform(low=-0.15, high=0.15)
+ target_translation = np.array([target_x, target_y, 0.020])
+
+ target_sampled_angle = math.pi + self._rng.uniform(
+ low=-math.pi / 6, high=math.pi / 6
+ )
+ target_rotation = transform.Rotation.from_rotvec(
+ [0, 0, target_sampled_angle]
+ )
+
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._target_id,
+ target_translation.tolist(),
+ target_rotation.as_quat().tolist(),
+ )
+ else:
+ (
+ target_translation,
+ target_orientation_quat,
+ ) = self._pybullet_client.getBasePositionAndOrientation(self._target_id)
+ target_rotation = transform.Rotation.from_quat(target_orientation_quat)
+ target_translation = np.array(target_translation)
+
+ self._target_pose = Pose3d(
+ rotation=target_rotation, translation=target_translation
+ )
+
+ if reset_poses:
+ self.step_simulation_to_stabilize()
+
+ state = self._compute_state()
+ self._previous_state = state
+
+ if self._task == BlockTaskVariant.REACH:
+ self._compute_reach_target(state)
+
+ self._init_goal_distance = self._compute_goal_distance(state)
+ init_goal_eps = 1e-7
+ assert self._init_goal_distance > init_goal_eps
+ self.best_fraction_reduced_goal_dist = 0.0
+
+ return state
+
+ def _compute_goal_distance(self, state):
+ goal_translation = self.get_goal_translation()
+ if self._task != BlockTaskVariant.REACH:
+ goal_distance = np.linalg.norm(
+ state["block_translation"] - goal_translation[0:2]
+ )
+ else:
+ goal_distance = np.linalg.norm(
+ state["effector_translation"] - goal_translation[0:2]
+ )
+ return goal_distance
+
+ def _compute_reach_target(self, state):
+ xy_block = state["block_translation"]
+ xy_target = state["target_translation"]
+
+ xy_block_to_target = xy_target - xy_block
+ xy_dir_block_to_target = (xy_block_to_target) / np.linalg.norm(
+ xy_block_to_target
+ )
+ self.reach_target_translation = xy_block + -1 * xy_dir_block_to_target * 0.05
+
+ def _compute_state(self):
+ effector_pose = self._robot.forward_kinematics()
+ block_position_and_orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self._block_ids[0])
+ )
+ block_pose = Pose3d(
+ rotation=transform.Rotation.from_quat(block_position_and_orientation[1]),
+ translation=block_position_and_orientation[0],
+ )
+
+ def _yaw_from_pose(pose):
+ return np.array([pose.rotation.as_euler("xyz", degrees=False)[-1]])
+
+ obs = collections.OrderedDict(
+ block_translation=block_pose.translation[0:2],
+ block_orientation=_yaw_from_pose(block_pose),
+ effector_translation=effector_pose.translation[0:2],
+ effector_target_translation=self._target_effector_pose.translation[0:2],
+ target_translation=self._target_pose.translation[0:2],
+ target_orientation=_yaw_from_pose(self._target_pose),
+ )
+ if self._image_size is not None:
+ obs["rgb"] = self._render_camera(self._image_size)
+ return obs
+
+ def _step_robot_and_sim(self, action):
+ """Steps the robot and pybullet sim."""
+ # Compute target_effector_pose by shifting the effector's pose by the
+ # action.
+ if self._abs_action:
+ target_effector_translation = np.array([action[0], action[1], 0])
+ else:
+ target_effector_translation = np.array(
+ self._target_effector_pose.translation
+ ) + np.array([action[0], action[1], 0])
+
+ target_effector_translation[0:2] = np.clip(
+ target_effector_translation[0:2],
+ self.workspace_bounds[0],
+ self.workspace_bounds[1],
+ )
+ target_effector_translation[-1] = self.effector_height
+ target_effector_pose = Pose3d(
+ rotation=EFFECTOR_DOWN_ROTATION, translation=target_effector_translation
+ )
+
+ self._set_robot_target_effector_pose(target_effector_pose)
+
+ # Update sleep time dynamically to stay near real-time.
+ frame_sleep_time = 0
+ if self._connection_mode == pybullet.SHARED_MEMORY:
+ cur_time = time.time()
+ if self._last_loop_time is not None:
+ # Calculate the total, non-sleeping time from the previous frame, this
+ # includes the actual step as well as any compute that happens in the
+ # caller thread (model inference, etc).
+ compute_time = (
+ cur_time
+ - self._last_loop_time
+ - self._last_loop_frame_sleep_time * self._sim_steps_per_step
+ )
+ # Use this to calculate the current frame's total sleep time to ensure
+ # that env.step runs at policy rate. This is an estimate since the
+ # previous frame's compute time may not match the current frame.
+ total_sleep_time = max((1 / self._control_frequency) - compute_time, 0)
+ # Now spread this out over the inner sim steps. This doesn't change
+ # control in any way, but makes the animation appear smooth.
+ frame_sleep_time = total_sleep_time / self._sim_steps_per_step
+ else:
+ # No estimate of the previous frame's compute, assume it is zero.
+ frame_sleep_time = 1 / self._step_frequency
+
+ # Cache end of this loop time, to compute sleep time on next iteration.
+ self._last_loop_time = cur_time
+ self._last_loop_frame_sleep_time = frame_sleep_time
+
+ for _ in range(self._sim_steps_per_step):
+ if self._connection_mode == pybullet.SHARED_MEMORY:
+ sleep_spin(frame_sleep_time)
+ self._pybullet_client.stepSimulation()
+
+ def step(self, action):
+ self._step_robot_and_sim(action)
+
+ state = self._compute_state()
+
+ goal_distance = self._compute_goal_distance(state)
+ fraction_reduced_goal_distance = 1.0 - (
+ goal_distance / self._init_goal_distance
+ )
+ if fraction_reduced_goal_distance > self.best_fraction_reduced_goal_dist:
+ self.best_fraction_reduced_goal_dist = fraction_reduced_goal_distance
+
+ done = False
+ reward = self.best_fraction_reduced_goal_dist
+
+ # Terminate the episode if the block is close enough to the target.
+ if goal_distance < self.goal_dist_tolerance:
+ reward = 1.0
+ done = True
+
+ return state, reward, done, {}
+
+ @property
+ def succeeded(self):
+ state = self._compute_state()
+ goal_distance = self._compute_goal_distance(state)
+ if goal_distance < self.goal_dist_tolerance:
+ return True
+ return False
+
+ @property
+ def goal_distance(self):
+ state = self._compute_state()
+ return self._compute_goal_distance(state)
+
+ def render(self, mode="rgb_array"):
+ if self._image_size is not None:
+ image_size = self._image_size
+ else:
+ # This allows rendering even for state-only obs,
+ # for visualization.
+ image_size = (IMAGE_HEIGHT, IMAGE_WIDTH)
+
+ data = self._render_camera(image_size=(image_size[0], image_size[1]))
+ if mode == "human":
+ if self.rendered_img is None:
+ self.rendered_img = plt.imshow(
+ np.zeros((image_size[0], image_size[1], 4))
+ )
+ else:
+ self.rendered_img.set_data(data)
+ plt.draw()
+ plt.pause(0.00001)
+ return data
+
+ def close(self):
+ self._pybullet_client.disconnect()
+
+ def calc_camera_params(self, image_size):
+ # Mimic RealSense D415 camera parameters.
+ intrinsics = self._camera_instrinsics
+
+ # Set default camera poses.
+ front_position = self._camera_pose
+ front_rotation = self._camera_orientation
+ front_rotation = self._pybullet_client.getQuaternionFromEuler(front_rotation)
+ # Default camera configs.
+ zrange = (0.01, 10.0)
+
+ # OpenGL camera settings.
+ lookdir = np.float32([0, 0, 1]).reshape(3, 1)
+ updir = np.float32([0, -1, 0]).reshape(3, 1)
+ rotation = self._pybullet_client.getMatrixFromQuaternion(front_rotation)
+ rotm = np.float32(rotation).reshape(3, 3)
+ lookdir = (rotm @ lookdir).reshape(-1)
+ updir = (rotm @ updir).reshape(-1)
+ lookat = front_position + lookdir
+ focal_len = intrinsics[0]
+ znear, zfar = zrange
+ viewm = self._pybullet_client.computeViewMatrix(front_position, lookat, updir)
+ fovh = (image_size[0] / 2) / focal_len
+ fovh = 180 * np.arctan(fovh) * 2 / np.pi
+
+ # Notes: 1) FOV is vertical FOV 2) aspect must be float
+ aspect_ratio = image_size[1] / image_size[0]
+ projm = self._pybullet_client.computeProjectionMatrixFOV(
+ fovh, aspect_ratio, znear, zfar
+ )
+
+ return viewm, projm, front_position, lookat, updir
+
+ def _render_camera(self, image_size):
+ """Render RGB image with RealSense configuration."""
+ viewm, projm, _, _, _ = self.calc_camera_params(image_size)
+
+ # Render with OpenGL camera settings.
+ _, _, color, _, _ = self._pybullet_client.getCameraImage(
+ width=image_size[1],
+ height=image_size[0],
+ viewMatrix=viewm,
+ projectionMatrix=projm,
+ flags=pybullet.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+ renderer=pybullet.ER_BULLET_HARDWARE_OPENGL,
+ )
+
+ # Get color image.
+ color_image_size = (image_size[0], image_size[1], 4)
+ color = np.array(color, dtype=np.uint8).reshape(color_image_size)
+ color = color[:, :, :3] # remove alpha channel
+
+ return color.astype(np.uint8)
+
+ def _create_observation_space(self, image_size):
+ pi2 = math.pi * 2
+
+ obs_dict = collections.OrderedDict(
+ block_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ block_orientation=spaces.Box(low=-pi2, high=pi2, shape=(1,)), # phi
+ effector_translation=spaces.Box(
+ low=self.workspace_bounds[0] - 0.1, # Small buffer for to IK noise.
+ high=self.workspace_bounds[1] + 0.1,
+ ), # x,y
+ effector_target_translation=spaces.Box(
+ low=self.workspace_bounds[0] - 0.1, # Small buffer for to IK noise.
+ high=self.workspace_bounds[1] + 0.1,
+ ), # x,y
+ target_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ target_orientation=spaces.Box(
+ low=-pi2,
+ high=pi2,
+ shape=(1,),
+ ), # theta
+ )
+ if image_size is not None:
+ obs_dict["rgb"] = spaces.Box(
+ low=0, high=255, shape=(image_size[0], image_size[1], 3), dtype=np.uint8
+ )
+ return spaces.Dict(obs_dict)
+
+ def get_pybullet_state(self):
+ """Save pybullet state of the scene.
+
+ Returns:
+ dict containing 'robots', 'robot_end_effectors', 'targets', 'objects',
+ each containing a list of ObjState.
+ """
+ state: Dict[str, List[ObjState]] = {}
+
+ state["robots"] = [
+ XarmState.get_bullet_state(
+ self._pybullet_client,
+ self.robot.xarm,
+ target_effector_pose=self._target_effector_pose,
+ goal_translation=self.get_goal_translation(),
+ )
+ ]
+
+ state["robot_end_effectors"] = []
+ if self.robot.end_effector:
+ state["robot_end_effectors"].append(
+ ObjState.get_bullet_state(
+ self._pybullet_client, self.robot.end_effector
+ )
+ )
+
+ state["targets"] = []
+ if self._target_id:
+ state["targets"].append(
+ ObjState.get_bullet_state(self._pybullet_client, self._target_id)
+ )
+
+ state["objects"] = []
+ for obj_id in self.get_obj_ids():
+ state["objects"].append(
+ ObjState.get_bullet_state(self._pybullet_client, obj_id)
+ )
+
+ return state
+
+ def set_pybullet_state(self, state):
+ """Restore pyullet state.
+
+ WARNING: py_environment wrapper assumes environments aren't reset in their
+ constructor and will often reset the environment unintentionally. It is
+ always recommended that you call env.reset on the tfagents wrapper before
+ playback (replaying pybullet_state).
+
+ Args:
+ state: dict containing 'robots', 'robot_end_effectors', 'targets',
+ 'objects', each containing a list of ObjState.
+ """
+
+ assert isinstance(state["robots"][0], XarmState)
+ xarm_state: XarmState = state["robots"][0]
+ xarm_state.set_bullet_state(self._pybullet_client, self.robot.xarm)
+ self._set_robot_target_effector_pose(xarm_state.target_effector_pose)
+
+ def _set_state_safe(obj_state, obj_id):
+ if obj_state is not None:
+ assert obj_id is not None, "Cannot set state for missing object."
+ obj_state.set_bullet_state(self._pybullet_client, obj_id)
+ else:
+ assert obj_id is None, f"No state found for obj_id {obj_id}"
+
+ robot_end_effectors = state["robot_end_effectors"]
+ _set_state_safe(
+ None if not robot_end_effectors else robot_end_effectors[0],
+ self.robot.end_effector,
+ )
+
+ targets = state["targets"]
+ _set_state_safe(None if not targets else targets[0], self._target_id)
+
+ obj_ids = self.get_obj_ids()
+ assert len(state["objects"]) == len(obj_ids), "State length mismatch"
+ for obj_state, obj_id in zip(state["objects"], obj_ids):
+ _set_state_safe(obj_state, obj_id)
+
+ self.reset(reset_poses=False)
+
+
+class BlockPushNormalized(gym.Env):
+ """Simple XArm environment for block pushing, normalized state and actions."""
+
+ def __init__(
+ self,
+ control_frequency=10.0,
+ task=BlockTaskVariant.PUSH_NORMALIZED,
+ image_size=None,
+ shared_memory=False,
+ seed=None,
+ ):
+ """Creates an env instance.
+
+ Args:
+ control_frequency: Control frequency for the arm. Each env step will
+ advance the simulation by 1/control_frequency seconds.
+ task: enum for which task, see BlockTaskVariant enum.
+ image_size: Optional image size (height, width). If None, no image
+ observations will be used.
+ shared_memory: If True `pybullet.SHARED_MEMORY` is used to connect to
+ pybullet. Useful to debug.
+ seed: Optional seed for the environment.
+ """
+ # Map normalized task to unnormalized task.
+ if task == BlockTaskVariant.PUSH_NORMALIZED:
+ env_task = BlockTaskVariant.PUSH
+ elif task == BlockTaskVariant.REACH_NORMALIZED:
+ env_task = BlockTaskVariant.REACH
+ else:
+ raise ValueError("Unsupported task %s" % str(task))
+ self._env = BlockPush(
+ control_frequency, env_task, image_size, shared_memory, seed
+ )
+ self.action_space = spaces.Box(low=-1, high=1, shape=(2,))
+ self.observation_space = spaces.Dict(
+ collections.OrderedDict(
+ effector_target_translation=spaces.Box(low=-1, high=1, shape=(2,)),
+ effector_target_to_block_translation=spaces.Box(
+ low=-1, high=1, shape=(2,)
+ ),
+ block_orientation_cos_sin=spaces.Box(low=-1, high=1, shape=(2,)),
+ effector_target_to_target_translation=spaces.Box(
+ low=-1, high=1, shape=(2,)
+ ),
+ target_orientation_cos_sin=spaces.Box(low=-1, high=1, shape=(2,)),
+ )
+ )
+ self.reset()
+
+ def get_control_frequency(self):
+ return self._env.get_control_frequency()
+
+ @property
+ def reach_target_translation(self):
+ return self._env.reach_target_translation
+
+ def seed(self, seed=None):
+ self._env.seed(seed)
+
+ def reset(self):
+ state = self._env.reset()
+ return self.calc_normalized_state(state)
+
+ def step(self, action):
+ # The environment is normalized [mean-3*std, mean+3*std] -> [-1, 1].
+ action = np.clip(action, a_min=-1.0, a_max=1.0)
+ state, reward, done, info = self._env.step(
+ self.calc_unnormalized_action(action)
+ )
+ state = self.calc_normalized_state(state)
+ reward = reward * 100 # Keep returns in [0, 100]
+ return state, reward, done, info
+
+ def render(self, mode="rgb_array"):
+ return self._env.render(mode)
+
+ def close(self):
+ self._env.close()
+
+ @staticmethod
+ def _normalize(values, values_min, values_max):
+ offset = (values_max + values_min) * 0.5
+ scale = (values_max - values_min) * 0.5
+ return (values - offset) / scale # [min, max] -> [-1, 1]
+
+ @staticmethod
+ def _unnormalize(values, values_min, values_max):
+ offset = (values_max + values_min) * 0.5
+ scale = (values_max - values_min) * 0.5
+ return values * scale + offset # [-1, 1] -> [min, max]
+
+ @classmethod
+ def calc_normalized_action(cls, action):
+ return cls._normalize(action, ACTION_MIN, ACTION_MAX)
+
+ @classmethod
+ def calc_unnormalized_action(cls, norm_action):
+ return cls._unnormalize(norm_action, ACTION_MIN, ACTION_MAX)
+
+ @classmethod
+ def calc_normalized_state(cls, state):
+
+ effector_target_translation = cls._normalize(
+ state["effector_target_translation"],
+ EFFECTOR_TARGET_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TRANSLATION_MAX,
+ )
+
+ effector_target_to_block_translation = cls._normalize(
+ state["block_translation"] - state["effector_target_translation"],
+ EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MAX,
+ )
+ ori = state["block_orientation"][0]
+ block_orientation_cos_sin = cls._normalize(
+ np.array([math.cos(ori), math.sin(ori)], np.float32),
+ BLOCK_ORIENTATION_COS_SIN_MIN,
+ BLOCK_ORIENTATION_COS_SIN_MAX,
+ )
+
+ effector_target_to_target_translation = cls._normalize(
+ state["target_translation"] - state["effector_target_translation"],
+ EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MAX,
+ )
+ ori = state["target_orientation"][0]
+ target_orientation_cos_sin = cls._normalize(
+ np.array([math.cos(ori), math.sin(ori)], np.float32),
+ TARGET_ORIENTATION_COS_SIN_MIN,
+ TARGET_ORIENTATION_COS_SIN_MAX,
+ )
+
+ # Note: We do not include effector_translation in the normalized state.
+ # This means the unnormalized -> normalized mapping is not invertable.
+ return collections.OrderedDict(
+ effector_target_translation=effector_target_translation,
+ effector_target_to_block_translation=effector_target_to_block_translation,
+ block_orientation_cos_sin=block_orientation_cos_sin,
+ effector_target_to_target_translation=effector_target_to_target_translation,
+ target_orientation_cos_sin=target_orientation_cos_sin,
+ )
+
+ @classmethod
+ def calc_unnormalized_state(cls, norm_state):
+
+ effector_target_translation = cls._unnormalize(
+ norm_state["effector_target_translation"],
+ EFFECTOR_TARGET_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TRANSLATION_MAX,
+ )
+ # Note: normalized state does not include effector_translation state, this
+ # means this component will be missing (and is marked nan).
+ effector_translation = np.array([np.nan, np.nan], np.float32)
+
+ effector_target_to_block_translation = cls._unnormalize(
+ norm_state["effector_target_to_block_translation"],
+ EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TO_BLOCK_TRANSLATION_MAX,
+ )
+ block_translation = (
+ effector_target_to_block_translation + effector_target_translation
+ )
+ ori_cos_sin = cls._unnormalize(
+ norm_state["block_orientation_cos_sin"],
+ BLOCK_ORIENTATION_COS_SIN_MIN,
+ BLOCK_ORIENTATION_COS_SIN_MAX,
+ )
+ block_orientation = np.array(
+ [math.atan2(ori_cos_sin[1], ori_cos_sin[0])], np.float32
+ )
+
+ effector_target_to_target_translation = cls._unnormalize(
+ norm_state["effector_target_to_target_translation"],
+ EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MIN,
+ EFFECTOR_TARGET_TO_TARGET_TRANSLATION_MAX,
+ )
+ target_translation = (
+ effector_target_to_target_translation + effector_target_translation
+ )
+ ori_cos_sin = cls._unnormalize(
+ norm_state["target_orientation_cos_sin"],
+ TARGET_ORIENTATION_COS_SIN_MIN,
+ TARGET_ORIENTATION_COS_SIN_MAX,
+ )
+ target_orientation = np.array(
+ [math.atan2(ori_cos_sin[1], ori_cos_sin[0])], np.float32
+ )
+
+ return collections.OrderedDict(
+ block_translation=block_translation,
+ block_orientation=block_orientation,
+ effector_translation=effector_translation,
+ effector_target_translation=effector_target_translation,
+ target_translation=target_translation,
+ target_orientation=target_orientation,
+ )
+
+ def get_pybullet_state(self):
+ return self._env.get_pybullet_state()
+
+ def set_pybullet_state(self, state):
+ return self._env.set_pybullet_state(state)
+
+ @property
+ def pybullet_client(self):
+ return self._env.pybullet_client
+
+ def calc_camera_params(self, image_size):
+ return self._env.calc_camera_params(image_size)
+
+ def _compute_state(self):
+ return self.calc_normalized_state(
+ self._env._compute_state()
+ ) # pylint: disable=protected-access
+
+
+# Make sure we only register once to allow us to reload the module in colab for
+# debugging.
+if "BlockPush-v0" in registration.registry.env_specs:
+ del registration.registry.env_specs["BlockInsert-v0"]
+ del registration.registry.env_specs["BlockPush-v0"]
+ del registration.registry.env_specs["BlockPushNormalized-v0"]
+ del registration.registry.env_specs["BlockPushRgbNormalized-v0"]
+ del registration.registry.env_specs["BlockReach-v0"]
+ del registration.registry.env_specs["BlockReachNormalized-v0"]
+ del registration.registry.env_specs["BlockReachRgbNormalized-v0"]
+ del registration.registry.env_specs["SharedBlockInsert-v0"]
+ del registration.registry.env_specs["SharedBlockPush-v0"]
+ del registration.registry.env_specs["SharedBlockReach-v0"]
+
+registration.register(
+ id="BlockInsert-v0",
+ entry_point=BlockPush,
+ kwargs=dict(task=BlockTaskVariant.INSERT),
+ max_episode_steps=50,
+)
+registration.register(id="BlockPush-v0", entry_point=BlockPush, max_episode_steps=100)
+registration.register(
+ id="BlockPushNormalized-v0",
+ entry_point=BlockPushNormalized,
+ kwargs=dict(task=BlockTaskVariant.PUSH_NORMALIZED),
+ max_episode_steps=100,
+)
+registration.register(
+ id="BlockPushRgb-v0",
+ entry_point=BlockPush,
+ max_episode_steps=100,
+ kwargs=dict(image_size=(IMAGE_HEIGHT, IMAGE_WIDTH)),
+)
+registration.register(
+ id="BlockPushRgbNormalized-v0",
+ entry_point=BlockPushNormalized,
+ kwargs=dict(
+ task=BlockTaskVariant.PUSH_NORMALIZED, image_size=(IMAGE_HEIGHT, IMAGE_WIDTH)
+ ),
+ max_episode_steps=100,
+)
+registration.register(
+ id="BlockReach-v0",
+ entry_point=BlockPush,
+ kwargs=dict(task=BlockTaskVariant.REACH),
+ max_episode_steps=50,
+)
+registration.register(
+ id="BlockReachRgb-v0",
+ entry_point=BlockPush,
+ max_episode_steps=100,
+ kwargs=dict(task=BlockTaskVariant.REACH, image_size=(IMAGE_HEIGHT, IMAGE_WIDTH)),
+)
+registration.register(
+ id="BlockReachNormalized-v0",
+ entry_point=BlockPushNormalized,
+ kwargs=dict(task=BlockTaskVariant.REACH_NORMALIZED),
+ max_episode_steps=50,
+)
+registration.register(
+ id="BlockReachRgbNormalized-v0",
+ entry_point=BlockPushNormalized,
+ kwargs=dict(
+ task=BlockTaskVariant.REACH_NORMALIZED, image_size=(IMAGE_HEIGHT, IMAGE_WIDTH)
+ ),
+ max_episode_steps=50,
+)
+
+registration.register(
+ id="SharedBlockInsert-v0",
+ entry_point=BlockPush,
+ kwargs=dict(task=BlockTaskVariant.INSERT, shared_memory=True),
+ max_episode_steps=50,
+)
+registration.register(
+ id="SharedBlockPush-v0",
+ entry_point=BlockPush,
+ kwargs=dict(shared_memory=True),
+ max_episode_steps=100,
+)
+registration.register(
+ id="SharedBlockPushNormalized-v0",
+ entry_point=BlockPushNormalized,
+ kwargs=dict(task=BlockTaskVariant.PUSH_NORMALIZED, shared_memory=True),
+ max_episode_steps=100,
+)
+registration.register(
+ id="SharedBlockReach-v0",
+ entry_point=BlockPush,
+ kwargs=dict(task=BlockTaskVariant.REACH, shared_memory=True),
+ max_episode_steps=50,
+)
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_discontinuous.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_discontinuous.py
new file mode 100644
index 0000000000000000000000000000000000000000..06e9dc78ff5ffba1efb0e43b3640fa8a31111fd1
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_discontinuous.py
@@ -0,0 +1,338 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Discontinuous block pushing."""
+import collections
+import enum
+import math
+from typing import List, Optional
+
+from gym import spaces
+from gym.envs import registration
+from diffusion_policy.env.block_pushing import block_pushing
+from diffusion_policy.env.block_pushing.utils import utils_pybullet
+from diffusion_policy.env.block_pushing.utils.pose3d import Pose3d
+import numpy as np
+from scipy.spatial import transform
+import pybullet
+import pybullet_utils.bullet_client as bullet_client
+
+ZONE2_URDF_PATH = "third_party/py/envs/assets/zone2.urdf"
+
+MIN_TARGET_DIST = 0.15
+NUM_RESET_ATTEMPTS = 1000
+
+
+def build_env_name(task, shared_memory, use_image_obs):
+ """Construct the env name from parameters."""
+ del task
+ env_name = "BlockPushDiscontinuous"
+
+ if use_image_obs:
+ env_name = env_name + "Rgb"
+
+ if shared_memory:
+ env_name = "Shared" + env_name
+
+ env_name = env_name + "-v0"
+
+ return env_name
+
+
+class BlockTaskVariant(enum.Enum):
+ REACH = "Reach"
+ REACH_NORMALIZED = "ReachNormalized"
+ PUSH = "Push"
+ PUSH_NORMALIZED = "PushNormalized"
+ INSERT = "Insert"
+
+
+# pytype: skip-file
+class BlockPushDiscontinuous(block_pushing.BlockPush):
+ """Discontinuous block pushing."""
+
+ def __init__(
+ self,
+ control_frequency=10.0,
+ task=BlockTaskVariant.PUSH,
+ image_size=None,
+ shared_memory=False,
+ seed=None,
+ goal_dist_tolerance=0.04,
+ ):
+ super(BlockPushDiscontinuous, self).__init__(
+ control_frequency=control_frequency,
+ task=task,
+ image_size=image_size,
+ shared_memory=shared_memory,
+ seed=seed,
+ goal_dist_tolerance=goal_dist_tolerance,
+ )
+
+ @property
+ def target_poses(self):
+ return self._target_poses
+
+ def get_goal_translation(self):
+ """Return the translation component of the goal (2D)."""
+ if self._target_poses:
+ return [i.translation for i in self._target_poses]
+ else:
+ return None
+
+ def _setup_pybullet_scene(self):
+ self._pybullet_client = bullet_client.BulletClient(self._connection_mode)
+
+ # Temporarily disable rendering to speed up loading URDFs.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
+
+ self._setup_workspace_and_robot()
+ target_urdf_path = block_pushing.ZONE_URDF_PATH
+
+ self._target_ids = []
+ for _ in [block_pushing.ZONE_URDF_PATH, ZONE2_URDF_PATH]:
+ self._target_ids.append(
+ utils_pybullet.load_urdf(
+ self._pybullet_client, target_urdf_path, useFixedBase=True
+ )
+ )
+ self._block_ids = [
+ utils_pybullet.load_urdf(
+ self._pybullet_client, block_pushing.BLOCK_URDF_PATH, useFixedBase=False
+ )
+ ]
+
+ # Re-enable rendering.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
+
+ self.step_simulation_to_stabilize()
+
+ def _reset_target_poses(self, workspace_center_x):
+ """Resets target poses."""
+ self._target_poses = [None for _ in range(len(self._target_ids))]
+
+ def _reset_target_pose(idx, avoid=None):
+ def _get_random_translation():
+ # Choose x,y randomly.
+ target_x = workspace_center_x + self._rng.uniform(low=-0.10, high=0.10)
+ # Fix ys for this environment.
+ if idx == 0:
+ target_y = 0
+ else:
+ target_y = 0.4
+ target_translation = np.array([target_x, target_y, 0.020])
+ return target_translation
+
+ if avoid is None:
+ target_translation = _get_random_translation()
+ else:
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ target_translation = _get_random_translation()
+ dist = np.linalg.norm(target_translation[0] - avoid[0])
+ if dist > MIN_TARGET_DIST:
+ break
+ target_sampled_angle = math.pi + self._rng.uniform(
+ low=-math.pi / 6, high=math.pi / 6
+ )
+ target_rotation = transform.Rotation.from_rotvec(
+ [0, 0, target_sampled_angle]
+ )
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._target_ids[idx],
+ target_translation.tolist(),
+ target_rotation.as_quat().tolist(),
+ )
+ self._target_poses[idx] = Pose3d(
+ rotation=target_rotation, translation=target_translation
+ )
+
+ try_idx = 0
+ while True:
+ # Choose the first target.
+ _reset_target_pose(0)
+ # Choose the second target, avoiding the first.
+ _reset_target_pose(1, avoid=self._target_poses[0].translation)
+ dist = np.linalg.norm(
+ self._target_poses[0].translation[0]
+ - self._target_poses[1].translation[0]
+ )
+ if dist > MIN_TARGET_DIST:
+ break
+ try_idx += 1
+ if try_idx >= NUM_RESET_ATTEMPTS:
+ raise ValueError("could not find matching target")
+ assert dist > MIN_TARGET_DIST
+
+ def reset(self):
+ self._pybullet_client.restoreState(self._saved_state)
+
+ rotation = transform.Rotation.from_rotvec([0, math.pi, 0])
+ translation = np.array([0.3, -0.4, block_pushing.EFFECTOR_HEIGHT])
+ starting_pose = Pose3d(rotation=rotation, translation=translation)
+ self._set_robot_target_effector_pose(starting_pose)
+
+ workspace_center_x = 0.4
+
+ # Reset block pose.
+ block_x = workspace_center_x + self._rng.uniform(low=-0.1, high=0.1)
+ block_y = -0.2 + self._rng.uniform(low=-0.15, high=0.15)
+ block_translation = np.array([block_x, block_y, 0])
+ block_sampled_angle = self._rng.uniform(math.pi)
+ block_rotation = transform.Rotation.from_rotvec([0, 0, block_sampled_angle])
+
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._block_ids[0],
+ block_translation.tolist(),
+ block_rotation.as_quat().tolist(),
+ )
+
+ # Reset target pose.
+ self._reset_target_poses(workspace_center_x)
+
+ self.step_simulation_to_stabilize()
+ state = self._compute_state()
+ self._previous_state = state
+ self.min_dist_to_first_goal = np.inf
+ self.min_dist_to_second_goal = np.inf
+ self.steps = 0
+ return state
+
+ def _compute_goal_distance(self, state):
+ # Reward is 1. blocks is inside any target.
+ return np.mean([self.min_dist_to_first_goal, self.min_dist_to_second_goal])
+
+ def _compute_state(self):
+ effector_pose = self._robot.forward_kinematics()
+ block_position_and_orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self._block_ids[0])
+ )
+ block_pose = Pose3d(
+ rotation=transform.Rotation.from_quat(block_position_and_orientation[1]),
+ translation=block_position_and_orientation[0],
+ )
+
+ def _yaw_from_pose(pose):
+ return np.array([pose.rotation.as_euler("xyz", degrees=False)[-1]])
+
+ obs = collections.OrderedDict(
+ block_translation=block_pose.translation[0:2],
+ block_orientation=_yaw_from_pose(block_pose),
+ effector_translation=effector_pose.translation[0:2],
+ effector_target_translation=self._target_effector_pose.translation[0:2],
+ target_translation=self._target_poses[0].translation[0:2],
+ target_orientation=_yaw_from_pose(self._target_poses[0]),
+ target2_translation=self._target_poses[1].translation[0:2],
+ target2_orientation=_yaw_from_pose(self._target_poses[1]),
+ )
+ if self._image_size is not None:
+ obs["rgb"] = self._render_camera(self._image_size)
+ return obs
+
+ def step(self, action):
+ self._step_robot_and_sim(action)
+ state = self._compute_state()
+ reward = self._get_reward(state)
+ done = False
+ if reward > 0.0:
+ done = True
+ # Cache so we can compute success.
+ self.state = state
+ return state, reward, done, {}
+
+ def dist(self, state, target):
+ # Reward is 1. blocks is inside any target.
+ return np.linalg.norm(
+ state["block_translation"] - state["%s_translation" % target]
+ )
+
+ def _get_reward(self, state):
+ """Reward is 1.0 if agent hits both goals and stays at second."""
+ # This also statefully updates these values.
+ self.min_dist_to_first_goal = min(
+ self.dist(state, "target"), self.min_dist_to_first_goal
+ )
+ self.min_dist_to_second_goal = min(
+ self.dist(state, "target2"), self.min_dist_to_second_goal
+ )
+
+ def _reward(thresh):
+ reward_first = True if self.min_dist_to_first_goal < thresh else False
+ reward_second = True if self.min_dist_to_second_goal < thresh else False
+ return 1.0 if (reward_first and reward_second) else 0.0
+
+ reward = _reward(self.goal_dist_tolerance)
+ return reward
+
+ @property
+ def succeeded(self):
+ thresh = self.goal_dist_tolerance
+ hit_first = True if self.min_dist_to_first_goal < thresh else False
+ hit_second = True if self.min_dist_to_first_goal < thresh else False
+ current_distance_to_second = self.dist(self.state, "target2")
+ still_at_second = True if current_distance_to_second < thresh else False
+ return hit_first and hit_second and still_at_second
+
+ def _create_observation_space(self, image_size):
+ pi2 = math.pi * 2
+
+ obs_dict = collections.OrderedDict(
+ block_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ block_orientation=spaces.Box(low=-pi2, high=pi2, shape=(1,)), # phi
+ effector_translation=spaces.Box(
+ # Small buffer for to IK noise.
+ low=block_pushing.WORKSPACE_BOUNDS[0] - 0.1,
+ high=block_pushing.WORKSPACE_BOUNDS[1] + 0.1,
+ ), # x,y
+ effector_target_translation=spaces.Box(
+ # Small buffer for to IK noise.
+ low=block_pushing.WORKSPACE_BOUNDS[0] - 0.1,
+ high=block_pushing.WORKSPACE_BOUNDS[1] + 0.1,
+ ), # x,y
+ target_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ target_orientation=spaces.Box(
+ low=-pi2,
+ high=pi2,
+ shape=(1,),
+ ), # theta
+ target2_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ target2_orientation=spaces.Box(
+ low=-pi2,
+ high=pi2,
+ shape=(1,),
+ ), # theta
+ )
+ if image_size is not None:
+ obs_dict["rgb"] = spaces.Box(
+ low=0, high=255, shape=(image_size[0], image_size[1], 3), dtype=np.uint8
+ )
+ return spaces.Dict(obs_dict)
+
+
+if "BlockPushDiscontinuous-v0" in registration.registry.env_specs:
+ del registration.registry.env_specs["BlockPushDiscontinuous-v0"]
+
+registration.register(
+ id="BlockPushDiscontinuous-v0",
+ entry_point=BlockPushDiscontinuous,
+ max_episode_steps=200,
+)
+
+registration.register(
+ id="BlockPushDiscontinuousRgb-v0",
+ entry_point=BlockPushDiscontinuous,
+ max_episode_steps=200,
+ kwargs=dict(image_size=(block_pushing.IMAGE_HEIGHT, block_pushing.IMAGE_WIDTH)),
+)
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_multimodal.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_multimodal.py
new file mode 100644
index 0000000000000000000000000000000000000000..6ec494289ecf1b989f04615c06f6f498c33f52c3
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/block_pushing_multimodal.py
@@ -0,0 +1,802 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Multimodal block environments for the XArm."""
+
+import collections
+import logging
+import math
+from typing import Dict, List, Optional, Union
+import copy
+import time
+
+from gym import spaces
+from gym.envs import registration
+from diffusion_policy.env.block_pushing import block_pushing
+from diffusion_policy.env.block_pushing.utils import utils_pybullet
+from diffusion_policy.env.block_pushing.utils.pose3d import Pose3d
+from diffusion_policy.env.block_pushing.utils.utils_pybullet import ObjState
+from diffusion_policy.env.block_pushing.utils.utils_pybullet import XarmState
+import numpy as np
+from scipy.spatial import transform
+import pybullet
+import pybullet_utils.bullet_client as bullet_client
+
+# pytype: skip-file
+BLOCK2_URDF_PATH = "third_party/py/envs/assets/block2.urdf"
+ZONE2_URDF_PATH = "third_party/py/envs/assets/zone2.urdf"
+
+# When resetting multiple targets, they should all be this far apart.
+MIN_BLOCK_DIST = 0.1
+MIN_TARGET_DIST = 0.12
+# pylint: enable=line-too-long
+NUM_RESET_ATTEMPTS = 1000
+
+# Random movement of blocks
+RANDOM_X_SHIFT = 0.1
+RANDOM_Y_SHIFT = 0.15
+
+logging.basicConfig(
+ level="INFO",
+ format="%(asctime)s [%(levelname)s] %(message)s",
+ filemode="w",
+)
+logger = logging.getLogger()
+
+
+def build_env_name(task, shared_memory, use_image_obs):
+ """Construct the env name from parameters."""
+ del task
+ env_name = "BlockPushMultimodal"
+
+ if use_image_obs:
+ env_name = env_name + "Rgb"
+
+ if shared_memory:
+ env_name = "Shared" + env_name
+
+ env_name = env_name + "-v0"
+
+ return env_name
+
+
+class BlockPushEventManager:
+ def __init__(self):
+ self.event_steps = {
+ 'REACH_0': -1,
+ 'REACH_1': -1,
+ 'TARGET_0_0': -1,
+ 'TARGET_0_1': -1,
+ 'TARGET_1_0': -1,
+ 'TARGET_1_1': -1
+ }
+
+ def reach(self, step, block_id):
+ key = f'REACH_{block_id}'
+ if self.event_steps[key] < 0:
+ self.event_steps[key] = step
+
+ def target(self, step, block_id, target_id):
+ key = f'TARGET_{block_id}_{target_id}'
+ if self.event_steps[key] < 0:
+ self.event_steps[key] = step
+
+ def reset(self):
+ for key in list(self.event_steps):
+ self.event_steps[key] = -1
+
+ def get_info(self):
+ return copy.deepcopy(self.event_steps)
+
+class BlockPushMultimodal(block_pushing.BlockPush):
+ """2 blocks, 2 targets."""
+
+ def __init__(
+ self,
+ control_frequency=10.0,
+ task=block_pushing.BlockTaskVariant.PUSH,
+ image_size=None,
+ shared_memory=False,
+ seed=None,
+ goal_dist_tolerance=0.05,
+ abs_action=False
+ ):
+ """Creates an env instance.
+
+ Args:
+ control_frequency: Control frequency for the arm. Each env step will
+ advance the simulation by 1/control_frequency seconds.
+ task: enum for which task, see BlockTaskVariant enum.
+ image_size: Optional image size (height, width). If None, no image
+ observations will be used.
+ shared_memory: If True `pybullet.SHARED_MEMORY` is used to connect to
+ pybullet. Useful to debug.
+ seed: Optional seed for the environment.
+ goal_dist_tolerance: float, how far away from the goal to terminate.
+ """
+ self._target_ids = None
+ self._target_poses = None
+ self._event_manager = BlockPushEventManager()
+ super(BlockPushMultimodal, self).__init__(
+ control_frequency=control_frequency,
+ task=task,
+ image_size=image_size,
+ shared_memory=shared_memory,
+ seed=seed,
+ goal_dist_tolerance=goal_dist_tolerance,
+ )
+ self._init_distance = [-1.0, -1.0]
+ self._in_target = [[-1.0, -1.0], [-1.0, -1.0]]
+ self._first_move = [-1, -1]
+ self._step_num = 0
+ self._abs_action = abs_action
+
+ @property
+ def target_poses(self):
+ return self._target_poses
+
+ def get_goal_translation(self):
+ """Return the translation component of the goal (2D)."""
+ if self._target_poses:
+ return [i.translation for i in self._target_poses]
+ else:
+ return None
+
+ def _setup_pybullet_scene(self):
+ self._pybullet_client = bullet_client.BulletClient(self._connection_mode)
+
+ # Temporarily disable rendering to speed up loading URDFs.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
+
+ self._setup_workspace_and_robot()
+
+ self._target_ids = [
+ utils_pybullet.load_urdf(self._pybullet_client, i, useFixedBase=True)
+ for i in [block_pushing.ZONE_URDF_PATH, ZONE2_URDF_PATH]
+ ]
+ self._block_ids = []
+ for i in [block_pushing.BLOCK_URDF_PATH, BLOCK2_URDF_PATH]:
+ self._block_ids.append(
+ utils_pybullet.load_urdf(self._pybullet_client, i, useFixedBase=False)
+ )
+
+ # Re-enable rendering.
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
+
+ self.step_simulation_to_stabilize()
+
+ def _reset_block_poses(self, workspace_center_x):
+ """Resets block poses."""
+
+ # Helper for choosing random block position.
+ def _reset_block_pose(idx, add=0.0, avoid=None):
+ def _get_random_translation():
+ block_x = (
+ workspace_center_x
+ + add
+ + self._rng.uniform(low=-RANDOM_X_SHIFT, high=RANDOM_X_SHIFT)
+ )
+ block_y = -0.2 + self._rng.uniform(
+ low=-RANDOM_Y_SHIFT, high=RANDOM_Y_SHIFT
+ )
+ block_translation = np.array([block_x, block_y, 0])
+ return block_translation
+
+ if avoid is None:
+ block_translation = _get_random_translation()
+ else:
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ block_translation = _get_random_translation()
+ dist = np.linalg.norm(block_translation[0] - avoid[0])
+ # print('block inner try_idx %d, dist %.3f' % (try_idx, dist))
+ if dist > MIN_BLOCK_DIST:
+ break
+ block_sampled_angle = self._rng.uniform(math.pi)
+ block_rotation = transform.Rotation.from_rotvec([0, 0, block_sampled_angle])
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._block_ids[idx],
+ block_translation.tolist(),
+ block_rotation.as_quat().tolist(),
+ )
+ return block_translation
+
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ # Reset first block.
+ b0_translation = _reset_block_pose(0)
+ # Reset second block away from first block.
+ b1_translation = _reset_block_pose(1, avoid=b0_translation)
+ dist = np.linalg.norm(b0_translation[0] - b1_translation[0])
+ if dist > MIN_BLOCK_DIST:
+ break
+ else:
+ raise ValueError("could not find matching block")
+ assert dist > MIN_BLOCK_DIST
+
+ def _reset_target_poses(self, workspace_center_x):
+ """Resets target poses."""
+
+ def _reset_target_pose(idx, add=0.0, avoid=None):
+ def _get_random_translation():
+ # Choose x,y randomly.
+ target_x = (
+ workspace_center_x
+ + add
+ + self._rng.uniform(
+ low=-0.05 * RANDOM_X_SHIFT, high=0.05 * RANDOM_X_SHIFT
+ )
+ )
+ target_y = 0.2 + self._rng.uniform(
+ low=-0.05 * RANDOM_Y_SHIFT, high=0.05 * RANDOM_Y_SHIFT
+ )
+ target_translation = np.array([target_x, target_y, 0.020])
+ return target_translation
+
+ if avoid is None:
+ target_translation = _get_random_translation()
+ else:
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ target_translation = _get_random_translation()
+ dist = np.linalg.norm(target_translation[0] - avoid[0])
+ # print('target inner try_idx %d, dist %.3f' % (try_idx, dist))
+ if dist > MIN_TARGET_DIST:
+ break
+ target_sampled_angle = math.pi + self._rng.uniform(
+ low=-math.pi / 30, high=math.pi / 30
+ )
+ target_rotation = transform.Rotation.from_rotvec(
+ [0, 0, target_sampled_angle]
+ )
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._target_ids[idx],
+ target_translation.tolist(),
+ target_rotation.as_quat().tolist(),
+ )
+ self._target_poses[idx] = Pose3d(
+ rotation=target_rotation, translation=target_translation
+ )
+
+ if self._target_poses is None:
+ self._target_poses = [None for _ in range(len(self._target_ids))]
+
+ for _ in range(NUM_RESET_ATTEMPTS):
+ # Choose the first target.
+ add = 0.12 * self._rng.choice([-1, 1])
+ # Randomly flip the location of the targets.
+ _reset_target_pose(0, add=add)
+ _reset_target_pose(1, add=-add, avoid=self._target_poses[0].translation)
+ dist = np.linalg.norm(
+ self._target_poses[0].translation[0]
+ - self._target_poses[1].translation[0]
+ )
+ if dist > MIN_TARGET_DIST:
+ break
+ else:
+ raise ValueError("could not find matching target")
+ assert dist > MIN_TARGET_DIST
+
+ def _reset_object_poses(self, workspace_center_x, workspace_center_y):
+ # Reset block poses.
+ self._reset_block_poses(workspace_center_x)
+
+ # Reset target poses.
+ self._reset_target_poses(workspace_center_x)
+
+ self._init_distance = [-1.0, -1.0]
+ self._in_target = [[-1.0, -1.0], [-1.0, -1.0]]
+ self._step_num = 0
+
+ def reset(self, reset_poses=True):
+ workspace_center_x = 0.4
+ workspace_center_y = 0.0
+
+ if reset_poses:
+ self._pybullet_client.restoreState(self._saved_state)
+
+ rotation = transform.Rotation.from_rotvec([0, math.pi, 0])
+ translation = np.array([0.3, -0.4, block_pushing.EFFECTOR_HEIGHT])
+ starting_pose = Pose3d(rotation=rotation, translation=translation)
+ self._set_robot_target_effector_pose(starting_pose)
+ self._reset_object_poses(workspace_center_x, workspace_center_y)
+
+ # else:
+ self._target_poses = [
+ self._get_target_pose(idx) for idx in self._target_ids
+ ]
+
+ if reset_poses:
+ self.step_simulation_to_stabilize()
+
+ state = self._compute_state()
+ self._previous_state = state
+ self._event_manager.reset()
+ return state
+
+ def _get_target_pose(self, idx):
+ (
+ target_translation,
+ target_orientation_quat,
+ ) = self._pybullet_client.getBasePositionAndOrientation(idx)
+ target_rotation = transform.Rotation.from_quat(target_orientation_quat)
+ target_translation = np.array(target_translation)
+ return Pose3d(rotation=target_rotation, translation=target_translation)
+
+ def _compute_reach_target(self, state):
+ xy_block = state["block_translation"]
+ xy_target = state["target_translation"]
+
+ xy_block_to_target = xy_target - xy_block
+ xy_dir_block_to_target = (xy_block_to_target) / np.linalg.norm(
+ xy_block_to_target
+ )
+ self.reach_target_translation = xy_block + -1 * xy_dir_block_to_target * 0.05
+
+ def _compute_state(self):
+ effector_pose = self._robot.forward_kinematics()
+
+ def _get_block_pose(idx):
+ block_position_and_orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._block_ids[idx]
+ )
+ )
+ block_pose = Pose3d(
+ rotation=transform.Rotation.from_quat(
+ block_position_and_orientation[1]
+ ),
+ translation=block_position_and_orientation[0],
+ )
+ return block_pose
+
+ block_poses = [_get_block_pose(i) for i in range(len(self._block_ids))]
+
+ def _yaw_from_pose(pose):
+ return np.array([pose.rotation.as_euler("xyz", degrees=False)[-1] % np.pi])
+
+ obs = collections.OrderedDict(
+ block_translation=block_poses[0].translation[0:2],
+ block_orientation=_yaw_from_pose(block_poses[0]),
+ block2_translation=block_poses[1].translation[0:2],
+ block2_orientation=_yaw_from_pose(block_poses[1]),
+ effector_translation=effector_pose.translation[0:2],
+ effector_target_translation=self._target_effector_pose.translation[0:2],
+ target_translation=self._target_poses[0].translation[0:2],
+ target_orientation=_yaw_from_pose(self._target_poses[0]),
+ target2_translation=self._target_poses[1].translation[0:2],
+ target2_orientation=_yaw_from_pose(self._target_poses[1]),
+ )
+
+ for i in range(2):
+ new_distance = np.linalg.norm(
+ block_poses[i].translation[0:2]
+ ) # + np.linalg.norm(_yaw_from_pose(block_poses[i]))
+ if self._init_distance[i] == -1:
+ self._init_distance[i] = new_distance
+ else:
+ if self._init_distance[i] != 100:
+ if np.abs(new_distance - self._init_distance[i]) > 1e-3:
+ logger.info(f"Block {i} moved on step {self._step_num}")
+ self._event_manager.reach(step=self._step_num, block_id=i)
+ self._init_distance[i] = 100
+
+ self._step_num += 1
+ if self._image_size is not None:
+ obs["rgb"] = self._render_camera(self._image_size)
+ return obs
+
+ def step(self, action):
+ self._step_robot_and_sim(action)
+
+ state = self._compute_state()
+ done = False
+ reward = self._get_reward(state)
+ if reward >= 0.5:
+ # Terminate the episode if both blocks are close enough to the targets.
+ done = True
+
+ info = self._event_manager.get_info()
+ return state, reward, done, info
+
+ def _step_robot_and_sim(self, action):
+ """Steps the robot and pybullet sim."""
+ # Compute target_effector_pose by shifting the effector's pose by the
+ # action.
+ if self._abs_action:
+ target_effector_translation = np.array([action[0], action[1], 0])
+ else:
+ target_effector_translation = np.array(
+ self._target_effector_pose.translation
+ ) + np.array([action[0], action[1], 0])
+
+ target_effector_translation[0:2] = np.clip(
+ target_effector_translation[0:2],
+ self.workspace_bounds[0],
+ self.workspace_bounds[1],
+ )
+ target_effector_translation[-1] = self.effector_height
+ target_effector_pose = Pose3d(
+ rotation=block_pushing.EFFECTOR_DOWN_ROTATION, translation=target_effector_translation
+ )
+
+ self._set_robot_target_effector_pose(target_effector_pose)
+
+ # Update sleep time dynamically to stay near real-time.
+ frame_sleep_time = 0
+ if self._connection_mode == pybullet.SHARED_MEMORY:
+ cur_time = time.time()
+ if self._last_loop_time is not None:
+ # Calculate the total, non-sleeping time from the previous frame, this
+ # includes the actual step as well as any compute that happens in the
+ # caller thread (model inference, etc).
+ compute_time = (
+ cur_time
+ - self._last_loop_time
+ - self._last_loop_frame_sleep_time * self._sim_steps_per_step
+ )
+ # Use this to calculate the current frame's total sleep time to ensure
+ # that env.step runs at policy rate. This is an estimate since the
+ # previous frame's compute time may not match the current frame.
+ total_sleep_time = max((1 / self._control_frequency) - compute_time, 0)
+ # Now spread this out over the inner sim steps. This doesn't change
+ # control in any way, but makes the animation appear smooth.
+ frame_sleep_time = total_sleep_time / self._sim_steps_per_step
+ else:
+ # No estimate of the previous frame's compute, assume it is zero.
+ frame_sleep_time = 1 / self._step_frequency
+
+ # Cache end of this loop time, to compute sleep time on next iteration.
+ self._last_loop_time = cur_time
+ self._last_loop_frame_sleep_time = frame_sleep_time
+
+ for _ in range(self._sim_steps_per_step):
+ if self._connection_mode == pybullet.SHARED_MEMORY:
+ block_pushing.sleep_spin(frame_sleep_time)
+ self._pybullet_client.stepSimulation()
+
+ def _get_reward(self, state):
+ # Reward is 1. if both blocks are inside targets, but not the same target.
+ targets = ["target", "target2"]
+
+ def _block_target_dist(block, target):
+ return np.linalg.norm(
+ state["%s_translation" % block] - state["%s_translation" % target]
+ )
+
+ def _closest_target(block):
+ # Distances to all targets.
+ dists = [_block_target_dist(block, t) for t in targets]
+ # Which is closest.
+ closest_target = targets[np.argmin(dists)]
+ closest_dist = np.min(dists)
+ # Is it in the closest target?
+ in_target = closest_dist < self.goal_dist_tolerance
+ return closest_target, in_target
+
+ blocks = ["block", "block2"]
+
+ reward = 0.0
+
+ for t_i, t in enumerate(targets):
+ for b_i, b in enumerate(blocks):
+ if self._in_target[t_i][b_i] == -1:
+ dist = _block_target_dist(b, t)
+ if dist < self.goal_dist_tolerance:
+ self._in_target[t_i][b_i] = 0
+ logger.info(
+ f"Block {b_i} entered target {t_i} on step {self._step_num}"
+ )
+ self._event_manager.target(step=self._step_num, block_id=b_i, target_id=t_i)
+ reward += 0.49
+
+ b0_closest_target, b0_in_target = _closest_target("block")
+ b1_closest_target, b1_in_target = _closest_target("block2")
+ # reward = 0.0
+ if b0_in_target and b1_in_target and (b0_closest_target != b1_closest_target):
+ reward = 0.51
+ return reward
+
+ def _compute_goal_distance(self, state):
+ blocks = ["block", "block2"]
+
+ def _target_block_dist(target, block):
+ return np.linalg.norm(
+ state["%s_translation" % block] - state["%s_translation" % target]
+ )
+
+ def _closest_block_dist(target):
+ dists = [_target_block_dist(target, b) for b in blocks]
+ closest_dist = np.min(dists)
+ return closest_dist
+
+ t0_closest_dist = _closest_block_dist("target")
+ t1_closest_dist = _closest_block_dist("target2")
+ return np.mean([t0_closest_dist, t1_closest_dist])
+
+ @property
+ def succeeded(self):
+ state = self._compute_state()
+ reward = self._get_reward(state)
+ if reward >= 0.5:
+ return True
+ return False
+
+ def _create_observation_space(self, image_size):
+ pi2 = math.pi * 2
+
+ obs_dict = collections.OrderedDict(
+ block_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ block_orientation=spaces.Box(low=-pi2, high=pi2, shape=(1,)), # phi
+ block2_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ block2_orientation=spaces.Box(low=-pi2, high=pi2, shape=(1,)), # phi
+ effector_translation=spaces.Box(
+ low=block_pushing.WORKSPACE_BOUNDS[0] - 0.1,
+ high=block_pushing.WORKSPACE_BOUNDS[1] + 0.1,
+ ), # x,y
+ effector_target_translation=spaces.Box(
+ low=block_pushing.WORKSPACE_BOUNDS[0] - 0.1,
+ high=block_pushing.WORKSPACE_BOUNDS[1] + 0.1,
+ ), # x,y
+ target_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ target_orientation=spaces.Box(
+ low=-pi2,
+ high=pi2,
+ shape=(1,),
+ ), # theta
+ target2_translation=spaces.Box(low=-5, high=5, shape=(2,)), # x,y
+ target2_orientation=spaces.Box(
+ low=-pi2,
+ high=pi2,
+ shape=(1,),
+ ), # theta
+ )
+ if image_size is not None:
+ obs_dict["rgb"] = spaces.Box(
+ low=0, high=255, shape=(image_size[0], image_size[1], 3), dtype=np.uint8
+ )
+ return spaces.Dict(obs_dict)
+
+ def get_pybullet_state(self):
+ """Save pybullet state of the scene.
+
+ Returns:
+ dict containing 'robots', 'robot_end_effectors', 'targets', 'objects',
+ each containing a list of ObjState.
+ """
+ state: Dict[str, List[ObjState]] = {}
+
+ state["robots"] = [
+ XarmState.get_bullet_state(
+ self._pybullet_client,
+ self.robot.xarm,
+ target_effector_pose=self._target_effector_pose,
+ goal_translation=None,
+ )
+ ]
+
+ state["robot_end_effectors"] = []
+ if self.robot.end_effector:
+ state["robot_end_effectors"].append(
+ ObjState.get_bullet_state(
+ self._pybullet_client, self.robot.end_effector
+ )
+ )
+
+ state["targets"] = []
+ if self._target_ids:
+ for target_id in self._target_ids:
+ state["targets"].append(
+ ObjState.get_bullet_state(self._pybullet_client, target_id)
+ )
+
+ state["objects"] = []
+ for obj_id in self.get_obj_ids():
+ state["objects"].append(
+ ObjState.get_bullet_state(self._pybullet_client, obj_id)
+ )
+
+ return state
+
+ def set_pybullet_state(self, state):
+ """Restore pyullet state.
+
+ WARNING: py_environment wrapper assumes environments aren't reset in their
+ constructor and will often reset the environment unintentionally. It is
+ always recommended that you call env.reset on the tfagents wrapper before
+ playback (replaying pybullet_state).
+
+ Args:
+ state: dict containing 'robots', 'robot_end_effectors', 'targets',
+ 'objects', each containing a list of ObjState.
+ """
+
+ assert isinstance(state["robots"][0], XarmState)
+ xarm_state: XarmState = state["robots"][0]
+ xarm_state.set_bullet_state(self._pybullet_client, self.robot.xarm)
+ self._set_robot_target_effector_pose(xarm_state.target_effector_pose)
+
+ def _set_state_safe(obj_state, obj_id):
+ if obj_state is not None:
+ assert obj_id is not None, "Cannot set state for missing object."
+ obj_state.set_bullet_state(self._pybullet_client, obj_id)
+ else:
+ assert obj_id is None, f"No state found for obj_id {obj_id}"
+
+ robot_end_effectors = state["robot_end_effectors"]
+ _set_state_safe(
+ None if not robot_end_effectors else robot_end_effectors[0],
+ self.robot.end_effector,
+ )
+
+ for target_state, target_id in zip(state["targets"], self._target_ids):
+ _set_state_safe(target_state, target_id)
+
+ obj_ids = self.get_obj_ids()
+ assert len(state["objects"]) == len(obj_ids), "State length mismatch"
+ for obj_state, obj_id in zip(state["objects"], obj_ids):
+ _set_state_safe(obj_state, obj_id)
+
+ self.reset(reset_poses=False)
+
+
+class BlockPushHorizontalMultimodal(BlockPushMultimodal):
+ def _reset_object_poses(self, workspace_center_x, workspace_center_y):
+ # Reset block poses.
+ self._reset_block_poses(workspace_center_y)
+
+ # Reset target poses.
+ self._reset_target_poses(workspace_center_y)
+
+ def _reset_block_poses(self, workspace_center_y):
+ """Resets block poses."""
+
+ # Helper for choosing random block position.
+ def _reset_block_pose(idx, add=0.0, avoid=None):
+ def _get_random_translation():
+ block_x = 0.35 + 0.5 * self._rng.uniform(
+ low=-RANDOM_X_SHIFT, high=RANDOM_X_SHIFT
+ )
+ block_y = (
+ workspace_center_y
+ + add
+ + 0.5 * self._rng.uniform(low=-RANDOM_Y_SHIFT, high=RANDOM_Y_SHIFT)
+ )
+ block_translation = np.array([block_x, block_y, 0])
+ return block_translation
+
+ if avoid is None:
+ block_translation = _get_random_translation()
+ else:
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ block_translation = _get_random_translation()
+ dist = np.linalg.norm(block_translation[0] - avoid[0])
+ # print('block inner try_idx %d, dist %.3f' % (try_idx, dist))
+ if dist > MIN_BLOCK_DIST:
+ break
+ block_sampled_angle = self._rng.uniform(math.pi)
+ block_rotation = transform.Rotation.from_rotvec([0, 0, block_sampled_angle])
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._block_ids[idx],
+ block_translation.tolist(),
+ block_rotation.as_quat().tolist(),
+ )
+ return block_translation
+
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ # Reset first block.
+ add = 0.2 * self._rng.choice([-1, 1])
+ b0_translation = _reset_block_pose(0, add=add)
+ # Reset second block away from first block.
+ b1_translation = _reset_block_pose(1, add=-add, avoid=b0_translation)
+ dist = np.linalg.norm(b0_translation[0] - b1_translation[0])
+ if dist > MIN_BLOCK_DIST:
+ break
+ else:
+ raise ValueError("could not find matching block")
+ assert dist > MIN_BLOCK_DIST
+
+ def _reset_target_poses(self, workspace_center_y):
+ """Resets target poses."""
+
+ def _reset_target_pose(idx, add=0.0, avoid=None):
+ def _get_random_translation():
+ # Choose x,y randomly.
+ target_x = 0.5 + self._rng.uniform(
+ low=-0.05 * RANDOM_X_SHIFT, high=0.05 * RANDOM_X_SHIFT
+ )
+ target_y = (
+ workspace_center_y
+ + add
+ + self._rng.uniform(
+ low=-0.05 * RANDOM_Y_SHIFT, high=0.05 * RANDOM_Y_SHIFT
+ )
+ )
+ target_translation = np.array([target_x, target_y, 0.020])
+ return target_translation
+
+ if avoid is None:
+ target_translation = _get_random_translation()
+ else:
+ # Reject targets too close to `avoid`.
+ for _ in range(NUM_RESET_ATTEMPTS):
+ target_translation = _get_random_translation()
+ dist = np.linalg.norm(target_translation[0] - avoid[0])
+ # print('target inner try_idx %d, dist %.3f' % (try_idx, dist))
+ if dist > MIN_TARGET_DIST:
+ break
+ target_sampled_angle = math.pi + self._rng.uniform(
+ low=-math.pi / 30, high=math.pi / 30
+ )
+ target_rotation = transform.Rotation.from_rotvec(
+ [0, 0, target_sampled_angle]
+ )
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._target_ids[idx],
+ target_translation.tolist(),
+ target_rotation.as_quat().tolist(),
+ )
+ self._target_poses[idx] = Pose3d(
+ rotation=target_rotation, translation=target_translation
+ )
+
+ if self._target_poses is None:
+ self._target_poses = [None for _ in range(len(self._target_ids))]
+
+ for _ in range(NUM_RESET_ATTEMPTS):
+ # Choose the first target.
+ add = 0.2 * self._rng.choice([-1, 1])
+ # Randomly flip the location of the targets.
+ _reset_target_pose(0, add=add)
+ _reset_target_pose(1, add=-add, avoid=self._target_poses[0].translation)
+ dist = np.linalg.norm(
+ self._target_poses[0].translation[0]
+ - self._target_poses[1].translation[0]
+ )
+ break
+ # if dist > MIN_TARGET_DIST:
+ # break
+ else:
+ raise ValueError("could not find matching target")
+ # assert dist > MIN_TARGET_DIST
+
+
+if "BlockPushMultimodal-v0" in registration.registry.env_specs:
+ del registration.registry.env_specs["BlockPushMultimodal-v0"]
+
+registration.register(
+ id="BlockPushMultimodal-v0", entry_point=BlockPushMultimodal, max_episode_steps=350
+)
+
+registration.register(
+ id="BlockPushMultimodalFlipped-v0",
+ entry_point=BlockPushHorizontalMultimodal,
+ max_episode_steps=25,
+)
+
+registration.register(
+ id="SharedBlockPushMultimodal-v0",
+ entry_point=BlockPushMultimodal,
+ kwargs=dict(shared_memory=True),
+ max_episode_steps=350,
+)
+registration.register(
+ id="BlockPushMultimodalRgb-v0",
+ entry_point=BlockPushMultimodal,
+ max_episode_steps=350,
+ kwargs=dict(image_size=(block_pushing.IMAGE_HEIGHT, block_pushing.IMAGE_WIDTH)),
+)
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/discontinuous_push_oracle.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/discontinuous_push_oracle.py
new file mode 100644
index 0000000000000000000000000000000000000000..2fd074b70deaaa9fb2182c922645583d9ae61931
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/discontinuous_push_oracle.py
@@ -0,0 +1,70 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Pushes to first target, waits, then pushes to second target."""
+
+import diffusion_policy.env.block_pushing.oracles.oriented_push_oracle as oriented_push_oracle_module
+import numpy as np
+from tf_agents.trajectories import policy_step
+from tf_agents.trajectories import time_step as ts
+from tf_agents.typing import types
+
+# Only used for debug visualization.
+import pybullet # pylint: disable=unused-import
+
+
+class DiscontinuousOrientedPushOracle(oriented_push_oracle_module.OrientedPushOracle):
+ """Pushes to first target, waits, then pushes to second target."""
+
+ def __init__(self, env, goal_tolerance=0.04, wait=0):
+ super(DiscontinuousOrientedPushOracle, self).__init__(env)
+ self._countdown = 0
+ self._wait = wait
+ self._goal_dist_tolerance = goal_tolerance
+
+ def reset(self):
+ self.phase = "move_to_pre_block"
+ self._countdown = 0
+
+ def _action(self, time_step, policy_state):
+ if time_step.is_first():
+ self.reset()
+ # Move to first target first.
+ self._current_target = "target"
+ self._has_switched = False
+
+ def _block_target_dist(block, target):
+ dist = np.linalg.norm(
+ time_step.observation["%s_translation" % block]
+ - time_step.observation["%s_translation" % target]
+ )
+ return dist
+
+ d1 = _block_target_dist("block", "target")
+ if d1 < self._goal_dist_tolerance and not self._has_switched:
+ self._countdown = self._wait
+ # If first block has been pushed to first target, switch to second block.
+ self._has_switched = True
+ self._current_target = "target2"
+
+ xy_delta = self._get_action_for_block_target(
+ time_step, block="block", target=self._current_target
+ )
+
+ if self._countdown > 0:
+ xy_delta = np.zeros_like(xy_delta)
+ self._countdown -= 1
+
+ return policy_step.PolicyStep(action=np.asarray(xy_delta, dtype=np.float32))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/multimodal_push_oracle.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/multimodal_push_oracle.py
new file mode 100644
index 0000000000000000000000000000000000000000..29a63866d1249c123466b7f64585185011f1e75e
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/multimodal_push_oracle.py
@@ -0,0 +1,187 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Oracle for multimodal pushing task."""
+import diffusion_policy.env.block_pushing.oracles.oriented_push_oracle as oriented_push_oracle_module
+import numpy as np
+from tf_agents.trajectories import policy_step
+from tf_agents.trajectories import time_step as ts
+from tf_agents.typing import types
+
+# Only used for debug visualization.
+import pybullet # pylint: disable=unused-import
+
+
+class MultimodalOrientedPushOracle(oriented_push_oracle_module.OrientedPushOracle):
+ """Oracle for multimodal pushing task."""
+
+ def __init__(self, env, goal_dist_tolerance=0.04, action_noise_std=0.0):
+ super(MultimodalOrientedPushOracle, self).__init__(env)
+ self._goal_dist_tolerance = goal_dist_tolerance
+ self._action_noise_std = action_noise_std
+
+ def reset(self):
+ self.origin = None
+ self.first_preblock = None
+ self.phase = "move_to_pre_block"
+
+ def _get_move_to_preblock(self, xy_pre_block, xy_ee):
+ max_step_velocity = 0.3
+ # Go 5 cm away from the block, on the line between the block and target.
+ xy_delta_to_preblock = xy_pre_block - xy_ee
+ diff = np.linalg.norm(xy_delta_to_preblock)
+ if diff < 0.001:
+ self.phase = "move_to_block"
+ if self.first_preblock is None:
+ self.first_preblock = np.copy(xy_pre_block)
+ xy_delta = xy_delta_to_preblock
+ return xy_delta, max_step_velocity
+
+ def _get_action_for_block_target(self, time_step, block="block", target="target"):
+ # Specifying this as velocity makes it independent of control frequency.
+ max_step_velocity = 0.35
+
+ info = self._get_action_info(time_step, block, target)
+
+ if self.origin is None:
+ self.origin = np.copy(info.xy_ee)
+
+ if self.phase == "move_to_pre_block":
+ xy_delta, max_step_velocity = self._get_move_to_preblock(
+ info.xy_pre_block, info.xy_ee
+ )
+
+ if self.phase == "return_to_first_preblock":
+ max_step_velocity = 0.3
+ if self.first_preblock is None:
+ self.first_preblock = self.origin
+ # Return to the first preblock.
+ xy_delta_to_origin = self.first_preblock - info.xy_ee
+ diff = np.linalg.norm(xy_delta_to_origin)
+ if diff < 0.001:
+ self.phase = "return_to_origin"
+ xy_delta = xy_delta_to_origin
+
+ if self.phase == "return_to_origin":
+ max_step_velocity = 0.3
+ # Go 5 cm away from the block, on the line between the block and target.
+ xy_delta_to_origin = self.origin - info.xy_ee
+ diff = np.linalg.norm(xy_delta_to_origin)
+ if diff < 0.001:
+ self.phase = "move_to_pre_block"
+ xy_delta = xy_delta_to_origin
+
+ if self.phase == "move_to_block":
+ xy_delta = self._get_move_to_block(
+ info.xy_delta_to_nexttoblock,
+ info.theta_threshold_to_orient,
+ info.theta_error,
+ )
+
+ if self.phase == "push_block":
+ xy_delta = self._get_push_block(
+ info.theta_error,
+ info.theta_threshold_to_orient,
+ info.xy_delta_to_touchingblock,
+ )
+
+ orient_circle_diameter = 0.025
+
+ if self.phase == "orient_block_left" or self.phase == "orient_block_right":
+ max_step_velocity = 0.15
+
+ if self.phase == "orient_block_left":
+ xy_delta = self._get_orient_block_left(
+ info.xy_dir_block_to_ee,
+ orient_circle_diameter,
+ info.xy_block,
+ info.xy_ee,
+ info.theta_error,
+ info.theta_threshold_flat_enough,
+ )
+
+ if self.phase == "orient_block_right":
+ xy_delta = self._get_orient_block_right(
+ info.xy_dir_block_to_ee,
+ orient_circle_diameter,
+ info.xy_block,
+ info.xy_ee,
+ info.theta_error,
+ info.theta_threshold_flat_enough,
+ )
+
+ if self._action_noise_std != 0.0:
+ xy_delta += self._np_random_state.randn(2) * self._action_noise_std
+
+ max_step_distance = max_step_velocity * (1 / self._env.get_control_frequency())
+ length = np.linalg.norm(xy_delta)
+ if length > max_step_distance:
+ xy_direction = xy_delta / length
+ xy_delta = xy_direction * max_step_distance
+ return xy_delta
+
+ def _choose_goal_order(self):
+ """Chooses block->target order for multimodal pushing."""
+ # Define all possible ((first_block, first_target),
+ # (second_block, second_target)).
+ possible_orders = [
+ (("block", "target"), ("block2", "target2")),
+ (("block", "target2"), ("block2", "target")),
+ (("block2", "target"), ("block", "target2")),
+ (("block2", "target2"), ("block", "target")),
+ ]
+ # import pdb; pdb.set_trace()
+ # result = random.choice(possible_orders)
+ result = possible_orders[self._env._rng.choice(len(possible_orders))]
+ return result
+
+ def _action(self, time_step, policy_state):
+ if time_step.is_first():
+ self.reset()
+ (
+ (self._first_block, self._first_target),
+ (self._second_block, self._second_target),
+ ) = self._choose_goal_order()
+ self._current_block, self._current_target = (
+ self._first_block,
+ self._first_target,
+ )
+ self._has_switched = False
+
+ def _block_target_dist(block, target):
+ dist = np.linalg.norm(
+ time_step.observation["%s_translation" % block]
+ - time_step.observation["%s_translation" % target]
+ )
+ return dist
+
+ if (
+ _block_target_dist(self._first_block, self._first_target)
+ < self._goal_dist_tolerance
+ and not self._has_switched
+ ):
+ # If first block has been pushed to first target, switch to second block.
+ self._current_block, self._current_target = (
+ self._second_block,
+ self._second_target,
+ )
+ self._has_switched = True
+ self.phase = "return_to_first_preblock"
+
+ xy_delta = self._get_action_for_block_target(
+ time_step, block=self._current_block, target=self._current_target
+ )
+
+ return policy_step.PolicyStep(action=np.asarray(xy_delta, dtype=np.float32))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/oriented_push_oracle.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/oriented_push_oracle.py
new file mode 100644
index 0000000000000000000000000000000000000000..39c4c6a34fbbd319e7949f96257ae0f96bcc12b0
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/oriented_push_oracle.py
@@ -0,0 +1,258 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Oracle for pushing task which orients the block then pushes it."""
+
+import diffusion_policy.env.block_pushing.oracles.pushing_info as pushing_info_module
+import numpy as np
+from tf_agents.policies import py_policy
+from tf_agents.trajectories import policy_step
+from tf_agents.trajectories import time_step as ts
+from tf_agents.typing import types
+
+# Only used for debug visualization.
+import pybullet # pylint: disable=unused-import
+
+
+class OrientedPushOracle(py_policy.PyPolicy):
+ """Oracle for pushing task which orients the block then pushes it."""
+
+ def __init__(self, env, action_noise_std=0.0):
+ super(OrientedPushOracle, self).__init__(
+ env.time_step_spec(), env.action_spec()
+ )
+ self._env = env
+ self._np_random_state = np.random.RandomState(0)
+ self.phase = "move_to_pre_block"
+ self._action_noise_std = action_noise_std
+
+ def reset(self):
+ self.phase = "move_to_pre_block"
+
+ def get_theta_from_vector(self, vector):
+ return np.arctan2(vector[1], vector[0])
+
+ def theta_to_rotation2d(self, theta):
+ r = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])
+ return r
+
+ def rotate(self, theta, xy_dir_block_to_ee):
+ rot_2d = self.theta_to_rotation2d(theta)
+ return rot_2d @ xy_dir_block_to_ee
+
+ def _get_action_info(self, time_step, block, target):
+ xy_block = time_step.observation["%s_translation" % block][:2]
+ theta_block = time_step.observation["%s_orientation" % block]
+ xy_target = time_step.observation["%s_translation" % target][:2]
+ xy_ee = time_step.observation["effector_target_translation"][:2]
+
+ xy_block_to_target = xy_target - xy_block
+ xy_dir_block_to_target = (xy_block_to_target) / np.linalg.norm(
+ xy_block_to_target
+ )
+ theta_to_target = self.get_theta_from_vector(xy_dir_block_to_target)
+
+ theta_error = theta_to_target - theta_block
+ # Block has 4-way symmetry.
+ while theta_error > np.pi / 4:
+ theta_error -= np.pi / 2.0
+ while theta_error < -np.pi / 4:
+ theta_error += np.pi / 2.0
+
+ xy_pre_block = xy_block + -xy_dir_block_to_target * 0.05
+ xy_nexttoblock = xy_block + -xy_dir_block_to_target * 0.03
+ xy_touchingblock = xy_block + -xy_dir_block_to_target * 0.01
+ xy_delta_to_nexttoblock = xy_nexttoblock - xy_ee
+ xy_delta_to_touchingblock = xy_touchingblock - xy_ee
+
+ xy_block_to_ee = xy_ee - xy_block
+ xy_dir_block_to_ee = xy_block_to_ee / np.linalg.norm(xy_block_to_ee)
+
+ theta_threshold_to_orient = 0.2
+ theta_threshold_flat_enough = 0.03
+ return pushing_info_module.PushingInfo(
+ xy_block=xy_block,
+ xy_ee=xy_ee,
+ xy_pre_block=xy_pre_block,
+ xy_delta_to_nexttoblock=xy_delta_to_nexttoblock,
+ xy_delta_to_touchingblock=xy_delta_to_touchingblock,
+ xy_dir_block_to_ee=xy_dir_block_to_ee,
+ theta_threshold_to_orient=theta_threshold_to_orient,
+ theta_threshold_flat_enough=theta_threshold_flat_enough,
+ theta_error=theta_error,
+ )
+
+ def _get_move_to_preblock(self, xy_pre_block, xy_ee):
+ max_step_velocity = 0.3
+ # Go 5 cm away from the block, on the line between the block and target.
+ xy_delta_to_preblock = xy_pre_block - xy_ee
+ diff = np.linalg.norm(xy_delta_to_preblock)
+ if diff < 0.001:
+ self.phase = "move_to_block"
+ xy_delta = xy_delta_to_preblock
+ return xy_delta, max_step_velocity
+
+ def _get_move_to_block(
+ self, xy_delta_to_nexttoblock, theta_threshold_to_orient, theta_error
+ ):
+ diff = np.linalg.norm(xy_delta_to_nexttoblock)
+ if diff < 0.001:
+ self.phase = "push_block"
+ # If need to re-oorient, then re-orient.
+ if theta_error > theta_threshold_to_orient:
+ self.phase = "orient_block_left"
+ if theta_error < -theta_threshold_to_orient:
+ self.phase = "orient_block_right"
+ # Otherwise, push into the block.
+ xy_delta = xy_delta_to_nexttoblock
+ return xy_delta
+
+ def _get_push_block(
+ self, theta_error, theta_threshold_to_orient, xy_delta_to_touchingblock
+ ):
+ # If need to reorient, go back to move_to_pre_block, move_to_block first.
+ if theta_error > theta_threshold_to_orient:
+ self.phase = "move_to_pre_block"
+ if theta_error < -theta_threshold_to_orient:
+ self.phase = "move_to_pre_block"
+ xy_delta = xy_delta_to_touchingblock
+ return xy_delta
+
+ def _get_orient_block_left(
+ self,
+ xy_dir_block_to_ee,
+ orient_circle_diameter,
+ xy_block,
+ xy_ee,
+ theta_error,
+ theta_threshold_flat_enough,
+ ):
+ xy_dir_block_to_ee = self.rotate(0.2, xy_dir_block_to_ee)
+ xy_block_to_ee = xy_dir_block_to_ee * orient_circle_diameter
+ xy_push_left_spot = xy_block + xy_block_to_ee
+ xy_delta = xy_push_left_spot - xy_ee
+ if theta_error < theta_threshold_flat_enough:
+ self.phase = "move_to_pre_block"
+ return xy_delta
+
+ def _get_orient_block_right(
+ self,
+ xy_dir_block_to_ee,
+ orient_circle_diameter,
+ xy_block,
+ xy_ee,
+ theta_error,
+ theta_threshold_flat_enough,
+ ):
+ xy_dir_block_to_ee = self.rotate(-0.2, xy_dir_block_to_ee)
+ xy_block_to_ee = xy_dir_block_to_ee * orient_circle_diameter
+ xy_push_left_spot = xy_block + xy_block_to_ee
+ xy_delta = xy_push_left_spot - xy_ee
+ if theta_error > -theta_threshold_flat_enough:
+ self.phase = "move_to_pre_block"
+ return xy_delta
+
+ def _get_action_for_block_target(self, time_step, block="block", target="target"):
+ # Specifying this as velocity makes it independent of control frequency.
+ max_step_velocity = 0.35
+ info = self._get_action_info(time_step, block, target)
+
+ if self.phase == "move_to_pre_block":
+ xy_delta, max_step_velocity = self._get_move_to_preblock(
+ info.xy_pre_block, info.xy_ee
+ )
+
+ if self.phase == "move_to_block":
+ xy_delta = self._get_move_to_block(
+ info.xy_delta_to_nexttoblock,
+ info.theta_threshold_to_orient,
+ info.theta_error,
+ )
+
+ if self.phase == "push_block":
+ xy_delta = self._get_push_block(
+ info.theta_error,
+ info.theta_threshold_to_orient,
+ info.xy_delta_to_touchingblock,
+ )
+
+ orient_circle_diameter = 0.025
+
+ if self.phase == "orient_block_left" or self.phase == "orient_block_right":
+ max_step_velocity = 0.15
+
+ if self.phase == "orient_block_left":
+ xy_delta = self._get_orient_block_left(
+ info.xy_dir_block_to_ee,
+ orient_circle_diameter,
+ info.xy_block,
+ info.xy_ee,
+ info.theta_error,
+ info.theta_threshold_flat_enough,
+ )
+
+ if self.phase == "orient_block_right":
+ xy_delta = self._get_orient_block_right(
+ info.xy_dir_block_to_ee,
+ orient_circle_diameter,
+ info.xy_block,
+ info.xy_ee,
+ info.theta_error,
+ info.theta_threshold_flat_enough,
+ )
+
+ if self._action_noise_std != 0.0:
+ xy_delta += self._np_random_state.randn(2) * self._action_noise_std
+
+ max_step_distance = max_step_velocity * (1 / self._env.get_control_frequency())
+ length = np.linalg.norm(xy_delta)
+ if length > max_step_distance:
+ xy_direction = xy_delta / length
+ xy_delta = xy_direction * max_step_distance
+ return xy_delta
+
+ def _action(self, time_step, policy_state):
+ if time_step.is_first():
+ self.reset()
+ xy_delta = self._get_action_for_block_target(
+ time_step, block="block", target="target"
+ )
+ return policy_step.PolicyStep(action=np.asarray(xy_delta, dtype=np.float32))
+
+
+class OrientedPushNormalizedOracle(py_policy.PyPolicy):
+ """Oracle for pushing task which orients the block then pushes it."""
+
+ def __init__(self, env):
+ super(OrientedPushNormalizedOracle, self).__init__(
+ env.time_step_spec(), env.action_spec()
+ )
+ self._oracle = OrientedPushOracle(env)
+ self._env = env
+
+ def reset(self):
+ self._oracle.reset()
+
+ def _action(self, time_step, policy_state):
+ time_step = time_step._asdict()
+ time_step["observation"] = self._env.calc_unnormalized_state(
+ time_step["observation"]
+ )
+ step = self._oracle._action(
+ ts.TimeStep(**time_step), policy_state
+ ) # pylint: disable=protected-access
+ return policy_step.PolicyStep(
+ action=self._env.calc_normalized_action(step.action)
+ )
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/pushing_info.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/pushing_info.py
new file mode 100644
index 0000000000000000000000000000000000000000..0c01cbb0fb6e345d7bc47675d06cfe00a3362f51
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/pushing_info.py
@@ -0,0 +1,35 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Dataclass holding info needed for pushing oracles."""
+import dataclasses
+from typing import Any
+
+
+@dataclasses.dataclass
+class PushingInfo:
+ """Holds onto info necessary for pushing state machine."""
+
+ xy_block: Any = None
+ xy_ee: Any = None
+ xy_pre_block: Any = None
+ xy_delta_to_nexttoblock: Any = None
+ xy_delta_to_touchingblock: Any = None
+ xy_dir_block_to_ee: Any = None
+ theta_threshold_to_orient: Any = None
+ theta_threshold_flat_enough: Any = None
+ theta_error: Any = None
+ obstacle_poses: Any = None
+ distance_to_target: Any = None
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/reach_oracle.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/reach_oracle.py
new file mode 100644
index 0000000000000000000000000000000000000000..8b7746cfddc4eec5694286da4593c64b8ac331f0
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/oracles/reach_oracle.py
@@ -0,0 +1,61 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Reach oracle."""
+import numpy as np
+from tf_agents.policies import py_policy
+from tf_agents.trajectories import policy_step
+from tf_agents.trajectories import time_step as ts
+from tf_agents.typing import types
+
+# Only used for debug visualization.
+import pybullet # pylint: disable=unused-import
+
+
+class ReachOracle(py_policy.PyPolicy):
+ """Oracle for moving to a specific spot relative to the block and target."""
+
+ def __init__(self, env, block_pushing_oracles_action_std=0.0):
+ super(ReachOracle, self).__init__(env.time_step_spec(), env.action_spec())
+ self._env = env
+ self._np_random_state = np.random.RandomState(0)
+ self._block_pushing_oracles_action_std = block_pushing_oracles_action_std
+
+ def _action(self, time_step, policy_state):
+
+ # Specifying this as velocity makes it independent of control frequency.
+ max_step_velocity = 0.2
+
+ xy_ee = time_step.observation["effector_target_translation"]
+
+ # This should be observable from block and target translation,
+ # but re-using the computation from the env so that it's only done once, and
+ # used for reward / completion computation.
+ xy_pre_block = self._env.reach_target_translation
+
+ xy_delta = xy_pre_block - xy_ee
+
+ if self._block_pushing_oracles_action_std != 0.0:
+ xy_delta += (
+ self._np_random_state.randn(2) * self._block_pushing_oracles_action_std
+ )
+
+ max_step_distance = max_step_velocity * (1 / self._env.get_control_frequency())
+ length = np.linalg.norm(xy_delta)
+ if length > max_step_distance:
+ xy_direction = xy_delta / length
+ xy_delta = xy_direction * max_step_distance
+
+ return policy_step.PolicyStep(action=np.asarray(xy_delta, dtype=np.float32))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/pose3d.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/pose3d.py
new file mode 100644
index 0000000000000000000000000000000000000000..a25beeb18b90d0204b36861bce10df44d632c025
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/pose3d.py
@@ -0,0 +1,70 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""A simple 6DOF pose container.
+"""
+
+import dataclasses
+import numpy as np
+from scipy.spatial import transform
+
+
+class NoCopyAsDict(object):
+ """Base class for dataclasses. Avoids a copy in the asdict() call."""
+
+ def asdict(self):
+ """Replacement for dataclasses.asdict.
+
+ TF Dataset does not handle dataclasses.asdict, which uses copy.deepcopy when
+ setting values in the output dict. This causes issues with tf.Dataset.
+ Instead, shallow copy contents.
+
+ Returns:
+ dict containing contents of dataclass.
+ """
+ return {k.name: getattr(self, k.name) for k in dataclasses.fields(self)}
+
+
+@dataclasses.dataclass
+class Pose3d(NoCopyAsDict):
+ """Simple container for translation and rotation."""
+
+ rotation: transform.Rotation
+ translation: np.ndarray
+
+ @property
+ def vec7(self):
+ return np.concatenate([self.translation, self.rotation.as_quat()])
+
+ def serialize(self):
+ return {
+ "rotation": self.rotation.as_quat().tolist(),
+ "translation": self.translation.tolist(),
+ }
+
+ @staticmethod
+ def deserialize(data):
+ return Pose3d(
+ rotation=transform.Rotation.from_quat(data["rotation"]),
+ translation=np.array(data["translation"]),
+ )
+
+ def __eq__(self, other):
+ return np.array_equal(
+ self.rotation.as_quat(), other.rotation.as_quat()
+ ) and np.array_equal(self.translation, other.translation)
+
+ def __ne__(self, other):
+ return not self.__eq__(other)
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/utils_pybullet.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/utils_pybullet.py
new file mode 100644
index 0000000000000000000000000000000000000000..d9a664d7b4879dbc0ff0c8a1071d2699c32c94ad
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/utils_pybullet.py
@@ -0,0 +1,451 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Assortment of utilities to interact with bullet within g3."""
+import dataclasses
+import datetime
+import getpass
+import gzip
+import json
+import os
+import time
+from typing import Any, Dict, List, Optional, Tuple
+
+from absl import logging
+from diffusion_policy.env.block_pushing.utils.pose3d import Pose3d
+import numpy as np
+from scipy.spatial import transform
+import six
+
+
+import pybullet
+import pybullet_data
+import pybullet_utils.bullet_client as bullet_client
+
+Vec3 = Tuple[float, float, float]
+Vec4 = Tuple[float, float, float, float]
+PYBULLET_STATE_VERSION = 2 # Basic versioning of serialized pybullet state.
+
+
+# Note about rotation_to_matrix and matrix_to_rotation below:
+# The abstractions below allow us to use older versions of scipy.
+def rotation_to_matrix(rotation):
+ if hasattr(rotation, "as_dcm"):
+ return rotation.as_dcm()
+ else:
+ assert hasattr(rotation, "as_matrix")
+ return rotation.as_matrix()
+
+
+def matrix_to_rotation(matrix):
+ if hasattr(transform.Rotation, "from_dcm"):
+ return transform.Rotation.from_dcm(matrix)
+ else:
+ assert hasattr(transform.Rotation, "from_matrix")
+ return transform.Rotation.from_matrix(matrix)
+
+
+def load_urdf(pybullet_client, file_path, *args, **kwargs):
+ """Loads the given URDF filepath."""
+
+ # Handles most general file open case.
+ try:
+ if os.path.exists(file_path):
+ return pybullet_client.loadURDF(file_path, *args, **kwargs)
+ except pybullet_client.error:
+ pass
+
+ try:
+ import pathlib
+ asset_path = str(pathlib.Path(__file__).parent.parent.joinpath('assets'))
+ if file_path.startswith("third_party/py/envs/assets/"):
+ pybullet_client.setAdditionalSearchPath(asset_path)
+ file_path = file_path[len("third_party/py/envs/assets/") :]
+ if file_path.startswith(
+ "third_party/bullet/examples/pybullet/gym/pybullet_data/"
+ ):
+ pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
+ file_path = file_path[55:]
+ # logging.info("Loading URDF %s", file_path)
+ return pybullet_client.loadURDF(file_path, *args, **kwargs)
+ except pybullet.error:
+ raise FileNotFoundError("Cannot load the URDF file {}".format(file_path))
+
+
+def add_visual_sphere(client, center=(0, 0, 0), radius=0.1, rgba=(0.5, 0.5, 0.5, 0.5)):
+ """Add a sphere to bullet scene (visual only, no physics).
+
+ Args:
+ client: pybullet client (or pybullet library handle).
+ center: Center of sphere.
+ radius: Sphere radius.
+ rgba: rgba color of sphere.
+
+ Returns:
+ Unique integer bullet id of constructed object.
+ """
+ vis_obj_id = client.createVisualShape(
+ client.GEOM_SPHERE, radius=radius, rgbaColor=rgba
+ )
+ obj_id = client.createMultiBody(
+ baseCollisionShapeIndex=-1, baseVisualShapeIndex=vis_obj_id, basePosition=center
+ )
+ return obj_id
+
+
+def pybullet_mat_to_numpy_4x4(pybullet_matrix):
+ assert len(pybullet_matrix) == 16, "pybullet matrix should be len 16"
+ return np.transpose(np.reshape(np.array(pybullet_matrix, dtype=np.float64), (4, 4)))
+
+
+def decompose_view_matrix(pybullet_view_matrix):
+ """Decompose view matrix into pos + quat format (assumes mat is rigid!)."""
+ # It would be MUCH better to use something from bullet, however pybullet does
+ # not expose all of the linear algebra library.
+ mat = pybullet_mat_to_numpy_4x4(pybullet_view_matrix)
+
+ # View matrix is now:
+ # | R_11 R_12 R_13 t_1 |
+ # | R_21 R_22 R_23 t_2 |
+ # | R_31 R_32 R_33 t_3 |
+ # | 0 0 0 1 |
+
+ # R is the inverse eye to target at orientation, and t is R * eye.
+ mat_view_to_world = np.linalg.inv(mat)
+
+ # mat_view_to_world is the view to world transform, therefore the translation
+ # component of this matrix is simply the world space position (since mat *
+ # (0, 0, 0, 1)) is just copying the right column.
+ world_xyz_view = np.copy(mat_view_to_world[0:3, 3])
+
+ mat_view_to_world[0:3, 3] = 0 # Zero out the position change.
+ world_quat_view = matrix_to_rotation(mat_view_to_world).as_quat()
+
+ return world_xyz_view, world_quat_view
+
+
+def world_obj_to_view(world_xyz_obj, world_quat_obj, camera_view, client):
+ """Transform object into view space."""
+ world_xyz_view, world_quat_view = decompose_view_matrix(camera_view)
+ view_xyz_world, view_quat_world = client.invertTransform(
+ world_xyz_view, world_quat_view
+ )
+ view_xyz_obj, view_quat_obj = client.multiplyTransforms(
+ view_xyz_world, view_quat_world, world_xyz_obj, world_quat_obj
+ )
+
+ return view_xyz_obj, view_quat_obj
+
+
+def image_xy_to_view_ray(xy, cam_width, cam_height, proj_mat_inv):
+ """Calculate view-space ray from pixel location."""
+ # Recall (from http://www.songho.ca/opengl/gl_projectionmatrix.html):
+ # xyzw_clip = M_proj * xyzw_eye, and
+ # xyz_ndc = xyzw_clip[0:3] / xwzw_clip[3].
+ xyz_ndc = np.array(
+ [2.0 * xy[0] / cam_width - 1.0, -(2.0 * xy[1] / cam_height - 1.0), 0]
+ ) # in [-1, 1]
+ xyzw_clip = np.concatenate([xyz_ndc, [1]])
+ xyzw_eye = proj_mat_inv @ xyzw_clip
+ origin = np.zeros(3)
+ vec = xyzw_eye[:3] / max(np.linalg.norm(xyzw_eye[:3]), 1e-6)
+ return origin, vec
+
+
+def view_ray_to_world_ray(origin, vec, view_mat_inv):
+ """Transform view-space ray into world space."""
+ origin = view_mat_inv @ np.concatenate([origin, [1]])
+ vec = view_mat_inv @ np.concatenate([vec, [0]])
+
+ return origin[:3], vec[:3]
+
+
+def ray_to_plane_test(ray_origin, ray_vec, plane_origin, plane_normal):
+ """Perform a ray-plane intersection test."""
+ ln = np.dot(plane_normal, ray_vec)
+ if abs(ln) < np.finfo(np.float32).eps:
+ return None
+
+ # Solve for the intersection fraction t.
+ t = np.dot(plane_normal, plane_origin - ray_origin) / ln
+ if t >= 0:
+ return ray_origin + ray_vec * t
+ else:
+ return None
+
+
+def get_workspace(env):
+ (
+ workspace_origin,
+ workspace_quat,
+ ) = env.pybullet_client.getBasePositionAndOrientation(env.workspace_uid)
+ workspace_normal = rotation_to_matrix(transform.Rotation.from_quat(workspace_quat))[
+ 2, 0:3
+ ]
+
+ return workspace_origin, workspace_normal
+
+
+def reset_camera_pose(env, view_type):
+ """Reset camera pose to canonical frame."""
+ p = env.pybullet_client
+
+ if view_type == "POLICY":
+ camera_info = p.getDebugVisualizerCamera()
+ image_size = (camera_info[0], camera_info[1])
+
+ viewm, _, front_position, lookat, _ = env.calc_camera_params(image_size)
+
+ euler = matrix_to_rotation(pybullet_mat_to_numpy_4x4(viewm)[0:3, 0:3]).as_euler(
+ "xyz", degrees=False
+ )
+ pitch = euler[1]
+ yaw = -euler[2]
+ # The distance is a bit far away (the GL view has higher FOV).
+ distance = np.linalg.norm(front_position - lookat) * 0.6
+ elif view_type == "TOP_DOWN":
+ workspace_origin, _ = get_workspace(env)
+ distance = 0.5
+ lookat = workspace_origin
+ yaw = np.pi / 2
+ # Note: pi/2 pitch results in gimble lock and pybullet doesn't support it.
+ pitch = -(np.pi / 2 - 1e-5)
+ else:
+ raise ValueError("unsupported view_type %s" % view_type)
+ p.resetDebugVisualizerCamera(
+ cameraDistance=distance,
+ cameraYaw=360 * yaw / (2.0 * np.pi),
+ cameraPitch=360 * pitch / (2.0 * np.pi),
+ cameraTargetPosition=lookat,
+ )
+
+
+def _lists_to_tuple(obj):
+ if isinstance(obj, list):
+ return tuple([_lists_to_tuple(v) for v in obj])
+ else:
+ return obj
+
+
+@dataclasses.dataclass
+class ObjState:
+ """A container for storing pybullet object state."""
+
+ obj_id: int
+
+ # base_pose: (xyz, quat).
+ base_pose: Tuple[Vec3, Vec4]
+ # base_vel: (vel, ang_vel).
+ base_vel: Tuple[Vec3, Vec3]
+ joint_info: Any
+ joint_state: Any
+
+ @staticmethod
+ def get_bullet_state(client, obj_id):
+ """Read Pybullet internal state."""
+ base_pose = client.getBasePositionAndOrientation(obj_id)
+ base_vel = client.getBaseVelocity(obj_id)
+
+ joint_info = []
+ joint_state = []
+ for i in range(client.getNumJoints(obj_id)):
+ joint_state.append(client.getJointState(obj_id, i))
+ joint_info.append(ObjState._get_joint_info(client, obj_id, i))
+
+ return ObjState(
+ obj_id=obj_id,
+ base_pose=base_pose,
+ base_vel=base_vel,
+ joint_info=tuple(joint_info),
+ joint_state=tuple(joint_state),
+ )
+
+ @staticmethod
+ def _get_joint_info(client, obj_id, joint_index):
+ ji = client.getJointInfo(obj_id, joint_index)
+ return tuple([v if not isinstance(v, bytes) else v.decode("utf-8") for v in ji])
+
+ def set_bullet_state(self, client, obj_id):
+ """Hard set the current bullet state."""
+ xyz, quat = self.base_pose
+ client.resetBasePositionAndOrientation(obj_id, xyz, quat)
+ vel, ang_vel = self.base_vel
+ client.resetBaseVelocity(obj_id, vel, ang_vel)
+
+ njoints = client.getNumJoints(obj_id)
+ if njoints != len(self.joint_info) or njoints != len(self.joint_state):
+ raise ValueError("Incorrect number of joint info state pairs.")
+
+ for i, (joint_info, joint_state) in enumerate(
+ zip(self.joint_info, self.joint_state)
+ ):
+ joint_index = joint_info[0]
+ if joint_index != i:
+ raise ValueError("Joint index mismatch.")
+
+ # Check that the current joint we're trying to restore state for has the
+ # same info as the state joint.
+ cur_joint_info = ObjState._get_joint_info(client, obj_id, joint_index)
+ if cur_joint_info != joint_info:
+ raise ValueError(
+ "joint_info mismatch %s vs %s (expected)"
+ % (str(cur_joint_info), str(joint_info))
+ )
+ joint_position = joint_state[0]
+ joint_velocity = joint_state[1]
+ client.resetJointState(
+ obj_id, i, targetValue=joint_position, targetVelocity=joint_velocity
+ )
+
+ def serialize(self):
+ return {
+ "obj_id": self.obj_id,
+ "base_pose": self.base_pose,
+ "base_vel": self.base_vel,
+ "joint_info": self.joint_info,
+ "joint_state": self.joint_state,
+ }
+
+ @staticmethod
+ def deserialize(data):
+ return ObjState(
+ obj_id=_lists_to_tuple(data["obj_id"]),
+ base_pose=_lists_to_tuple(data["base_pose"]),
+ base_vel=_lists_to_tuple(data["base_vel"]),
+ joint_info=_lists_to_tuple(data["joint_info"]),
+ joint_state=_lists_to_tuple(data["joint_state"]),
+ )
+
+
+@dataclasses.dataclass
+class XarmState(ObjState):
+ """A container for storing pybullet robot state."""
+
+ # The set point of the robot's controller.
+ target_effector_pose: Pose3d
+ goal_translation: Optional[Vec3]
+
+ @staticmethod
+ def get_bullet_state(client, obj_id, target_effector_pose, goal_translation):
+ if goal_translation is not None:
+ goal_translation = tuple(goal_translation.tolist())
+ return XarmState(
+ **dataclasses.asdict(ObjState.get_bullet_state(client, obj_id)),
+ target_effector_pose=target_effector_pose,
+ goal_translation=goal_translation
+ )
+
+ def serialize(self):
+ data = ObjState.serialize(self)
+ data["target_effector_pose"] = self.target_effector_pose.serialize()
+ if self.goal_translation is not None:
+ data["goal_translation"] = self.goal_translation
+ else:
+ data["goal_translation"] = []
+ return data
+
+ @staticmethod
+ def deserialize(data):
+ goal_translation = (
+ None
+ if not data["goal_translation"]
+ else _lists_to_tuple(data["goal_translation"])
+ )
+ return XarmState(
+ obj_id=data["obj_id"],
+ base_pose=_lists_to_tuple(data["base_pose"]),
+ base_vel=_lists_to_tuple(data["base_vel"]),
+ joint_info=_lists_to_tuple(data["joint_info"]),
+ joint_state=_lists_to_tuple(data["joint_state"]),
+ goal_translation=goal_translation,
+ target_effector_pose=Pose3d.deserialize(data["target_effector_pose"]),
+ )
+
+
+def _serialize_pybullet_state(pybullet_state):
+ """Convert data to POD types."""
+ if isinstance(pybullet_state, list):
+ return [_serialize_pybullet_state(entry) for entry in pybullet_state]
+ elif isinstance(pybullet_state, dict):
+ assert "_serialized_obj_name" not in pybullet_state
+ return {
+ key: _serialize_pybullet_state(value)
+ for key, value in pybullet_state.items()
+ }
+ elif isinstance(pybullet_state, (XarmState, ObjState)):
+ return {
+ "_serialized_obj_name": type(pybullet_state).__name__,
+ "_serialized_data": pybullet_state.serialize(),
+ }
+ elif isinstance(pybullet_state, int):
+ return pybullet_state
+ else:
+ raise ValueError(
+ "Unhandled type for object %s, type %s"
+ % (str(pybullet_state), type(pybullet_state))
+ )
+
+
+def _deserialize_pybullet_state(state):
+ """Parse data from POD types."""
+ if isinstance(state, list):
+ return [_deserialize_pybullet_state(item) for item in state]
+ elif isinstance(state, dict):
+ if "_serialized_obj_name" in state:
+ if state["_serialized_obj_name"] == XarmState.__name__:
+ return XarmState.deserialize(state["_serialized_data"])
+ elif state["_serialized_obj_name"] == ObjState.__name__:
+ return ObjState.deserialize(state["_serialized_data"])
+ else:
+ raise ValueError("Unsupported: %s" % state["_serialized_obj_name"])
+ else:
+ return {
+ key: _deserialize_pybullet_state(value) for key, value in state.items()
+ }
+ elif isinstance(state, int):
+ return state
+ else:
+ raise ValueError("Unhandled type for object %s" % str(state))
+
+
+def write_pybullet_state(filename, pybullet_state, task, actions=None):
+ """Serialize pybullet state to json file."""
+ import torch
+ data = {
+ "pybullet_state": _serialize_pybullet_state(pybullet_state),
+ "state_version": PYBULLET_STATE_VERSION,
+ "ts_ms": int(time.mktime(datetime.datetime.now().timetuple())) * 1000,
+ "user": getpass.getuser(),
+ "task": task,
+ "actions": actions if actions is not None else [],
+ }
+ torch.save(data, filename)
+
+
+def read_pybullet_state(filename):
+ """Deserialize pybullet state from json file."""
+ import torch
+ data = torch.load(filename)
+
+ assert isinstance(data, dict)
+
+ if data["state_version"] != PYBULLET_STATE_VERSION:
+ raise ValueError(
+ "incompatible state data (version %d, expected %d)"
+ % (data["state_version"], PYBULLET_STATE_VERSION)
+ )
+
+ data["pybullet_state"] = _deserialize_pybullet_state(data["pybullet_state"])
+ return data
diff --git a/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/xarm_sim_robot.py b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/xarm_sim_robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..600dd7ec0f3ef363d6474a9f4c493c001a287d69
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/block_pushing/utils/xarm_sim_robot.py
@@ -0,0 +1,230 @@
+# coding=utf-8
+# Copyright 2022 The Reach ML Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""XArm Robot Kinematics."""
+from diffusion_policy.env.block_pushing.utils import utils_pybullet
+from diffusion_policy.env.block_pushing.utils.pose3d import Pose3d
+import numpy as np
+from scipy.spatial import transform
+import pybullet
+
+XARM_URDF_PATH = (
+ "third_party/bullet/examples/pybullet/gym/pybullet_data/" "xarm/xarm6_robot.urdf"
+)
+SUCTION_URDF_PATH = "third_party/py/envs/assets/suction/" "suction-head-long.urdf"
+CYLINDER_URDF_PATH = "third_party/py/envs/assets/suction/" "cylinder.urdf"
+CYLINDER_REAL_URDF_PATH = "third_party/py/envs/assets/suction/" "cylinder_real.urdf"
+HOME_JOINT_POSITIONS = np.deg2rad([0, -20, -80, 0, 100, -30])
+
+
+class XArmSimRobot:
+ """A simulated PyBullet XArm robot, mostly for forward/inverse kinematics."""
+
+ def __init__(
+ self,
+ pybullet_client,
+ initial_joint_positions=HOME_JOINT_POSITIONS,
+ end_effector="none",
+ color="default",
+ ):
+ self._pybullet_client = pybullet_client
+ self.initial_joint_positions = initial_joint_positions
+
+ if color == "default":
+ self.xarm = utils_pybullet.load_urdf(
+ pybullet_client, XARM_URDF_PATH, [0, 0, 0]
+ )
+ else:
+ raise ValueError("Unrecognized xarm color %s" % color)
+
+ # Get revolute joints of robot (skip fixed joints).
+ joints = []
+ joint_indices = []
+ for i in range(self._pybullet_client.getNumJoints(self.xarm)):
+ joint_info = self._pybullet_client.getJointInfo(self.xarm, i)
+ if joint_info[2] == pybullet.JOINT_REVOLUTE:
+ joints.append(joint_info[0])
+ joint_indices.append(i)
+ # Note examples in pybullet do this, but it is not clear what the
+ # benefits are.
+ self._pybullet_client.changeDynamics(
+ self.xarm, i, linearDamping=0, angularDamping=0
+ )
+
+ self._n_joints = len(joints)
+ self._joints = tuple(joints)
+ self._joint_indices = tuple(joint_indices)
+
+ # Move robot to home joint configuration
+ self.reset_joints(self.initial_joint_positions)
+ self.effector_link = 6
+
+ if (
+ end_effector == "suction"
+ or end_effector == "cylinder"
+ or end_effector == "cylinder_real"
+ ):
+ self.end_effector = self._setup_end_effector(end_effector)
+ else:
+ if end_effector != "none":
+ raise ValueError('end_effector "%s" is not supported.' % end_effector)
+ self.end_effector = None
+
+ def _setup_end_effector(self, end_effector):
+ """Adds a suction or cylinder end effector."""
+ pose = self.forward_kinematics()
+ if end_effector == "suction":
+ body = utils_pybullet.load_urdf(
+ self._pybullet_client,
+ SUCTION_URDF_PATH,
+ pose.translation,
+ pose.rotation.as_quat(),
+ )
+ elif end_effector == "cylinder":
+ body = utils_pybullet.load_urdf(
+ self._pybullet_client,
+ CYLINDER_URDF_PATH,
+ pose.translation,
+ pose.rotation.as_quat(),
+ )
+ elif end_effector == "cylinder_real":
+ body = utils_pybullet.load_urdf(
+ self._pybullet_client,
+ CYLINDER_REAL_URDF_PATH,
+ pose.translation,
+ pose.rotation.as_quat(),
+ )
+ else:
+ raise ValueError('end_effector "%s" is not supported.' % end_effector)
+
+ constraint_id = self._pybullet_client.createConstraint(
+ parentBodyUniqueId=self.xarm,
+ parentLinkIndex=6,
+ childBodyUniqueId=body,
+ childLinkIndex=-1,
+ jointType=pybullet.JOINT_FIXED,
+ jointAxis=(0, 0, 0),
+ parentFramePosition=(0, 0, 0),
+ childFramePosition=(0, 0, 0),
+ )
+ self._pybullet_client.changeConstraint(constraint_id, maxForce=50)
+
+ return body
+
+ def reset_joints(self, joint_values):
+ """Sets the position of the Robot's joints.
+
+ *Note*: This should only be used at the start while not running the
+ simulation resetJointState overrides all physics simulation.
+
+ Args:
+ joint_values: Iterable with desired joint positions.
+ """
+ for i in range(self._n_joints):
+ self._pybullet_client.resetJointState(
+ self.xarm, self._joints[i], joint_values[i]
+ )
+
+ def get_joints_measured(self):
+ joint_states = self._pybullet_client.getJointStates(
+ self.xarm, self._joint_indices
+ )
+ joint_positions = np.array([state[0] for state in joint_states])
+ joint_velocities = np.array([state[1] for state in joint_states])
+ joint_torques = np.array([state[3] for state in joint_states])
+ return joint_positions, joint_velocities, joint_torques
+
+ def get_joint_positions(self):
+ joint_states = self._pybullet_client.getJointStates(
+ self.xarm, self._joint_indices
+ )
+ joint_positions = np.array([state[0] for state in joint_states])
+ return joint_positions
+
+ def forward_kinematics(self):
+ """Forward kinematics."""
+ effector_state = self._pybullet_client.getLinkState(
+ self.xarm, self.effector_link
+ )
+ return Pose3d(
+ translation=np.array(effector_state[0]),
+ rotation=transform.Rotation.from_quat(effector_state[1]),
+ )
+
+ def inverse_kinematics(
+ self, world_effector_pose, max_iterations=100, residual_threshold=1e-10
+ ):
+ """Inverse kinematics.
+
+ Args:
+ world_effector_pose: Target Pose3d for the robot's end effector.
+ max_iterations: Refine the IK solution until the distance between target
+ and actual end effector position is below this threshold, or the
+ maxNumIterations is reached. Default is 20 iterations.
+ residual_threshold: Refine the IK solution until the distance between
+ target and actual end effector position is below this threshold, or the
+ maxNumIterations is reached.
+
+ Returns:
+ Numpy array with required joint angles to reach the requested pose.
+ """
+ return np.array(
+ self._pybullet_client.calculateInverseKinematics(
+ self.xarm,
+ self.effector_link,
+ world_effector_pose.translation,
+ world_effector_pose.rotation.as_quat(), # as_quat returns xyzw.
+ lowerLimits=[-17] * 6,
+ upperLimits=[17] * 6,
+ jointRanges=[17] * 6,
+ restPoses=[0, 0] + self.get_joint_positions()[2:].tolist(),
+ maxNumIterations=max_iterations,
+ residualThreshold=residual_threshold,
+ )
+ )
+
+ def set_target_effector_pose(self, world_effector_pose):
+ target_joint_positions = self.inverse_kinematics(world_effector_pose)
+ self.set_target_joint_positions(target_joint_positions)
+
+ def set_target_joint_velocities(self, target_joint_velocities):
+ self._pybullet_client.setJointMotorControlArray(
+ self.xarm,
+ self._joint_indices,
+ pybullet.VELOCITY_CONTROL,
+ targetVelocities=target_joint_velocities,
+ forces=[5 * 240.0] * 6,
+ )
+
+ def set_target_joint_positions(self, target_joint_positions):
+ self._pybullet_client.setJointMotorControlArray(
+ self.xarm,
+ self._joint_indices,
+ pybullet.POSITION_CONTROL,
+ targetPositions=target_joint_positions,
+ forces=[5 * 240.0] * 6,
+ )
+
+ def set_alpha_transparency(self, alpha):
+ visual_shape_data = self._pybullet_client.getVisualShapeData(self.xarm)
+
+ for i in range(self._pybullet_client.getNumJoints(self.xarm)):
+ object_id, link_index, _, _, _, _, _, rgba_color = visual_shape_data[i]
+ assert object_id == self.xarm, "xarm id mismatch."
+ assert link_index == i, "Link visual data was returned out of order."
+ rgba_color = list(rgba_color[0:3]) + [alpha]
+ self._pybullet_client.changeVisualShape(
+ self.xarm, linkIndex=i, rgbaColor=rgba_color
+ )
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.pylintrc b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.pylintrc
new file mode 100644
index 0000000000000000000000000000000000000000..9cda41207e11dceda158f4eb9864dd3bbd57dc64
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.pylintrc
@@ -0,0 +1,433 @@
+[MASTER]
+
+# A comma-separated list of package or module names from where C extensions may
+# be loaded. Extensions are loading into the active Python interpreter and may
+# run arbitrary code.
+extension-pkg-whitelist=
+
+# Add files or directories to the blacklist. They should be base names, not
+# paths.
+ignore=CVS
+
+# Add files or directories matching the regex patterns to the blacklist. The
+# regex matches against base names, not paths.
+ignore-patterns=
+
+# Python code to execute, usually for sys.path manipulation such as
+# pygtk.require().
+#init-hook=
+
+# Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the
+# number of processors available to use.
+jobs=1
+
+# Control the amount of potential inferred values when inferring a single
+# object. This can help the performance when dealing with large functions or
+# complex, nested conditions.
+limit-inference-results=100
+
+# List of plugins (as comma separated values of python modules names) to load,
+# usually to register additional checkers.
+load-plugins=
+
+# Pickle collected data for later comparisons.
+persistent=yes
+
+# Specify a configuration file.
+#rcfile=
+
+# When enabled, pylint would attempt to guess common misconfiguration and emit
+# user-friendly hints instead of false-positive error messages.
+suggestion-mode=yes
+
+# Allow loading of arbitrary C extensions. Extensions are imported into the
+# active Python interpreter and may run arbitrary code.
+unsafe-load-any-extension=no
+
+
+[MESSAGES CONTROL]
+
+# Only show warnings with the listed confidence levels. Leave empty to show
+# all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED.
+confidence=
+
+# Disable the message, report, category or checker with the given id(s). You
+# can either give multiple identifiers separated by comma (,) or put this
+# option multiple times (only on the command line, not in the configuration
+# file where it should appear only once). You can also use "--disable=all" to
+# disable everything first and then reenable specific checks. For example, if
+# you want to run only the similarities checker, you can use "--disable=all
+# --enable=similarities". If you want to run only the classes checker, but have
+# no Warning level messages displayed, use "--disable=all --enable=classes
+# --disable=W".
+disable=relative-beyond-top-level
+
+
+[REPORTS]
+
+# Python expression which should return a note less than 10 (10 is the highest
+# note). You have access to the variables errors warning, statement which
+# respectively contain the number of errors / warnings messages and the total
+# number of statements analyzed. This is used by the global evaluation report
+# (RP0004).
+evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10)
+
+# Template used to display messages. This is a python new-style format string
+# used to format the message information. See doc for all details.
+#msg-template=
+
+# Set the output format. Available formats are text, parseable, colorized, json
+# and msvs (visual studio). You can also give a reporter class, e.g.
+# mypackage.mymodule.MyReporterClass.
+output-format=text
+
+# Tells whether to display a full report or only the messages.
+reports=no
+
+# Activate the evaluation score.
+score=yes
+
+
+[REFACTORING]
+
+# Maximum number of nested blocks for function / method body
+max-nested-blocks=5
+
+# Complete name of functions that never returns. When checking for
+# inconsistent-return-statements if a never returning function is called then
+# it will be considered as an explicit return statement and no message will be
+# printed.
+never-returning-functions=sys.exit
+
+
+[LOGGING]
+
+# Format style used to check logging format string. `old` means using %
+# formatting, while `new` is for `{}` formatting.
+logging-format-style=old
+
+# Logging modules to check that the string format arguments are in logging
+# function parameter format.
+logging-modules=logging
+
+
+[VARIABLES]
+
+# List of additional names supposed to be defined in builtins. Remember that
+# you should avoid defining new builtins when possible.
+additional-builtins=
+
+# Tells whether unused global variables should be treated as a violation.
+allow-global-unused-variables=yes
+
+# List of strings which can identify a callback function by name. A callback
+# name must start or end with one of those strings.
+callbacks=cb_,
+ _cb
+
+# A regular expression matching the name of dummy variables (i.e. expected to
+# not be used).
+dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_
+
+# Argument names that match this expression will be ignored. Default to name
+# with leading underscore.
+ignored-argument-names=_.*|^ignored_|^unused_
+
+# Tells whether we should check for unused import in __init__ files.
+init-import=no
+
+# List of qualified module names which can have objects that can redefine
+# builtins.
+redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io
+
+
+[FORMAT]
+
+# Expected format of line ending, e.g. empty (any line ending), LF or CRLF.
+expected-line-ending-format=
+
+# Regexp for a line that is allowed to be longer than the limit.
+ignore-long-lines=^\s*(# )??$
+
+# Number of spaces of indent required inside a hanging or continued line.
+indent-after-paren=4
+
+# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1
+# tab).
+indent-string=' '
+
+# Maximum number of characters on a single line.
+max-line-length=80
+
+# Maximum number of lines in a module
+max-module-lines=99999
+
+# List of optional constructs for which whitespace checking is disabled. `dict-
+# separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}.
+# `trailing-comma` allows a space between comma and closing bracket: (a, ).
+# `empty-line` allows space-only lines.
+no-space-check=trailing-comma,
+ dict-separator
+
+# Allow the body of a class to be on the same line as the declaration if body
+# contains single statement.
+single-line-class-stmt=no
+
+# Allow the body of an if to be on the same line as the test if there is no
+# else.
+single-line-if-stmt=no
+
+
+[TYPECHECK]
+
+# List of decorators that produce context managers, such as
+# contextlib.contextmanager. Add to this list to register other decorators that
+# produce valid context managers.
+contextmanager-decorators=contextlib.contextmanager
+
+# List of members which are set dynamically and missed by pylint inference
+# system, and so shouldn't trigger E1101 when accessed. Python regular
+# expressions are accepted.
+generated-members=
+
+# Tells whether missing members accessed in mixin class should be ignored. A
+# mixin class is detected if its name ends with "mixin" (case insensitive).
+ignore-mixin-members=yes
+
+# Tells whether to warn about missing members when the owner of the attribute
+# is inferred to be None.
+ignore-none=yes
+
+# This flag controls whether pylint should warn about no-member and similar
+# checks whenever an opaque object is returned when inferring. The inference
+# can return multiple potential results while evaluating a Python object, but
+# some branches might not be evaluated, which results in partial inference. In
+# that case, it might be useful to still emit no-member and other checks for
+# the rest of the inferred objects.
+ignore-on-opaque-inference=yes
+
+# List of class names for which member attributes should not be checked (useful
+# for classes with dynamically set attributes). This supports the use of
+# qualified names.
+ignored-classes=optparse.Values,thread._local,_thread._local
+
+# List of module names for which member attributes should not be checked
+# (useful for modules/projects where namespaces are manipulated during runtime
+# and thus existing member attributes cannot be deduced by static analysis. It
+# supports qualified module names, as well as Unix pattern matching.
+ignored-modules=
+
+# Show a hint with possible names when a member name was not found. The aspect
+# of finding the hint is based on edit distance.
+missing-member-hint=yes
+
+# The minimum edit distance a name should have in order to be considered a
+# similar match for a missing member name.
+missing-member-hint-distance=1
+
+# The total number of similar names that should be taken in consideration when
+# showing a hint for a missing member.
+missing-member-max-choices=1
+
+
+[SIMILARITIES]
+
+# Ignore comments when computing similarities.
+ignore-comments=yes
+
+# Ignore docstrings when computing similarities.
+ignore-docstrings=yes
+
+# Ignore imports when computing similarities.
+ignore-imports=no
+
+# Minimum lines number of a similarity.
+min-similarity-lines=4
+
+
+[BASIC]
+
+# Naming style matching correct argument names
+argument-naming-style=snake_case
+
+# Regular expression matching correct argument names. Overrides argument-
+# naming-style
+argument-rgx=^[a-z][a-z0-9_]*$
+
+# Naming style matching correct attribute names
+attr-naming-style=snake_case
+
+# Regular expression matching correct attribute names. Overrides attr-naming-
+# style
+attr-rgx=^_{0,2}[a-z][a-z0-9_]*$
+
+# Bad variable names which should always be refused, separated by a comma
+bad-names=
+
+# Naming style matching correct class attribute names
+class-attribute-naming-style=any
+
+# Regular expression matching correct class attribute names. Overrides class-
+# attribute-naming-style
+class-attribute-rgx=^(_?[A-Z][A-Z0-9_]*|__[a-z0-9_]+__|_?[a-z][a-z0-9_]*)$
+
+# Naming style matching correct class names
+class-naming-style=PascalCase
+
+# Regular expression matching correct class names. Overrides class-naming-style
+class-rgx=^_?[A-Z][a-zA-Z0-9]*$
+
+# Naming style matching correct constant names
+const-naming-style=UPPER_CASE
+
+# Regular expression matching correct constant names. Overrides const-naming-
+# style
+const-rgx=^(_?[A-Z][A-Z0-9_]*|__[a-z0-9_]+__|_?[a-z][a-z0-9_]*)$
+
+# Minimum line length for functions/classes that require docstrings, shorter
+# ones are exempt.
+docstring-min-length=10
+
+# Naming style matching correct function names
+function-naming-style=snake_case
+
+# Regular expression matching correct function names. Overrides function-
+# naming-style
+function-rgx=^(?:(?PsetUp|tearDown|setUpModule|tearDownModule)|(?P_?[A-Z][a-zA-Z0-9]*)|(?P_?[a-z][a-z0-9_]*))$
+
+# Good variable names which should always be accepted, separated by a comma
+good-names=main,
+ _
+
+# Include a hint for the correct naming format with invalid-name
+include-naming-hint=no
+
+# Naming style matching correct inline iteration names
+inlinevar-naming-style=any
+
+# Regular expression matching correct inline iteration names. Overrides
+# inlinevar-naming-style
+inlinevar-rgx=^[a-z][a-z0-9_]*$
+
+# Naming style matching correct method names
+method-naming-style=snake_case
+
+# Regular expression matching correct method names. Overrides method-naming-
+# style
+method-rgx=(?x)^(?:(?P_[a-z0-9_]+__|runTest|setUp|tearDown|setUpTestCase|tearDownTestCase|setupSelf|tearDownClass|setUpClass|(test|assert)_*[A-Z0-9][a-zA-Z0-9_]*|next)|(?P_{0,2}[A-Z][a-zA-Z0-9_]*)|(?P_{0,2}[a-z][a-z0-9_]*))$
+
+# Naming style matching correct module names
+module-naming-style=snake_case
+
+# Regular expression matching correct module names. Overrides module-naming-
+# style
+module-rgx=^(_?[a-z][a-z0-9_]*)|__init__|PRESUBMIT|PRESUBMIT_unittest$
+
+# Colon-delimited sets of names that determine each other's naming style when
+# the name regexes allow several styles.
+name-group=function:method
+
+# Regular expression which should only match function or class names that do
+# not require a docstring.
+no-docstring-rgx=(__.*__|main)
+
+# List of decorators that produce properties, such as abc.abstractproperty. Add
+# to this list to register other decorators that produce valid properties.
+property-classes=abc.abstractproperty,google3.pyglib.function_utils.cached.property
+
+# Naming style matching correct variable names
+variable-naming-style=snake_case
+
+# Regular expression matching correct variable names. Overrides variable-
+# naming-style
+variable-rgx=^[a-z][a-z0-9_]*$
+
+
+[SPELLING]
+
+# Limits count of emitted suggestions for spelling mistakes.
+max-spelling-suggestions=4
+
+# Spelling dictionary name. Available dictionaries: none. To make it working
+# install python-enchant package..
+spelling-dict=
+
+# List of comma separated words that should not be checked.
+spelling-ignore-words=
+
+# A path to a file that contains private dictionary; one word per line.
+spelling-private-dict-file=
+
+# Tells whether to store unknown words to indicated private dictionary in
+# --spelling-private-dict-file option instead of raising a message.
+spelling-store-unknown-words=no
+
+
+[MISCELLANEOUS]
+
+# List of note tags to take in consideration, separated by a comma.
+notes=FIXME,
+ XXX,
+ TODO
+
+
+[IMPORTS]
+
+# Allow wildcard imports from modules that define __all__.
+allow-wildcard-with-all=no
+
+# Analyse import fallback blocks. This can be used to support both Python 2 and
+# 3 compatible code, which means that the block might have code that exists
+# only in one or another interpreter, leading to false positives when analysed.
+analyse-fallback-blocks=no
+
+# Deprecated modules which should not be used, separated by a comma.
+deprecated-modules=optparse,tkinter.tix
+
+# Create a graph of external dependencies in the given file (report RP0402 must
+# not be disabled).
+ext-import-graph=
+
+# Create a graph of every (i.e. internal and external) dependencies in the
+# given file (report RP0402 must not be disabled).
+import-graph=
+
+# Create a graph of internal dependencies in the given file (report RP0402 must
+# not be disabled).
+int-import-graph=
+
+# Force import order to recognize a module as part of the standard
+# compatibility libraries.
+known-standard-library=
+
+# Force import order to recognize a module as part of a third party library.
+known-third-party=enchant
+
+
+[CLASSES]
+
+# List of method names used to declare (i.e. assign) instance attributes.
+defining-attr-methods=__init__,
+ __new__,
+ setUp
+
+# List of member names, which should be excluded from the protected access
+# warning.
+exclude-protected=_asdict,
+ _fields,
+ _replace,
+ _source,
+ _make
+
+# List of valid names for the first argument in a class method.
+valid-classmethod-first-arg=cls
+
+# List of valid names for the first argument in a metaclass class method.
+valid-metaclass-classmethod-first-arg=cls
+
+
+[EXCEPTIONS]
+
+# Exceptions that will emit a warning when being caught. Defaults to
+# "Exception".
+overgeneral-exceptions=Exception
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.style.yapf b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.style.yapf
new file mode 100644
index 0000000000000000000000000000000000000000..29f83ff21d1f14b3d47bdb5e22de7437f26cd49c
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/.style.yapf
@@ -0,0 +1,323 @@
+[style]
+# Align closing bracket with visual indentation.
+align_closing_bracket_with_visual_indent=False
+
+# Allow dictionary keys to exist on multiple lines. For example:
+#
+# x = {
+# ('this is the first element of a tuple',
+# 'this is the second element of a tuple'):
+# value,
+# }
+allow_multiline_dictionary_keys=False
+
+# Allow lambdas to be formatted on more than one line.
+allow_multiline_lambdas=False
+
+# Allow splitting before a default / named assignment in an argument list.
+allow_split_before_default_or_named_assigns=True
+
+# Allow splits before the dictionary value.
+allow_split_before_dict_value=True
+
+# Let spacing indicate operator precedence. For example:
+#
+# a = 1 * 2 + 3 / 4
+# b = 1 / 2 - 3 * 4
+# c = (1 + 2) * (3 - 4)
+# d = (1 - 2) / (3 + 4)
+# e = 1 * 2 - 3
+# f = 1 + 2 + 3 + 4
+#
+# will be formatted as follows to indicate precedence:
+#
+# a = 1*2 + 3/4
+# b = 1/2 - 3*4
+# c = (1+2) * (3-4)
+# d = (1-2) / (3+4)
+# e = 1*2 - 3
+# f = 1 + 2 + 3 + 4
+#
+arithmetic_precedence_indication=False
+
+# Number of blank lines surrounding top-level function and class
+# definitions.
+blank_lines_around_top_level_definition=2
+
+# Insert a blank line before a class-level docstring.
+blank_line_before_class_docstring=False
+
+# Insert a blank line before a module docstring.
+blank_line_before_module_docstring=False
+
+# Insert a blank line before a 'def' or 'class' immediately nested
+# within another 'def' or 'class'. For example:
+#
+# class Foo:
+# # <------ this blank line
+# def method():
+# ...
+blank_line_before_nested_class_or_def=True
+
+# Do not split consecutive brackets. Only relevant when
+# dedent_closing_brackets is set. For example:
+#
+# call_func_that_takes_a_dict(
+# {
+# 'key1': 'value1',
+# 'key2': 'value2',
+# }
+# )
+#
+# would reformat to:
+#
+# call_func_that_takes_a_dict({
+# 'key1': 'value1',
+# 'key2': 'value2',
+# })
+coalesce_brackets=False
+
+# The column limit.
+column_limit=80
+
+# The style for continuation alignment. Possible values are:
+#
+# - SPACE: Use spaces for continuation alignment. This is default behavior.
+# - FIXED: Use fixed number (CONTINUATION_INDENT_WIDTH) of columns
+# (ie: CONTINUATION_INDENT_WIDTH/INDENT_WIDTH tabs) for continuation
+# alignment.
+# - LESS: Slightly left if cannot vertically align continuation lines with
+# indent characters.
+# - VALIGN-RIGHT: Vertically align continuation lines with indent
+# characters. Slightly right (one more indent character) if cannot
+# vertically align continuation lines with indent characters.
+#
+# For options FIXED, and VALIGN-RIGHT are only available when USE_TABS is
+# enabled.
+continuation_align_style=SPACE
+
+# Indent width used for line continuations.
+continuation_indent_width=4
+
+# Put closing brackets on a separate line, dedented, if the bracketed
+# expression can't fit in a single line. Applies to all kinds of brackets,
+# including function definitions and calls. For example:
+#
+# config = {
+# 'key1': 'value1',
+# 'key2': 'value2',
+# } # <--- this bracket is dedented and on a separate line
+#
+# time_series = self.remote_client.query_entity_counters(
+# entity='dev3246.region1',
+# key='dns.query_latency_tcp',
+# transform=Transformation.AVERAGE(window=timedelta(seconds=60)),
+# start_ts=now()-timedelta(days=3),
+# end_ts=now(),
+# ) # <--- this bracket is dedented and on a separate line
+dedent_closing_brackets=False
+
+# Disable the heuristic which places each list element on a separate line
+# if the list is comma-terminated.
+disable_ending_comma_heuristic=False
+
+# Place each dictionary entry onto its own line.
+each_dict_entry_on_separate_line=True
+
+# The regex for an i18n comment. The presence of this comment stops
+# reformatting of that line, because the comments are required to be
+# next to the string they translate.
+i18n_comment=#\..*
+
+# The i18n function call names. The presence of this function stops
+# reformattting on that line, because the string it has cannot be moved
+# away from the i18n comment.
+i18n_function_call=N_, _
+
+# Indent blank lines.
+indent_blank_lines=False
+
+# Indent the dictionary value if it cannot fit on the same line as the
+# dictionary key. For example:
+#
+# config = {
+# 'key1':
+# 'value1',
+# 'key2': value1 +
+# value2,
+# }
+indent_dictionary_value=False
+
+# The number of columns to use for indentation.
+indent_width=4
+
+# Join short lines into one line. E.g., single line 'if' statements.
+join_multiple_lines=True
+
+# Do not include spaces around selected binary operators. For example:
+#
+# 1 + 2 * 3 - 4 / 5
+#
+# will be formatted as follows when configured with "*,/":
+#
+# 1 + 2*3 - 4/5
+#
+no_spaces_around_selected_binary_operators=
+
+# Use spaces around default or named assigns.
+spaces_around_default_or_named_assign=False
+
+# Use spaces around the power operator.
+spaces_around_power_operator=False
+
+# The number of spaces required before a trailing comment.
+# This can be a single value (representing the number of spaces
+# before each trailing comment) or list of values (representing
+# alignment column values; trailing comments within a block will
+# be aligned to the first column value that is greater than the maximum
+# line length within the block). For example:
+#
+# With spaces_before_comment=5:
+#
+# 1 + 1 # Adding values
+#
+# will be formatted as:
+#
+# 1 + 1 # Adding values <-- 5 spaces between the end of the statement and comment
+#
+# With spaces_before_comment=15, 20:
+#
+# 1 + 1 # Adding values
+# two + two # More adding
+#
+# longer_statement # This is a longer statement
+# short # This is a shorter statement
+#
+# a_very_long_statement_that_extends_beyond_the_final_column # Comment
+# short # This is a shorter statement
+#
+# will be formatted as:
+#
+# 1 + 1 # Adding values <-- end of line comments in block aligned to col 15
+# two + two # More adding
+#
+# longer_statement # This is a longer statement <-- end of line comments in block aligned to col 20
+# short # This is a shorter statement
+#
+# a_very_long_statement_that_extends_beyond_the_final_column # Comment <-- the end of line comments are aligned based on the line length
+# short # This is a shorter statement
+#
+spaces_before_comment=2
+
+# Insert a space between the ending comma and closing bracket of a list,
+# etc.
+space_between_ending_comma_and_closing_bracket=False
+
+# Split before arguments
+split_all_comma_separated_values=False
+
+# Split before arguments if the argument list is terminated by a
+# comma.
+split_arguments_when_comma_terminated=False
+
+# Set to True to prefer splitting before '&', '|' or '^' rather than
+# after.
+split_before_bitwise_operator=False
+
+# Split before the closing bracket if a list or dict literal doesn't fit on
+# a single line.
+split_before_closing_bracket=True
+
+# Split before a dictionary or set generator (comp_for). For example, note
+# the split before the 'for':
+#
+# foo = {
+# variable: 'Hello world, have a nice day!'
+# for variable in bar if variable != 42
+# }
+split_before_dict_set_generator=False
+
+# Split before the '.' if we need to split a longer expression:
+#
+# foo = ('This is a really long string: {}, {}, {}, {}'.format(a, b, c, d))
+#
+# would reformat to something like:
+#
+# foo = ('This is a really long string: {}, {}, {}, {}'
+# .format(a, b, c, d))
+split_before_dot=False
+
+# Split after the opening paren which surrounds an expression if it doesn't
+# fit on a single line.
+split_before_expression_after_opening_paren=False
+
+# If an argument / parameter list is going to be split, then split before
+# the first argument.
+split_before_first_argument=False
+
+# Set to True to prefer splitting before 'and' or 'or' rather than
+# after.
+split_before_logical_operator=False
+
+# Split named assignments onto individual lines.
+split_before_named_assigns=True
+
+# Set to True to split list comprehensions and generators that have
+# non-trivial expressions and multiple clauses before each of these
+# clauses. For example:
+#
+# result = [
+# a_long_var + 100 for a_long_var in xrange(1000)
+# if a_long_var % 10]
+#
+# would reformat to something like:
+#
+# result = [
+# a_long_var + 100
+# for a_long_var in xrange(1000)
+# if a_long_var % 10]
+split_complex_comprehension=True
+
+# The penalty for splitting right after the opening bracket.
+split_penalty_after_opening_bracket=30
+
+# The penalty for splitting the line after a unary operator.
+split_penalty_after_unary_operator=10000
+
+# The penalty for splitting right before an if expression.
+split_penalty_before_if_expr=0
+
+# The penalty of splitting the line around the '&', '|', and '^'
+# operators.
+split_penalty_bitwise_operator=300
+
+# The penalty for splitting a list comprehension or generator
+# expression.
+split_penalty_comprehension=2100
+
+# The penalty for characters over the column limit.
+split_penalty_excess_character=7000
+
+# The penalty incurred by adding a line split to the unwrapped line. The
+# more line splits added the higher the penalty.
+split_penalty_for_added_line_split=30
+
+# The penalty of splitting a list of "import as" names. For example:
+#
+# from a_very_long_or_indented_module_name_yada_yad import (long_argument_1,
+# long_argument_2,
+# long_argument_3)
+#
+# would reformat to something like:
+#
+# from a_very_long_or_indented_module_name_yada_yad import (
+# long_argument_1, long_argument_2, long_argument_3)
+split_penalty_import_names=0
+
+# The penalty of splitting the line around the 'and' and 'or'
+# operators.
+split_penalty_logical_operator=300
+
+# Use the Tab character for indentation.
+use_tabs=False
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/__init__.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..5e92a7bb514ef0df943ef53f458590861a33a8e2
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/__init__.py
@@ -0,0 +1,19 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import adept_envs.franka
+
+from adept_envs.utils.configurable import global_config
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/base_robot.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/base_robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..5c6f30f94f0f87df65f4108eff6b6d849d366cce
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/base_robot.py
@@ -0,0 +1,151 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import numpy as np
+from collections import deque
+
+class BaseRobot(object):
+ """Base class for all robot classes."""
+
+ def __init__(self,
+ n_jnt,
+ n_obj,
+ pos_bounds=None,
+ vel_bounds=None,
+ calibration_path=None,
+ is_hardware=False,
+ device_name=None,
+ overlay=False,
+ calibration_mode=False,
+ observation_cache_maxsize=5):
+ """Create a new robot.
+ Args:
+ n_jnt: The number of dofs in the robot.
+ n_obj: The number of dofs in the object.
+ pos_bounds: (n_jnt, 2)-shape matrix denoting the min and max joint
+ position for each joint.
+ vel_bounds: (n_jnt, 2)-shape matrix denoting the min and max joint
+ velocity for each joint.
+ calibration_path: File path to the calibration configuration file to
+ use.
+ is_hardware: Whether to run on hardware or not.
+ device_name: The device path for the robot hardware. Only required
+ in legacy mode.
+ overlay: Whether to show a simulation overlay of the hardware.
+ calibration_mode: Start with motors disengaged.
+ """
+
+ assert n_jnt > 0
+ assert n_obj >= 0
+
+ self._n_jnt = n_jnt
+ self._n_obj = n_obj
+ self._n_dofs = n_jnt + n_obj
+
+ self._pos_bounds = None
+ if pos_bounds is not None:
+ pos_bounds = np.array(pos_bounds, dtype=np.float32)
+ assert pos_bounds.shape == (self._n_dofs, 2)
+ for low, high in pos_bounds:
+ assert low < high
+ self._pos_bounds = pos_bounds
+ self._vel_bounds = None
+ if vel_bounds is not None:
+ vel_bounds = np.array(vel_bounds, dtype=np.float32)
+ assert vel_bounds.shape == (self._n_dofs, 2)
+ for low, high in vel_bounds:
+ assert low < high
+ self._vel_bounds = vel_bounds
+
+ self._is_hardware = is_hardware
+ self._device_name = device_name
+ self._calibration_path = calibration_path
+ self._overlay = overlay
+ self._calibration_mode = calibration_mode
+ self._observation_cache_maxsize = observation_cache_maxsize
+
+ # Gets updated
+ self._observation_cache = deque([], maxlen=self._observation_cache_maxsize)
+
+
+ @property
+ def n_jnt(self):
+ return self._n_jnt
+
+ @property
+ def n_obj(self):
+ return self._n_obj
+
+ @property
+ def n_dofs(self):
+ return self._n_dofs
+
+ @property
+ def pos_bounds(self):
+ return self._pos_bounds
+
+ @property
+ def vel_bounds(self):
+ return self._vel_bounds
+
+ @property
+ def is_hardware(self):
+ return self._is_hardware
+
+ @property
+ def device_name(self):
+ return self._device_name
+
+ @property
+ def calibration_path(self):
+ return self._calibration_path
+
+ @property
+ def overlay(self):
+ return self._overlay
+
+ @property
+ def has_obj(self):
+ return self._n_obj > 0
+
+ @property
+ def calibration_mode(self):
+ return self._calibration_mode
+
+ @property
+ def observation_cache_maxsize(self):
+ return self._observation_cache_maxsize
+
+ @property
+ def observation_cache(self):
+ return self._observation_cache
+
+
+ def clip_positions(self, positions):
+ """Clips the given joint positions to the position bounds.
+
+ Args:
+ positions: The joint positions.
+
+ Returns:
+ The bounded joint positions.
+ """
+ if self.pos_bounds is None:
+ return positions
+ assert len(positions) == self.n_jnt or len(positions) == self.n_dofs
+ pos_bounds = self.pos_bounds[:len(positions)]
+ return np.clip(positions, pos_bounds[:, 0], pos_bounds[:, 1])
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/__init__.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..528f3447d280872e394915e47bbb9fe8c1dd87a7
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/__init__.py
@@ -0,0 +1,24 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from gym.envs.registration import register
+
+# Relax the robot
+register(
+ id='kitchen_relax-v1',
+ entry_point='adept_envs.franka.kitchen_multitask_v0:KitchenTaskRelaxV1',
+ max_episode_steps=280,
+)
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/assets/franka_kitchen_jntpos_act_ab.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/assets/franka_kitchen_jntpos_act_ab.xml
new file mode 100644
index 0000000000000000000000000000000000000000..344138051430c24925e3558a38016c3b59fd06aa
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/assets/franka_kitchen_jntpos_act_ab.xml
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/kitchen_multitask_v0.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/kitchen_multitask_v0.py
new file mode 100644
index 0000000000000000000000000000000000000000..30bdd331af1db8be963716d59ff1fdc3d3be176e
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/kitchen_multitask_v0.py
@@ -0,0 +1,221 @@
+""" Kitchen environment for long horizon manipulation """
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import numpy as np
+from adept_envs import robot_env
+from adept_envs.utils.configurable import configurable
+from gym import spaces
+from dm_control.mujoco import engine
+
+@configurable(pickleable=True)
+class KitchenV0(robot_env.RobotEnv):
+
+ CALIBRATION_PATHS = {
+ 'default':
+ os.path.join(os.path.dirname(__file__), 'robot/franka_config.xml')
+ }
+ # Converted to velocity actuation
+ ROBOTS = {'robot': 'adept_envs.franka.robot.franka_robot:Robot_VelAct'}
+ MODEl = os.path.join(
+ os.path.dirname(__file__),
+ '../franka/assets/franka_kitchen_jntpos_act_ab.xml')
+ N_DOF_ROBOT = 9
+ N_DOF_OBJECT = 21
+
+ def __init__(self,
+ robot_params={}, frame_skip=40,
+ use_abs_action=False):
+ self.goal_concat = True
+ self.obs_dict = {}
+ self.robot_noise_ratio = 0.1 # 10% as per robot_config specs
+ self.goal = np.zeros((30,))
+ self.use_abs_action = use_abs_action
+ if use_abs_action:
+ self.ROBOTS = {'robot': 'adept_envs.franka.robot.franka_robot:Robot_PosAct'}
+
+ super().__init__(
+ self.MODEl,
+ robot=self.make_robot(
+ n_jnt=self.N_DOF_ROBOT, #root+robot_jnts
+ n_obj=self.N_DOF_OBJECT,
+ **robot_params),
+ frame_skip=frame_skip,
+ camera_settings=dict(
+ distance=4.5,
+ azimuth=-66,
+ elevation=-65,
+ ),
+ )
+ self.init_qpos = self.sim.model.key_qpos[0].copy()
+
+ # For the microwave kettle slide hinge
+ self.init_qpos = np.array([ 1.48388023e-01, -1.76848573e+00, 1.84390296e+00, -2.47685760e+00,
+ 2.60252026e-01, 7.12533105e-01, 1.59515394e+00, 4.79267505e-02,
+ 3.71350919e-02, -2.66279850e-04, -5.18043486e-05, 3.12877220e-05,
+ -4.51199853e-05, -3.90842156e-06, -4.22629655e-05, 6.28065475e-05,
+ 4.04984708e-05, 4.62730939e-04, -2.26906415e-04, -4.65501369e-04,
+ -6.44129196e-03, -1.77048263e-03, 1.08009684e-03, -2.69397440e-01,
+ 3.50383255e-01, 1.61944683e+00, 1.00618764e+00, 4.06395120e-03,
+ -6.62095997e-03, -2.68278933e-04])
+
+ self.init_qvel = self.sim.model.key_qvel[0].copy()
+
+ self.act_mid = np.zeros(self.N_DOF_ROBOT)
+ self.act_amp = 2.0 * np.ones(self.N_DOF_ROBOT)
+
+ act_lower = -1*np.ones((self.N_DOF_ROBOT,))
+ act_upper = 1*np.ones((self.N_DOF_ROBOT,))
+ if use_abs_action:
+ act_lower = act_lower * 8.
+ act_upper = act_upper * 8.
+ self.act_amp = np.ones(self.N_DOF_ROBOT)
+
+ self.action_space = spaces.Box(act_lower, act_upper)
+
+ obs_upper = 8. * np.ones(self.obs_dim)
+ obs_lower = -obs_upper
+ self.observation_space = spaces.Box(obs_lower, obs_upper)
+
+ def _get_reward_n_score(self, obs_dict):
+ raise NotImplementedError()
+
+ def step(self, a, b=None):
+ if not self.use_abs_action:
+ a = np.clip(a, -1.0, 1.0)
+
+ if not self.initializing:
+ a = self.act_mid + a * self.act_amp # mean center and scale
+ else:
+ self.goal = self._get_task_goal() # update goal if init
+
+ self.robot.step(
+ self, a, step_duration=self.skip * self.model.opt.timestep)
+
+ # observations
+ obs = self._get_obs()
+
+ #rewards
+ reward_dict, score = self._get_reward_n_score(self.obs_dict)
+
+ # termination
+ done = False
+
+ # finalize step
+ env_info = {
+ 'time': self.obs_dict['t'],
+ 'obs_dict': self.obs_dict,
+ 'rewards': reward_dict,
+ 'score': score,
+ # don't render every frame
+ # 'images': np.asarray(self.render(mode='rgb_array'))
+ }
+ # self.render()
+ return obs, reward_dict['r_total'], done, env_info
+
+ def _get_obs(self):
+ t, qp, qv, obj_qp, obj_qv = self.robot.get_obs(
+ self, robot_noise_ratio=self.robot_noise_ratio)
+
+ self.obs_dict = {}
+ self.obs_dict['t'] = t
+ self.obs_dict['qp'] = qp
+ self.obs_dict['qv'] = qv
+ self.obs_dict['obj_qp'] = obj_qp
+ self.obs_dict['obj_qv'] = obj_qv
+ self.obs_dict['goal'] = self.goal
+ if self.goal_concat:
+ return np.concatenate([self.obs_dict['qp'], self.obs_dict['obj_qp'], self.obs_dict['goal']])
+
+ def reset_model(self):
+ reset_pos = self.init_qpos[:].copy()
+ reset_vel = self.init_qvel[:].copy()
+ self.robot.reset(self, reset_pos, reset_vel)
+ self.sim.forward()
+ self.goal = self._get_task_goal() #sample a new goal on reset
+ return self._get_obs()
+
+ def evaluate_success(self, paths):
+ # score
+ mean_score_per_rollout = np.zeros(shape=len(paths))
+ for idx, path in enumerate(paths):
+ mean_score_per_rollout[idx] = np.mean(path['env_infos']['score'])
+ mean_score = np.mean(mean_score_per_rollout)
+
+ # success percentage
+ num_success = 0
+ num_paths = len(paths)
+ for path in paths:
+ num_success += bool(path['env_infos']['rewards']['bonus'][-1])
+ success_percentage = num_success * 100.0 / num_paths
+
+ # fuse results
+ return np.sign(mean_score) * (
+ 1e6 * round(success_percentage, 2) + abs(mean_score))
+
+ def close_env(self):
+ self.robot.close()
+
+ def set_goal(self, goal):
+ self.goal = goal
+
+ def _get_task_goal(self):
+ return self.goal
+
+ # Only include goal
+ @property
+ def goal_space(self):
+ len_obs = self.observation_space.low.shape[0]
+ env_lim = np.abs(self.observation_space.low[0])
+ return spaces.Box(low=-env_lim, high=env_lim, shape=(len_obs//2,))
+
+ def convert_to_active_observation(self, observation):
+ return observation
+
+class KitchenTaskRelaxV1(KitchenV0):
+ """Kitchen environment with proper camera and goal setup"""
+
+ def __init__(self, use_abs_action=False):
+ super(KitchenTaskRelaxV1, self).__init__(
+ use_abs_action=use_abs_action)
+
+ def _get_reward_n_score(self, obs_dict):
+ reward_dict = {}
+ reward_dict['true_reward'] = 0.
+ reward_dict['bonus'] = 0.
+ reward_dict['r_total'] = 0.
+ score = 0.
+ return reward_dict, score
+
+ def render(self, mode='human', width=1280, height=720, custom=True, **kwargs):
+ if custom:
+ camera = engine.MovableCamera(self.sim, height, width)
+ if 'distance' not in kwargs:
+ kwargs['distance'] = 2.2
+ if 'lookat' not in kwargs:
+ kwargs['lookat'] = [-0.2, .5, 2.]
+ if 'azimuth' not in kwargs:
+ kwargs['azimuth'] = 70
+ if 'elevation' not in kwargs:
+ kwargs['elevation'] = -35
+ camera.set_pose(**kwargs)
+ img = camera.render()
+ return img
+ else:
+ return super(KitchenTaskRelaxV1, self).render(
+ mode=mode, width=width, height=height, **kwargs)
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_config.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_config.xml
new file mode 100644
index 0000000000000000000000000000000000000000..aeb4f49c430ece4aa5f5e625461452ee55657084
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_config.xml
@@ -0,0 +1,59 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_robot.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..3c6cfc3c2669c778c6cb1faf15d94d41fbda8ff6
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/franka/robot/franka_robot.py
@@ -0,0 +1,264 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os, getpass
+import numpy as np
+from termcolor import cprint
+import time
+import copy
+import click
+
+from adept_envs import base_robot
+from adept_envs.utils.config import (get_config_root_node, read_config_from_node)
+
+# observations structure
+from collections import namedtuple
+observation = namedtuple('observation', ['time', 'qpos_robot', 'qvel_robot', 'qpos_object', 'qvel_object'])
+
+
+
+franka_interface = ''
+
+class Robot(base_robot.BaseRobot):
+
+ """
+ Abstracts away the differences between the robot_simulation and robot_hardware
+
+ """
+
+ def __init__(self, *args, **kwargs):
+ super(Robot, self).__init__(*args, **kwargs)
+ global franka_interface
+
+ # Read robot configurations
+ self._read_specs_from_config(robot_configs=self.calibration_path)
+
+
+ # Robot: Handware
+ if self.is_hardware:
+ if franka_interface == '':
+ raise NotImplementedError()
+ from handware.franka import franka
+
+ # initialize franka
+ self.franka_interface = franka()
+ franka_interface = self.franka_interface
+ cprint("Initializing %s Hardware (Status:%d)" % (self.robot_name, self.franka.okay(self.robot_hardware_dof)), 'white', 'on_grey')
+ else:
+ self.franka_interface = franka_interface
+ cprint("Reusing previours Franka session", 'white', 'on_grey')
+
+ # Robot: Simulation
+ else:
+ self.robot_name = "Franka"
+ cprint("Initializing %s sim" % self.robot_name, 'white', 'on_grey')
+
+ # Robot's time
+ self.time_start = time.time()
+ self.time = time.time()-self.time_start
+ self.time_render = -1 # time of rendering
+
+
+ # read specs from the calibration file
+ def _read_specs_from_config(self, robot_configs):
+ root, root_name = get_config_root_node(config_file_name=robot_configs)
+ self.robot_name = root_name[0]
+ self.robot_mode = np.zeros(self.n_dofs, dtype=int)
+ self.robot_mj_dof = np.zeros(self.n_dofs, dtype=int)
+ self.robot_hardware_dof = np.zeros(self.n_dofs, dtype=int)
+ self.robot_scale = np.zeros(self.n_dofs, dtype=float)
+ self.robot_offset = np.zeros(self.n_dofs, dtype=float)
+ self.robot_pos_bound = np.zeros([self.n_dofs, 2], dtype=float)
+ self.robot_vel_bound = np.zeros([self.n_dofs, 2], dtype=float)
+ self.robot_pos_noise_amp = np.zeros(self.n_dofs, dtype=float)
+ self.robot_vel_noise_amp = np.zeros(self.n_dofs, dtype=float)
+
+ print("Reading configurations for %s" % self.robot_name)
+ for i in range(self.n_dofs):
+ self.robot_mode[i] = read_config_from_node(root, "qpos"+str(i), "mode", int)
+ self.robot_mj_dof[i] = read_config_from_node(root, "qpos"+str(i), "mj_dof", int)
+ self.robot_hardware_dof[i] = read_config_from_node(root, "qpos"+str(i), "hardware_dof", int)
+ self.robot_scale[i] = read_config_from_node(root, "qpos"+str(i), "scale", float)
+ self.robot_offset[i] = read_config_from_node(root, "qpos"+str(i), "offset", float)
+ self.robot_pos_bound[i] = read_config_from_node(root, "qpos"+str(i), "pos_bound", float)
+ self.robot_vel_bound[i] = read_config_from_node(root, "qpos"+str(i), "vel_bound", float)
+ self.robot_pos_noise_amp[i] = read_config_from_node(root, "qpos"+str(i), "pos_noise_amp", float)
+ self.robot_vel_noise_amp[i] = read_config_from_node(root, "qpos"+str(i), "vel_noise_amp", float)
+
+
+ # convert to hardware space
+ def _de_calib(self, qp_mj, qv_mj=None):
+ qp_ad = (qp_mj-self.robot_offset)/self.robot_scale
+ if qv_mj is not None:
+ qv_ad = qv_mj/self.robot_scale
+ return qp_ad, qv_ad
+ else:
+ return qp_ad
+
+ # convert to mujoco space
+ def _calib(self, qp_ad, qv_ad):
+ qp_mj = qp_ad* self.robot_scale + self.robot_offset
+ qv_mj = qv_ad* self.robot_scale
+ return qp_mj, qv_mj
+
+
+ # refresh the observation cache
+ def _observation_cache_refresh(self, env):
+ for _ in range(self.observation_cache_maxsize):
+ self.get_obs(env, sim_mimic_hardware=False)
+
+ # get past observation
+ def get_obs_from_cache(self, env, index=-1):
+ assert (index>=0 and index=-self.observation_cache_maxsize), \
+ "cache index out of bound. (cache size is %2d)"%self.observation_cache_maxsize
+ obs = self.observation_cache[index]
+ if self.has_obj:
+ return obs.time, obs.qpos_robot, obs.qvel_robot, obs.qpos_object, obs.qvel_object
+ else:
+ return obs.time, obs.qpos_robot, obs.qvel_robot
+
+
+ # get observation
+ def get_obs(self, env, robot_noise_ratio=1, object_noise_ratio=1, sim_mimic_hardware=True):
+ if self.is_hardware:
+ raise NotImplementedError()
+
+ else:
+ #Gather simulated observation
+ qp = env.sim.data.qpos[:self.n_jnt].copy()
+ qv = env.sim.data.qvel[:self.n_jnt].copy()
+ if self.has_obj:
+ qp_obj = env.sim.data.qpos[-self.n_obj:].copy()
+ qv_obj = env.sim.data.qvel[-self.n_obj:].copy()
+ else:
+ qp_obj = None
+ qv_obj = None
+ self.time = env.sim.data.time
+
+ # Simulate observation noise
+ if not env.initializing:
+ qp += robot_noise_ratio*self.robot_pos_noise_amp[:self.n_jnt]*env.np_random.uniform(low=-1., high=1., size=self.n_jnt)
+ qv += robot_noise_ratio*self.robot_vel_noise_amp[:self.n_jnt]*env.np_random.uniform(low=-1., high=1., size=self.n_jnt)
+ if self.has_obj:
+ qp_obj += robot_noise_ratio*self.robot_pos_noise_amp[-self.n_obj:]*env.np_random.uniform(low=-1., high=1., size=self.n_obj)
+ qv_obj += robot_noise_ratio*self.robot_vel_noise_amp[-self.n_obj:]*env.np_random.uniform(low=-1., high=1., size=self.n_obj)
+
+ # cache observations
+ obs = observation(time=self.time, qpos_robot=qp, qvel_robot=qv, qpos_object=qp_obj, qvel_object=qv_obj)
+ self.observation_cache.append(obs)
+
+ if self.has_obj:
+ return obs.time, obs.qpos_robot, obs.qvel_robot, obs.qpos_object, obs.qvel_object
+ else:
+ return obs.time, obs.qpos_robot, obs.qvel_robot
+
+
+ # enforce position specs.
+ def ctrl_position_limits(self, ctrl_position):
+ ctrl_feasible_position = np.clip(ctrl_position, self.robot_pos_bound[:self.n_jnt, 0], self.robot_pos_bound[:self.n_jnt, 1])
+ return ctrl_feasible_position
+
+
+ # step the robot env
+ def step(self, env, ctrl_desired, step_duration, sim_override=False):
+
+ # Populate observation cache during startup
+ if env.initializing:
+ self._observation_cache_refresh(env)
+
+ # enforce velocity limits
+ ctrl_feasible = self.ctrl_velocity_limits(ctrl_desired, step_duration)
+
+ # enforce position limits
+ ctrl_feasible = self.ctrl_position_limits(ctrl_feasible)
+
+ # Send controls to the robot
+ if self.is_hardware and (not sim_override):
+ raise NotImplementedError()
+ else:
+ env.do_simulation(ctrl_feasible, int(step_duration/env.sim.model.opt.timestep)) # render is folded in here
+
+ # Update current robot state on the overlay
+ if self.overlay:
+ env.sim.data.qpos[self.n_jnt:2*self.n_jnt] = env.desired_pose.copy()
+ env.sim.forward()
+
+ # synchronize time
+ if self.is_hardware:
+ time_now = (time.time()-self.time_start)
+ time_left_in_step = step_duration - (time_now-self.time)
+ if(time_left_in_step>0.0001):
+ time.sleep(time_left_in_step)
+ return 1
+
+
+ def reset(self, env, reset_pose, reset_vel, overlay_mimic_reset_pose=True, sim_override=False):
+ reset_pose = self.clip_positions(reset_pose)
+
+ if self.is_hardware:
+ raise NotImplementedError()
+ else:
+ env.sim.reset()
+ env.sim.data.qpos[:self.n_jnt] = reset_pose[:self.n_jnt].copy()
+ env.sim.data.qvel[:self.n_jnt] = reset_vel[:self.n_jnt].copy()
+ if self.has_obj:
+ env.sim.data.qpos[-self.n_obj:] = reset_pose[-self.n_obj:].copy()
+ env.sim.data.qvel[-self.n_obj:] = reset_vel[-self.n_obj:].copy()
+ env.sim.forward()
+
+ if self.overlay:
+ env.sim.data.qpos[self.n_jnt:2*self.n_jnt] = env.desired_pose[:self.n_jnt].copy()
+ env.sim.forward()
+
+ # refresh observation cache before exit
+ self._observation_cache_refresh(env)
+
+
+ def close(self):
+ if self.is_hardware:
+ cprint("Closing Franka hardware... ", 'white', 'on_grey', end='', flush=True)
+ status = 0
+ raise NotImplementedError()
+ cprint("Closed (Status: {})".format(status), 'white', 'on_grey', flush=True)
+ else:
+ cprint("Closing Franka sim", 'white', 'on_grey', flush=True)
+
+
+class Robot_PosAct(Robot):
+
+ # enforce velocity sepcs.
+ # ALERT: This depends on previous observation. This is not ideal as it breaks MDP addumptions. Be careful
+ def ctrl_velocity_limits(self, ctrl_position, step_duration):
+ last_obs = self.observation_cache[-1]
+ ctrl_desired_vel = (ctrl_position-last_obs.qpos_robot[:self.n_jnt])/step_duration
+
+ ctrl_feasible_vel = np.clip(ctrl_desired_vel, self.robot_vel_bound[:self.n_jnt, 0], self.robot_vel_bound[:self.n_jnt, 1])
+ ctrl_feasible_position = last_obs.qpos_robot[:self.n_jnt] + ctrl_feasible_vel*step_duration
+ return ctrl_feasible_position
+
+
+class Robot_VelAct(Robot):
+
+ # enforce velocity sepcs.
+ # ALERT: This depends on previous observation. This is not ideal as it breaks MDP addumptions. Be careful
+ def ctrl_velocity_limits(self, ctrl_velocity, step_duration):
+ last_obs = self.observation_cache[-1]
+
+ ctrl_feasible_vel = np.clip(ctrl_velocity, self.robot_vel_bound[:self.n_jnt, 0], self.robot_vel_bound[:self.n_jnt, 1])
+ ctrl_feasible_position = last_obs.qpos_robot[:self.n_jnt] + ctrl_feasible_vel*step_duration
+ return ctrl_feasible_position
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/mujoco_env.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/mujoco_env.py
new file mode 100644
index 0000000000000000000000000000000000000000..e8cd9ec8c4b9e34ada17861e6fc8cb78d13618f7
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/mujoco_env.py
@@ -0,0 +1,202 @@
+"""Base environment for MuJoCo-based environments."""
+
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import collections
+import os
+import time
+from typing import Dict, Optional
+
+import gym
+from gym import spaces
+from gym.utils import seeding
+import numpy as np
+
+from adept_envs.simulation.sim_robot import MujocoSimRobot, RenderMode
+
+DEFAULT_RENDER_SIZE = 480
+
+USE_DM_CONTROL = True
+
+
+class MujocoEnv(gym.Env):
+ """Superclass for all MuJoCo environments."""
+
+ def __init__(self,
+ model_path: str,
+ frame_skip: int,
+ camera_settings: Optional[Dict] = None,
+ use_dm_backend: Optional[bool] = None,
+ ):
+ """Initializes a new MuJoCo environment.
+
+ Args:
+ model_path: The path to the MuJoCo XML file.
+ frame_skip: The number of simulation steps per environment step. On
+ hardware this influences the duration of each environment step.
+ camera_settings: Settings to initialize the simulation camera. This
+ can contain the keys `distance`, `azimuth`, and `elevation`.
+ use_dm_backend: A boolean to switch between mujoco-py and dm_control.
+ """
+ self._seed()
+ if not os.path.isfile(model_path):
+ raise IOError(
+ '[MujocoEnv]: Model path does not exist: {}'.format(model_path))
+ self.frame_skip = frame_skip
+
+ self.sim_robot = MujocoSimRobot(
+ model_path,
+ use_dm_backend=use_dm_backend or USE_DM_CONTROL,
+ camera_settings=camera_settings)
+ self.sim = self.sim_robot.sim
+ self.model = self.sim_robot.model
+ self.data = self.sim_robot.data
+
+ self.metadata = {
+ 'render.modes': ['human', 'rgb_array', 'depth_array'],
+ 'video.frames_per_second': int(np.round(1.0 / self.dt))
+ }
+ self.mujoco_render_frames = False
+
+ self.init_qpos = self.data.qpos.ravel().copy()
+ self.init_qvel = self.data.qvel.ravel().copy()
+ observation, _reward, done, _info = self.step(np.zeros(self.model.nu))
+ assert not done
+
+ bounds = self.model.actuator_ctrlrange.copy()
+ act_upper = bounds[:, 1]
+ act_lower = bounds[:, 0]
+
+ # Define the action and observation spaces.
+ # HACK: MJRL is still using gym 0.9.x so we can't provide a dtype.
+ try:
+ self.action_space = spaces.Box(
+ act_lower, act_upper, dtype=np.float32)
+ if isinstance(observation, collections.Mapping):
+ self.observation_space = spaces.Dict({
+ k: spaces.Box(-np.inf, np.inf, shape=v.shape, dtype=np.float32) for k, v in observation.items()})
+ else:
+ self.obs_dim = np.sum([o.size for o in observation]) if type(observation) is tuple else observation.size
+ self.observation_space = spaces.Box(
+ -np.inf, np.inf, observation.shape, dtype=np.float32)
+
+ except TypeError:
+ # Fallback case for gym 0.9.x
+ self.action_space = spaces.Box(act_lower, act_upper)
+ assert not isinstance(observation, collections.Mapping), 'gym 0.9.x does not support dictionary observation.'
+ self.obs_dim = np.sum([o.size for o in observation]) if type(observation) is tuple else observation.size
+ self.observation_space = spaces.Box(
+ -np.inf, np.inf, observation.shape)
+
+ def seed(self, seed=None): # Compatibility with new gym
+ return self._seed(seed)
+
+ def _seed(self, seed=None):
+ self.np_random, seed = seeding.np_random(seed)
+ return [seed]
+
+ # methods to override:
+ # ----------------------------
+
+ def reset_model(self):
+ """Reset the robot degrees of freedom (qpos and qvel).
+
+ Implement this in each subclass.
+ """
+ raise NotImplementedError
+
+ # -----------------------------
+
+ def reset(self): # compatibility with new gym
+ return self._reset()
+
+ def _reset(self):
+ self.sim.reset()
+ self.sim.forward()
+ ob = self.reset_model()
+ return ob
+
+ def set_state(self, qpos, qvel):
+ assert qpos.shape == (self.model.nq,) and qvel.shape == (self.model.nv,)
+ # we are directly manipulating mujoco state here
+ data = self.sim.data # MjData
+ for i in range(self.model.nq):
+ data.qpos[i] = qpos[i]
+ for i in range(self.model.nv):
+ data.qvel[i] = qvel[i]
+ # state = np.concatenate([self.data.qpos, self.data.qvel, self.data.act])
+ # self.sim.set_state(state)
+ self.sim.forward()
+
+ @property
+ def dt(self):
+ return self.model.opt.timestep * self.frame_skip
+
+ def do_simulation(self, ctrl, n_frames):
+ for i in range(self.model.nu):
+ self.sim.data.ctrl[i] = ctrl[i]
+
+ for _ in range(n_frames):
+ self.sim.step()
+
+ # TODO(michaelahn): Remove this; render should be called separately.
+ if self.mujoco_render_frames is True:
+ self.mj_render()
+
+ def render(self,
+ mode='human',
+ width=DEFAULT_RENDER_SIZE,
+ height=DEFAULT_RENDER_SIZE,
+ camera_id=-1):
+ """Renders the environment.
+
+ Args:
+ mode: The type of rendering to use.
+ - 'human': Renders to a graphical window.
+ - 'rgb_array': Returns the RGB image as an np.ndarray.
+ - 'depth_array': Returns the depth image as an np.ndarray.
+ width: The width of the rendered image. This only affects offscreen
+ rendering.
+ height: The height of the rendered image. This only affects
+ offscreen rendering.
+ camera_id: The ID of the camera to use. By default, this is the free
+ camera. If specified, only affects offscreen rendering.
+ """
+ if mode == 'human':
+ self.sim_robot.renderer.render_to_window()
+ elif mode == 'rgb_array':
+ assert width and height
+ return self.sim_robot.renderer.render_offscreen(
+ width, height, mode=RenderMode.RGB, camera_id=camera_id)
+ elif mode == 'depth_array':
+ assert width and height
+ return self.sim_robot.renderer.render_offscreen(
+ width, height, mode=RenderMode.DEPTH, camera_id=camera_id)
+ else:
+ raise NotImplementedError(mode)
+
+ def close(self):
+ self.sim_robot.close()
+
+ def mj_render(self):
+ """Backwards compatibility with MJRL."""
+ self.render(mode='human')
+
+ def state_vector(self):
+ state = self.sim.get_state()
+ return np.concatenate([state.qpos.flat, state.qvel.flat])
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/robot_env.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/robot_env.py
new file mode 100644
index 0000000000000000000000000000000000000000..c50c2ee5313ce7173056183e79e5000c6724db1b
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/robot_env.py
@@ -0,0 +1,166 @@
+"""Base class for robotics environments."""
+
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import importlib
+import inspect
+import os
+from typing import Dict, Optional
+
+import numpy as np
+
+
+from adept_envs import mujoco_env
+from adept_envs.base_robot import BaseRobot
+from adept_envs.utils.configurable import import_class_from_path
+from adept_envs.utils.constants import MODELS_PATH
+
+
+class RobotEnv(mujoco_env.MujocoEnv):
+ """Base environment for all adept robots."""
+
+ # Mapping of robot name to fully qualified class path.
+ # e.g. 'robot': 'adept_envs.dclaw.robot.Robot'
+ # Subclasses should override this to specify the Robot classes they support.
+ ROBOTS = {}
+
+ # Mapping of device path to the calibration file to use. If the device path
+ # is not found, the 'default' key is used.
+ # This can be overridden by subclasses.
+ CALIBRATION_PATHS = {}
+
+ def __init__(self,
+ model_path: str,
+ robot: BaseRobot,
+ frame_skip: int,
+ camera_settings: Optional[Dict] = None):
+ """Initializes a robotics environment.
+
+ Args:
+ model_path: The path to the model to run. Relative paths will be
+ interpreted as relative to the 'adept_models' folder.
+ robot: The Robot object to use.
+ frame_skip: The number of simulation steps per environment step. On
+ hardware this influences the duration of each environment step.
+ camera_settings: Settings to initialize the simulation camera. This
+ can contain the keys `distance`, `azimuth`, and `elevation`.
+ """
+ self._robot = robot
+
+ # Initial pose for first step.
+ self.desired_pose = np.zeros(self.n_jnt)
+
+ if not model_path.startswith('/'):
+ model_path = os.path.abspath(os.path.join(MODELS_PATH, model_path))
+
+ self.remote_viz = None
+
+ try:
+ from adept_envs.utils.remote_viz import RemoteViz
+ self.remote_viz = RemoteViz(model_path)
+ except ImportError:
+ pass
+
+
+ self._initializing = True
+ super(RobotEnv, self).__init__(
+ model_path, frame_skip, camera_settings=camera_settings)
+ self._initializing = False
+
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def n_jnt(self):
+ return self._robot.n_jnt
+
+ @property
+ def n_obj(self):
+ return self._robot.n_obj
+
+ @property
+ def skip(self):
+ """Alias for frame_skip. Needed for MJRL."""
+ return self.frame_skip
+
+ @property
+ def initializing(self):
+ return self._initializing
+
+ def close_env(self):
+ if self._robot is not None:
+ self._robot.close()
+
+ def make_robot(self,
+ n_jnt,
+ n_obj=0,
+ is_hardware=False,
+ device_name=None,
+ legacy=False,
+ **kwargs):
+ """Creates a new robot for the environment.
+
+ Args:
+ n_jnt: The number of joints in the robot.
+ n_obj: The number of object joints in the robot environment.
+ is_hardware: Whether to run on hardware or not.
+ device_name: The device path for the robot hardware.
+ legacy: If true, runs using direct dynamixel communication rather
+ than DDS.
+ kwargs: See BaseRobot for other parameters.
+
+ Returns:
+ A Robot object.
+ """
+ if not self.ROBOTS:
+ raise NotImplementedError('Subclasses must override ROBOTS.')
+
+ if is_hardware and not device_name:
+ raise ValueError('Must provide device name if running on hardware.')
+
+ robot_name = 'dds_robot' if not legacy and is_hardware else 'robot'
+ if robot_name not in self.ROBOTS:
+ raise KeyError("Unsupported robot '{}', available: {}".format(
+ robot_name, list(self.ROBOTS.keys())))
+
+ cls = import_class_from_path(self.ROBOTS[robot_name])
+
+ calibration_path = None
+ if self.CALIBRATION_PATHS:
+ if not device_name:
+ calibration_name = 'default'
+ elif device_name not in self.CALIBRATION_PATHS:
+ print('Device "{}" not in CALIBRATION_PATHS; using default.'
+ .format(device_name))
+ calibration_name = 'default'
+ else:
+ calibration_name = device_name
+
+ calibration_path = self.CALIBRATION_PATHS[calibration_name]
+ if not os.path.isfile(calibration_path):
+ raise OSError('Could not find calibration file at: {}'.format(
+ calibration_path))
+
+ return cls(
+ n_jnt,
+ n_obj,
+ is_hardware=is_hardware,
+ device_name=device_name,
+ calibration_path=calibration_path,
+ **kwargs)
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/module.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/module.py
new file mode 100644
index 0000000000000000000000000000000000000000..a1284c76c346062def8e7eb6d680169db776967c
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/module.py
@@ -0,0 +1,126 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Module for caching Python modules related to simulation."""
+
+import sys
+
+_MUJOCO_PY_MODULE = None
+
+_DM_MUJOCO_MODULE = None
+_DM_VIEWER_MODULE = None
+_DM_RENDER_MODULE = None
+
+_GLFW_MODULE = None
+
+
+def get_mujoco_py():
+ """Returns the mujoco_py module."""
+ global _MUJOCO_PY_MODULE
+ if _MUJOCO_PY_MODULE:
+ return _MUJOCO_PY_MODULE
+ try:
+ import mujoco_py
+ # Override the warning function.
+ from mujoco_py.builder import cymj
+ cymj.set_warning_callback(_mj_warning_fn)
+ except ImportError:
+ print(
+ 'Failed to import mujoco_py. Ensure that mujoco_py (using MuJoCo '
+ 'v1.50) is installed.',
+ file=sys.stderr)
+ sys.exit(1)
+ _MUJOCO_PY_MODULE = mujoco_py
+ return mujoco_py
+
+
+def get_mujoco_py_mjlib():
+ """Returns the mujoco_py mjlib module."""
+
+ class MjlibDelegate:
+ """Wrapper that forwards mjlib calls."""
+
+ def __init__(self, lib):
+ self._lib = lib
+
+ def __getattr__(self, name: str):
+ if name.startswith('mj'):
+ return getattr(self._lib, '_' + name)
+ raise AttributeError(name)
+
+ return MjlibDelegate(get_mujoco_py().cymj)
+
+
+def get_dm_mujoco():
+ """Returns the DM Control mujoco module."""
+ global _DM_MUJOCO_MODULE
+ if _DM_MUJOCO_MODULE:
+ return _DM_MUJOCO_MODULE
+ try:
+ from dm_control import mujoco
+ except ImportError:
+ print(
+ 'Failed to import dm_control.mujoco. Ensure that dm_control (using '
+ 'MuJoCo v2.00) is installed.',
+ file=sys.stderr)
+ sys.exit(1)
+ _DM_MUJOCO_MODULE = mujoco
+ return mujoco
+
+
+def get_dm_viewer():
+ """Returns the DM Control viewer module."""
+ global _DM_VIEWER_MODULE
+ if _DM_VIEWER_MODULE:
+ return _DM_VIEWER_MODULE
+ try:
+ from dm_control import viewer
+ except ImportError:
+ print(
+ 'Failed to import dm_control.viewer. Ensure that dm_control (using '
+ 'MuJoCo v2.00) is installed.',
+ file=sys.stderr)
+ sys.exit(1)
+ _DM_VIEWER_MODULE = viewer
+ return viewer
+
+
+def get_dm_render():
+ """Returns the DM Control render module."""
+ global _DM_RENDER_MODULE
+ if _DM_RENDER_MODULE:
+ return _DM_RENDER_MODULE
+ try:
+ try:
+ from dm_control import _render
+ render = _render
+ except ImportError:
+ print('Warning: DM Control is out of date.')
+ from dm_control import render
+ except ImportError:
+ print(
+ 'Failed to import dm_control.render. Ensure that dm_control (using '
+ 'MuJoCo v2.00) is installed.',
+ file=sys.stderr)
+ sys.exit(1)
+ _DM_RENDER_MODULE = render
+ return render
+
+
+def _mj_warning_fn(warn_data: bytes):
+ """Warning function override for mujoco_py."""
+ print('WARNING: Mujoco simulation is unstable (has NaNs): {}'.format(
+ warn_data.decode()))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/renderer.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/renderer.py
new file mode 100644
index 0000000000000000000000000000000000000000..28b9de294d3d612f15555ba2bbd9ebfab13ea1af
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/renderer.py
@@ -0,0 +1,293 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Module for viewing Physics objects in the DM Control viewer."""
+
+import abc
+import enum
+import sys
+from typing import Dict, Optional
+
+import numpy as np
+
+from adept_envs.simulation import module
+
+# Default window dimensions.
+DEFAULT_WINDOW_WIDTH = 1024
+DEFAULT_WINDOW_HEIGHT = 768
+
+DEFAULT_WINDOW_TITLE = 'MuJoCo Viewer'
+
+_MAX_RENDERBUFFER_SIZE = 2048
+
+
+class RenderMode(enum.Enum):
+ """Rendering modes for offscreen rendering."""
+ RGB = 0
+ DEPTH = 1
+ SEGMENTATION = 2
+
+
+class Renderer(abc.ABC):
+ """Base interface for rendering simulations."""
+
+ def __init__(self, camera_settings: Optional[Dict] = None):
+ self._camera_settings = camera_settings
+
+ @abc.abstractmethod
+ def close(self):
+ """Cleans up any resources being used by the renderer."""
+
+ @abc.abstractmethod
+ def render_to_window(self):
+ """Renders the simulation to a window."""
+
+ @abc.abstractmethod
+ def render_offscreen(self,
+ width: int,
+ height: int,
+ mode: RenderMode = RenderMode.RGB,
+ camera_id: int = -1) -> np.ndarray:
+ """Renders the camera view as a NumPy array of pixels.
+
+ Args:
+ width: The viewport width (pixels).
+ height: The viewport height (pixels).
+ mode: The rendering mode.
+ camera_id: The ID of the camera to render from. By default, uses
+ the free camera.
+
+ Returns:
+ A NumPy array of the pixels.
+ """
+
+ def _update_camera(self, camera):
+ """Updates the given camera to move to the initial settings."""
+ if not self._camera_settings:
+ return
+ distance = self._camera_settings.get('distance')
+ azimuth = self._camera_settings.get('azimuth')
+ elevation = self._camera_settings.get('elevation')
+ lookat = self._camera_settings.get('lookat')
+
+ if distance is not None:
+ camera.distance = distance
+ if azimuth is not None:
+ camera.azimuth = azimuth
+ if elevation is not None:
+ camera.elevation = elevation
+ if lookat is not None:
+ camera.lookat[:] = lookat
+
+
+class MjPyRenderer(Renderer):
+ """Class for rendering mujoco_py simulations."""
+
+ def __init__(self, sim, **kwargs):
+ assert isinstance(sim, module.get_mujoco_py().MjSim), \
+ 'MjPyRenderer takes a mujoco_py MjSim object.'
+ super().__init__(**kwargs)
+ self._sim = sim
+ self._onscreen_renderer = None
+ self._offscreen_renderer = None
+
+ def render_to_window(self):
+ """Renders the simulation to a window."""
+ if not self._onscreen_renderer:
+ self._onscreen_renderer = module.get_mujoco_py().MjViewer(self._sim)
+ self._update_camera(self._onscreen_renderer.cam)
+
+ self._onscreen_renderer.render()
+
+ def render_offscreen(self,
+ width: int,
+ height: int,
+ mode: RenderMode = RenderMode.RGB,
+ camera_id: int = -1) -> np.ndarray:
+ """Renders the camera view as a NumPy array of pixels.
+
+ Args:
+ width: The viewport width (pixels).
+ height: The viewport height (pixels).
+ mode: The rendering mode.
+ camera_id: The ID of the camera to render from. By default, uses
+ the free camera.
+
+ Returns:
+ A NumPy array of the pixels.
+ """
+ if not self._offscreen_renderer:
+ self._offscreen_renderer = module.get_mujoco_py() \
+ .MjRenderContextOffscreen(self._sim)
+
+ # Update the camera configuration for the free-camera.
+ if camera_id == -1:
+ self._update_camera(self._offscreen_renderer.cam)
+
+ self._offscreen_renderer.render(width, height, camera_id)
+ if mode == RenderMode.RGB:
+ data = self._offscreen_renderer.read_pixels(
+ width, height, depth=False)
+ # Original image is upside-down, so flip it
+ return data[::-1, :, :]
+ elif mode == RenderMode.DEPTH:
+ data = self._offscreen_renderer.read_pixels(
+ width, height, depth=True)[1]
+ # Original image is upside-down, so flip it
+ return data[::-1, :]
+ else:
+ raise NotImplementedError(mode)
+
+ def close(self):
+ """Cleans up any resources being used by the renderer."""
+
+
+class DMRenderer(Renderer):
+ """Class for rendering DM Control Physics objects."""
+
+ def __init__(self, physics, **kwargs):
+ assert isinstance(physics, module.get_dm_mujoco().Physics), \
+ 'DMRenderer takes a DM Control Physics object.'
+ super().__init__(**kwargs)
+ self._physics = physics
+ self._window = None
+
+ # Set the camera to lookat the center of the geoms. (mujoco_py does
+ # this automatically.
+ if 'lookat' not in self._camera_settings:
+ self._camera_settings['lookat'] = [
+ np.median(self._physics.data.geom_xpos[:, i]) for i in range(3)
+ ]
+
+ def render_to_window(self):
+ """Renders the Physics object to a window.
+
+ The window continuously renders the Physics in a separate thread.
+
+ This function is a no-op if the window was already created.
+ """
+ if not self._window:
+ self._window = DMRenderWindow()
+ self._window.load_model(self._physics)
+ self._update_camera(self._window.camera)
+ self._window.run_frame()
+
+ def render_offscreen(self,
+ width: int,
+ height: int,
+ mode: RenderMode = RenderMode.RGB,
+ camera_id: int = -1) -> np.ndarray:
+ """Renders the camera view as a NumPy array of pixels.
+
+ Args:
+ width: The viewport width (pixels).
+ height: The viewport height (pixels).
+ mode: The rendering mode.
+ camera_id: The ID of the camera to render from. By default, uses
+ the free camera.
+
+ Returns:
+ A NumPy array of the pixels.
+ """
+ mujoco = module.get_dm_mujoco()
+ # TODO(michaelahn): Consider caching the camera.
+ camera = mujoco.Camera(
+ physics=self._physics,
+ height=height,
+ width=width,
+ camera_id=camera_id)
+
+ # Update the camera configuration for the free-camera.
+ if camera_id == -1:
+ self._update_camera(
+ camera._render_camera, # pylint: disable=protected-access
+ )
+
+ image = camera.render(
+ depth=(mode == RenderMode.DEPTH),
+ segmentation=(mode == RenderMode.SEGMENTATION))
+ camera._scene.free() # pylint: disable=protected-access
+ return image
+
+ def close(self):
+ """Cleans up any resources being used by the renderer."""
+ if self._window:
+ self._window.close()
+ self._window = None
+
+
+class DMRenderWindow:
+ """Class that encapsulates a graphical window."""
+
+ def __init__(self,
+ width: int = DEFAULT_WINDOW_WIDTH,
+ height: int = DEFAULT_WINDOW_HEIGHT,
+ title: str = DEFAULT_WINDOW_TITLE):
+ """Creates a graphical render window.
+
+ Args:
+ width: The width of the window.
+ height: The height of the window.
+ title: The title of the window.
+ """
+ dmv = module.get_dm_viewer()
+ self._viewport = dmv.renderer.Viewport(width, height)
+ self._window = dmv.gui.RenderWindow(width, height, title)
+ self._viewer = dmv.viewer.Viewer(self._viewport, self._window.mouse,
+ self._window.keyboard)
+ self._draw_surface = None
+ self._renderer = dmv.renderer.NullRenderer()
+
+ @property
+ def camera(self):
+ return self._viewer._camera._camera
+
+ def close(self):
+ self._viewer.deinitialize()
+ self._renderer.release()
+ self._draw_surface.free()
+ self._window.close()
+
+ def load_model(self, physics):
+ """Loads the given Physics object to render."""
+ self._viewer.deinitialize()
+
+ self._draw_surface = module.get_dm_render().Renderer(
+ max_width=_MAX_RENDERBUFFER_SIZE, max_height=_MAX_RENDERBUFFER_SIZE)
+ self._renderer = module.get_dm_viewer().renderer.OffScreenRenderer(
+ physics.model, self._draw_surface)
+
+ self._viewer.initialize(physics, self._renderer, touchpad=False)
+
+ def run_frame(self):
+ """Renders one frame of the simulation.
+
+ NOTE: This is extremely slow at the moment.
+ """
+ glfw = module.get_dm_viewer().gui.glfw_gui.glfw
+ glfw_window = self._window._context.window
+ if glfw.window_should_close(glfw_window):
+ sys.exit(0)
+
+ self._viewport.set_size(*self._window.shape)
+ self._viewer.render()
+ pixels = self._renderer.pixels
+
+ with self._window._context.make_current() as ctx:
+ ctx.call(self._window._update_gui_on_render_thread, glfw_window,
+ pixels)
+ self._window._mouse.process_events()
+ self._window._keyboard.process_events()
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/sim_robot.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/sim_robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..195d524b801b95c9ef9ef5d7009e82a18b362659
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/simulation/sim_robot.py
@@ -0,0 +1,135 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Module for loading MuJoCo models."""
+
+import os
+from typing import Dict, Optional
+
+from adept_envs.simulation import module
+from adept_envs.simulation.renderer import DMRenderer, MjPyRenderer, RenderMode
+
+
+class MujocoSimRobot:
+ """Class that encapsulates a MuJoCo simulation.
+
+ This class exposes methods that are agnostic to the simulation backend.
+ Two backends are supported:
+ 1. mujoco_py - MuJoCo v1.50
+ 2. dm_control - MuJoCo v2.00
+ """
+
+ def __init__(self,
+ model_file: str,
+ use_dm_backend: bool = False,
+ camera_settings: Optional[Dict] = None):
+ """Initializes a new simulation.
+
+ Args:
+ model_file: The MuJoCo XML model file to load.
+ use_dm_backend: If True, uses DM Control's Physics (MuJoCo v2.0) as
+ the backend for the simulation. Otherwise, uses mujoco_py (MuJoCo
+ v1.5) as the backend.
+ camera_settings: Settings to initialize the renderer's camera. This
+ can contain the keys `distance`, `azimuth`, and `elevation`.
+ """
+ self._use_dm_backend = use_dm_backend
+
+ if not os.path.isfile(model_file):
+ raise ValueError(
+ '[MujocoSimRobot] Invalid model file path: {}'.format(
+ model_file))
+
+ if self._use_dm_backend:
+ dm_mujoco = module.get_dm_mujoco()
+ if model_file.endswith('.mjb'):
+ self.sim = dm_mujoco.Physics.from_binary_path(model_file)
+ else:
+ self.sim = dm_mujoco.Physics.from_xml_path(model_file)
+ self.model = self.sim.model
+ self._patch_mjlib_accessors(self.model, self.sim.data)
+ self.renderer = DMRenderer(
+ self.sim, camera_settings=camera_settings)
+ else: # Use mujoco_py
+ mujoco_py = module.get_mujoco_py()
+ self.model = mujoco_py.load_model_from_path(model_file)
+ self.sim = mujoco_py.MjSim(self.model)
+ self.renderer = MjPyRenderer(
+ self.sim, camera_settings=camera_settings)
+
+ self.data = self.sim.data
+
+ def close(self):
+ """Cleans up any resources being used by the simulation."""
+ self.renderer.close()
+
+ def save_binary(self, path: str):
+ """Saves the loaded model to a binary .mjb file."""
+ if os.path.exists(path):
+ raise ValueError(
+ '[MujocoSimRobot] Path already exists: {}'.format(path))
+ if not path.endswith('.mjb'):
+ path = path + '.mjb'
+ if self._use_dm_backend:
+ self.model.save_binary(path)
+ else:
+ with open(path, 'wb') as f:
+ f.write(self.model.get_mjb())
+
+ def get_mjlib(self):
+ """Returns an object that exposes the low-level MuJoCo API."""
+ if self._use_dm_backend:
+ return module.get_dm_mujoco().wrapper.mjbindings.mjlib
+ else:
+ return module.get_mujoco_py_mjlib()
+
+ def _patch_mjlib_accessors(self, model, data):
+ """Adds accessors to the DM Control objects to support mujoco_py API."""
+ assert self._use_dm_backend
+ mjlib = self.get_mjlib()
+
+ def name2id(type_name, name):
+ obj_id = mjlib.mj_name2id(model.ptr,
+ mjlib.mju_str2Type(type_name.encode()),
+ name.encode())
+ if obj_id < 0:
+ raise ValueError('No {} with name "{}" exists.'.format(
+ type_name, name))
+ return obj_id
+
+ if not hasattr(model, 'body_name2id'):
+ model.body_name2id = lambda name: name2id('body', name)
+
+ if not hasattr(model, 'geom_name2id'):
+ model.geom_name2id = lambda name: name2id('geom', name)
+
+ if not hasattr(model, 'site_name2id'):
+ model.site_name2id = lambda name: name2id('site', name)
+
+ if not hasattr(model, 'joint_name2id'):
+ model.joint_name2id = lambda name: name2id('joint', name)
+
+ if not hasattr(model, 'actuator_name2id'):
+ model.actuator_name2id = lambda name: name2id('actuator', name)
+
+ if not hasattr(model, 'camera_name2id'):
+ model.camera_name2id = lambda name: name2id('camera', name)
+
+ if not hasattr(data, 'body_xpos'):
+ data.body_xpos = data.xpos
+
+ if not hasattr(data, 'body_xquat'):
+ data.body_xquat = data.xquat
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/config.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/config.py
new file mode 100644
index 0000000000000000000000000000000000000000..6444bf7296a09620b28b23f28765b183003f5834
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/config.py
@@ -0,0 +1,99 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import numpy as np
+try:
+ import cElementTree as ET
+except ImportError:
+ try:
+ # Python 2.5 need to import a different module
+ import xml.etree.cElementTree as ET
+ except ImportError:
+ exit_err("Failed to import cElementTree from any known place")
+
+CONFIG_XML_DATA = """
+
+
+
+
+
+"""
+
+
+# Read config from root
+def read_config_from_node(root_node, parent_name, child_name, dtype=int):
+ # find parent
+ parent_node = root_node.find(parent_name)
+ if parent_node == None:
+ quit("Parent %s not found" % parent_name)
+
+ # get child data
+ child_data = parent_node.get(child_name)
+ if child_data == None:
+ quit("Child %s not found" % child_name)
+
+ config_val = np.array(child_data.split(), dtype=dtype)
+ return config_val
+
+
+# get config frlom file or string
+def get_config_root_node(config_file_name=None, config_file_data=None):
+ try:
+ # get root
+ if config_file_data is None:
+ config_file_content = open(config_file_name, "r")
+ config = ET.parse(config_file_content)
+ root_node = config.getroot()
+ else:
+ root_node = ET.fromstring(config_file_data)
+
+ # get root data
+ root_data = root_node.get('name')
+ root_name = np.array(root_data.split(), dtype=str)
+ except:
+ quit("ERROR: Unable to process config file %s" % config_file_name)
+
+ return root_node, root_name
+
+
+# Read config from config_file
+def read_config_from_xml(config_file_name, parent_name, child_name, dtype=int):
+ root_node, root_name = get_config_root_node(
+ config_file_name=config_file_name)
+ return read_config_from_node(root_node, parent_name, child_name, dtype)
+
+
+# tests
+if __name__ == '__main__':
+ print("Read config and parse -------------------------")
+ root, root_name = get_config_root_node(config_file_data=CONFIG_XML_DATA)
+ print("Root:name \t", root_name)
+ print("limit:low \t", read_config_from_node(root, "limits", "low", float))
+ print("limit:high \t", read_config_from_node(root, "limits", "high", float))
+ print("scale:joint \t", read_config_from_node(root, "scale", "joint",
+ float))
+ print("data:type \t", read_config_from_node(root, "data", "type", str))
+
+ # read straight from xml (dumb the XML data as duh.xml for this test)
+ root, root_name = get_config_root_node(config_file_name="duh.xml")
+ print("Read from xml --------------------------------")
+ print("limit:low \t", read_config_from_xml("duh.xml", "limits", "low",
+ float))
+ print("limit:high \t",
+ read_config_from_xml("duh.xml", "limits", "high", float))
+ print("scale:joint \t",
+ read_config_from_xml("duh.xml", "scale", "joint", float))
+ print("data:type \t", read_config_from_xml("duh.xml", "data", "type", str))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/configurable.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/configurable.py
new file mode 100644
index 0000000000000000000000000000000000000000..6685a5007b028c487cba5d0f5be96be297c17fa6
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/configurable.py
@@ -0,0 +1,163 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import importlib
+import inspect
+import os
+
+from gym.envs.registration import registry as gym_registry
+
+
+def import_class_from_path(class_path):
+ """Given 'path.to.module:object', imports and returns the object."""
+ module_path, class_name = class_path.split(":")
+ module = importlib.import_module(module_path)
+ return getattr(module, class_name)
+
+
+class ConfigCache(object):
+ """Configuration class to store constructor arguments.
+
+ This is used to store parameters to pass to Gym environments at init time.
+ """
+
+ def __init__(self):
+ self._configs = {}
+ self._default_config = {}
+
+ def set_default_config(self, config):
+ """Sets the default configuration used for all RobotEnv envs."""
+ self._default_config = dict(config)
+
+ def set_config(self, cls_or_env_id, config):
+ """Sets the configuration for the given environment within a context.
+
+ Args:
+ cls_or_env_id (Class | str): A class type or Gym environment ID to
+ configure.
+ config (dict): The configuration parameters.
+ """
+ config_key = self._get_config_key(cls_or_env_id)
+ self._configs[config_key] = dict(config)
+
+ def get_config(self, cls_or_env_id):
+ """Returns the configuration for the given env name.
+
+ Args:
+ cls_or_env_id (Class | str): A class type or Gym environment ID to
+ get the configuration of.
+ """
+ config_key = self._get_config_key(cls_or_env_id)
+ config = dict(self._default_config)
+ config.update(self._configs.get(config_key, {}))
+ return config
+
+ def clear_config(self, cls_or_env_id):
+ """Clears the configuration for the given ID."""
+ config_key = self._get_config_key(cls_or_env_id)
+ if config_key in self._configs:
+ del self._configs[config_key]
+
+ def _get_config_key(self, cls_or_env_id):
+ if inspect.isclass(cls_or_env_id):
+ return cls_or_env_id
+ env_id = cls_or_env_id
+ assert isinstance(env_id, str)
+ if env_id not in gym_registry.env_specs:
+ raise ValueError("Unregistered environment name {}.".format(env_id))
+ entry_point = gym_registry.env_specs[env_id]._entry_point
+ if callable(entry_point):
+ return entry_point
+ else:
+ return import_class_from_path(entry_point)
+
+
+# Global robot config.
+global_config = ConfigCache()
+
+
+def configurable(config_id=None, pickleable=False, config_cache=global_config):
+ """Class decorator to allow injection of constructor arguments.
+
+ This allows constructor arguments to be passed via ConfigCache.
+ Example usage:
+
+ @configurable()
+ class A:
+ def __init__(b=None, c=2, d='Wow'):
+ ...
+
+ global_config.set_config(A, {'b': 10, 'c': 20})
+ a = A() # b=10, c=20, d='Wow'
+ a = A(b=30) # b=30, c=20, d='Wow'
+
+ Args:
+ config_id: ID of the config to use. This defaults to the class type.
+ pickleable: Whether this class is pickleable. If true, causes the pickle
+ state to include the config and constructor arguments.
+ config_cache: The ConfigCache to use to read config data from. Uses
+ the global ConfigCache by default.
+ """
+ def cls_decorator(cls):
+ assert inspect.isclass(cls)
+
+ # Overwrite the class constructor to pass arguments from the config.
+ base_init = cls.__init__
+ def __init__(self, *args, **kwargs):
+
+ config = config_cache.get_config(config_id or type(self))
+ # Allow kwargs to override the config.
+ kwargs = {**config, **kwargs}
+
+ # print('Initializing {} with params: {}'.format(type(self).__name__,
+ # kwargs))
+
+ if pickleable:
+ self._pkl_env_args = args
+ self._pkl_env_kwargs = kwargs
+
+ base_init(self, *args, **kwargs)
+ cls.__init__ = __init__
+
+ # If the class is pickleable, overwrite the state methods to save
+ # the constructor arguments and config.
+ if pickleable:
+ # Use same pickle keys as gym.utils.ezpickle for backwards compat.
+ PKL_ARGS_KEY = '_ezpickle_args'
+ PKL_KWARGS_KEY = '_ezpickle_kwargs'
+
+ def __getstate__(self):
+ return {
+ PKL_ARGS_KEY: self._pkl_env_args,
+ PKL_KWARGS_KEY: self._pkl_env_kwargs,
+ }
+ cls.__getstate__ = __getstate__
+
+ def __setstate__(self, data):
+ saved_args = data[PKL_ARGS_KEY]
+ saved_kwargs = data[PKL_KWARGS_KEY]
+
+ # Override the saved state with the current config.
+ config = config_cache.get_config(config_id or type(self))
+ # Allow kwargs to override the config.
+ kwargs = {**saved_kwargs, **config}
+
+ inst = type(self)(*saved_args, **kwargs)
+ self.__dict__.update(inst.__dict__)
+ cls.__setstate__ = __setstate__
+
+ return cls
+ return cls_decorator
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/constants.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/constants.py
new file mode 100644
index 0000000000000000000000000000000000000000..9c63fb7885adf3bf8fc57b441d5b5a3e6ba5fa1d
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/constants.py
@@ -0,0 +1,23 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+ENVS_ROOT_PATH = os.path.abspath(os.path.join(
+ os.path.dirname(os.path.abspath(__file__)),
+ "../../"))
+
+MODELS_PATH = os.path.abspath(os.path.join(ENVS_ROOT_PATH, "../adept_models/"))
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/parse_demos.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/parse_demos.py
new file mode 100644
index 0000000000000000000000000000000000000000..01f9c36ca0131387766915fb417ef188ab2ae2e5
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/parse_demos.py
@@ -0,0 +1,221 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import click
+import glob
+import pickle
+import numpy as np
+from parse_mjl import parse_mjl_logs, viz_parsed_mjl_logs
+from mjrl.utils.gym_env import GymEnv
+import adept_envs
+import time as timer
+import skvideo.io
+import gym
+
+# headless renderer
+render_buffer = [] # rendering buffer
+
+
+def viewer(env,
+ mode='initialize',
+ filename='video',
+ frame_size=(640, 480),
+ camera_id=0,
+ render=None):
+ if render == 'onscreen':
+ env.mj_render()
+
+ elif render == 'offscreen':
+
+ global render_buffer
+ if mode == 'initialize':
+ render_buffer = []
+ mode = 'render'
+
+ if mode == 'render':
+ curr_frame = env.render(mode='rgb_array')
+ render_buffer.append(curr_frame)
+
+ if mode == 'save':
+ skvideo.io.vwrite(filename, np.asarray(render_buffer))
+ print("\noffscreen buffer saved", filename)
+
+ elif render == 'None':
+ pass
+
+ else:
+ print("unknown render: ", render)
+
+
+# view demos (physics ignored)
+def render_demos(env, data, filename='demo_rendering.mp4', render=None):
+ FPS = 30
+ render_skip = max(1, round(1. / \
+ (FPS * env.sim.model.opt.timestep * env.frame_skip)))
+ t0 = timer.time()
+
+ viewer(env, mode='initialize', render=render)
+ for i_frame in range(data['ctrl'].shape[0]):
+ env.sim.data.qpos[:] = data['qpos'][i_frame].copy()
+ env.sim.data.qvel[:] = data['qvel'][i_frame].copy()
+ env.sim.forward()
+ if i_frame % render_skip == 0:
+ viewer(env, mode='render', render=render)
+ print(i_frame, end=', ', flush=True)
+
+ viewer(env, mode='save', filename=filename, render=render)
+ print("time taken = %f" % (timer.time() - t0))
+
+
+# playback demos and get data(physics respected)
+def gather_training_data(env, data, filename='demo_playback.mp4', render=None):
+ env = env.env
+ FPS = 30
+ render_skip = max(1, round(1. / \
+ (FPS * env.sim.model.opt.timestep * env.frame_skip)))
+ t0 = timer.time()
+
+ # initialize
+ env.reset()
+ init_qpos = data['qpos'][0].copy()
+ init_qvel = data['qvel'][0].copy()
+ act_mid = env.act_mid
+ act_rng = env.act_amp
+
+ # prepare env
+ env.sim.data.qpos[:] = init_qpos
+ env.sim.data.qvel[:] = init_qvel
+ env.sim.forward()
+ viewer(env, mode='initialize', render=render)
+
+ # step the env and gather data
+ path_obs = None
+ for i_frame in range(data['ctrl'].shape[0] - 1):
+ # Reset every time step
+ # if i_frame % 1 == 0:
+ # qp = data['qpos'][i_frame].copy()
+ # qv = data['qvel'][i_frame].copy()
+ # env.sim.data.qpos[:] = qp
+ # env.sim.data.qvel[:] = qv
+ # env.sim.forward()
+
+ obs = env._get_obs()
+
+ # Construct the action
+ # ctrl = (data['qpos'][i_frame + 1][:9] - obs[:9]) / (env.skip * env.model.opt.timestep)
+ ctrl = (data['ctrl'][i_frame] - obs[:9])/(env.skip*env.model.opt.timestep)
+ act = (ctrl - act_mid) / act_rng
+ act = np.clip(act, -0.999, 0.999)
+ next_obs, reward, done, env_info = env.step(act)
+ if path_obs is None:
+ path_obs = obs
+ path_act = act
+ else:
+ path_obs = np.vstack((path_obs, obs))
+ path_act = np.vstack((path_act, act))
+
+ # render when needed to maintain FPS
+ if i_frame % render_skip == 0:
+ viewer(env, mode='render', render=render)
+ print(i_frame, end=', ', flush=True)
+
+ # finalize
+ if render:
+ viewer(env, mode='save', filename=filename, render=render)
+
+ t1 = timer.time()
+ print("time taken = %f" % (t1 - t0))
+
+ # note that are one step away from
+ return path_obs, path_act, init_qpos, init_qvel
+
+
+# MAIN =========================================================
+@click.command(help="parse tele-op demos")
+@click.option('--env', '-e', type=str, help='gym env name', required=True)
+@click.option(
+ '--demo_dir',
+ '-d',
+ type=str,
+ help='directory with tele-op logs',
+ required=True)
+@click.option(
+ '--skip',
+ '-s',
+ type=int,
+ help='number of frames to skip (1:no skip)',
+ default=1)
+@click.option('--graph', '-g', type=bool, help='plot logs', default=False)
+@click.option('--save_logs', '-l', type=bool, help='save logs', default=False)
+@click.option(
+ '--view', '-v', type=str, help='render/playback', default='render')
+@click.option(
+ '--render', '-r', type=str, help='onscreen/offscreen', default='onscreen')
+def main(env, demo_dir, skip, graph, save_logs, view, render):
+
+ gym_env = gym.make(env)
+ paths = []
+ print("Scanning demo_dir: " + demo_dir + "=========")
+ for ind, file in enumerate(glob.glob(demo_dir + "*.mjl")):
+
+ # process logs
+ print("processing: " + file, end=': ')
+
+ data = parse_mjl_logs(file, skip)
+
+ print("log duration %0.2f" % (data['time'][-1] - data['time'][0]))
+
+ # plot logs
+ if (graph):
+ print("plotting: " + file)
+ viz_parsed_mjl_logs(data)
+
+ # save logs
+ if (save_logs):
+ pickle.dump(data, open(file[:-4] + ".pkl", 'wb'))
+
+ # render logs to video
+ if view == 'render':
+ render_demos(
+ gym_env,
+ data,
+ filename=data['logName'][:-4] + '_demo_render.mp4',
+ render=render)
+
+ # playback logs and gather data
+ elif view == 'playback':
+ try:
+ obs, act,init_qpos, init_qvel = gather_training_data(gym_env, data,\
+ filename=data['logName'][:-4]+'_playback.mp4', render=render)
+ except Exception as e:
+ print(e)
+ continue
+ path = {
+ 'observations': obs,
+ 'actions': act,
+ 'goals': obs,
+ 'init_qpos': init_qpos,
+ 'init_qvel': init_qvel
+ }
+ paths.append(path)
+ # accept = input('accept demo?')
+ # if accept == 'n':
+ # continue
+ pickle.dump(path, open(demo_dir + env + str(ind) + "_path.pkl", 'wb'))
+ print(demo_dir + env + file + "_path.pkl")
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/quatmath.py b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/quatmath.py
new file mode 100644
index 0000000000000000000000000000000000000000..bae531a41acc0ca5c991aa55a2e452120be32cf1
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_envs/adept_envs/utils/quatmath.py
@@ -0,0 +1,180 @@
+#!/usr/bin/python
+#
+# Copyright 2020 Google LLC
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import numpy as np
+# For testing whether a number is close to zero
+_FLOAT_EPS = np.finfo(np.float64).eps
+_EPS4 = _FLOAT_EPS * 4.0
+
+
+def mulQuat(qa, qb):
+ res = np.zeros(4)
+ res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]
+ res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]
+ res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]
+ res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]
+ return res
+
+def negQuat(quat):
+ return np.array([quat[0], -quat[1], -quat[2], -quat[3]])
+
+def quat2Vel(quat, dt=1):
+ axis = quat[1:].copy()
+ sin_a_2 = np.sqrt(np.sum(axis**2))
+ axis = axis/(sin_a_2+1e-8)
+ speed = 2*np.arctan2(sin_a_2, quat[0])/dt
+ return speed, axis
+
+def quatDiff2Vel(quat1, quat2, dt):
+ neg = negQuat(quat1)
+ diff = mulQuat(quat2, neg)
+ return quat2Vel(diff, dt)
+
+
+def axis_angle2quat(axis, angle):
+ c = np.cos(angle/2)
+ s = np.sin(angle/2)
+ return np.array([c, s*axis[0], s*axis[1], s*axis[2]])
+
+def euler2mat(euler):
+ """ Convert Euler Angles to Rotation Matrix. See rotation.py for notes """
+ euler = np.asarray(euler, dtype=np.float64)
+ assert euler.shape[-1] == 3, "Invalid shaped euler {}".format(euler)
+
+ ai, aj, ak = -euler[..., 2], -euler[..., 1], -euler[..., 0]
+ si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak)
+ ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak)
+ cc, cs = ci * ck, ci * sk
+ sc, ss = si * ck, si * sk
+
+ mat = np.empty(euler.shape[:-1] + (3, 3), dtype=np.float64)
+ mat[..., 2, 2] = cj * ck
+ mat[..., 2, 1] = sj * sc - cs
+ mat[..., 2, 0] = sj * cc + ss
+ mat[..., 1, 2] = cj * sk
+ mat[..., 1, 1] = sj * ss + cc
+ mat[..., 1, 0] = sj * cs - sc
+ mat[..., 0, 2] = -sj
+ mat[..., 0, 1] = cj * si
+ mat[..., 0, 0] = cj * ci
+ return mat
+
+
+def euler2quat(euler):
+ """ Convert Euler Angles to Quaternions. See rotation.py for notes """
+ euler = np.asarray(euler, dtype=np.float64)
+ assert euler.shape[-1] == 3, "Invalid shape euler {}".format(euler)
+
+ ai, aj, ak = euler[..., 2] / 2, -euler[..., 1] / 2, euler[..., 0] / 2
+ si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak)
+ ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak)
+ cc, cs = ci * ck, ci * sk
+ sc, ss = si * ck, si * sk
+
+ quat = np.empty(euler.shape[:-1] + (4,), dtype=np.float64)
+ quat[..., 0] = cj * cc + sj * ss
+ quat[..., 3] = cj * sc - sj * cs
+ quat[..., 2] = -(cj * ss + sj * cc)
+ quat[..., 1] = cj * cs - sj * sc
+ return quat
+
+
+def mat2euler(mat):
+ """ Convert Rotation Matrix to Euler Angles. See rotation.py for notes """
+ mat = np.asarray(mat, dtype=np.float64)
+ assert mat.shape[-2:] == (3, 3), "Invalid shape matrix {}".format(mat)
+
+ cy = np.sqrt(mat[..., 2, 2] * mat[..., 2, 2] + mat[..., 1, 2] * mat[..., 1, 2])
+ condition = cy > _EPS4
+ euler = np.empty(mat.shape[:-1], dtype=np.float64)
+ euler[..., 2] = np.where(condition,
+ -np.arctan2(mat[..., 0, 1], mat[..., 0, 0]),
+ -np.arctan2(-mat[..., 1, 0], mat[..., 1, 1]))
+ euler[..., 1] = np.where(condition,
+ -np.arctan2(-mat[..., 0, 2], cy),
+ -np.arctan2(-mat[..., 0, 2], cy))
+ euler[..., 0] = np.where(condition,
+ -np.arctan2(mat[..., 1, 2], mat[..., 2, 2]),
+ 0.0)
+ return euler
+
+
+def mat2quat(mat):
+ """ Convert Rotation Matrix to Quaternion. See rotation.py for notes """
+ mat = np.asarray(mat, dtype=np.float64)
+ assert mat.shape[-2:] == (3, 3), "Invalid shape matrix {}".format(mat)
+
+ Qxx, Qyx, Qzx = mat[..., 0, 0], mat[..., 0, 1], mat[..., 0, 2]
+ Qxy, Qyy, Qzy = mat[..., 1, 0], mat[..., 1, 1], mat[..., 1, 2]
+ Qxz, Qyz, Qzz = mat[..., 2, 0], mat[..., 2, 1], mat[..., 2, 2]
+ # Fill only lower half of symmetric matrix
+ K = np.zeros(mat.shape[:-2] + (4, 4), dtype=np.float64)
+ K[..., 0, 0] = Qxx - Qyy - Qzz
+ K[..., 1, 0] = Qyx + Qxy
+ K[..., 1, 1] = Qyy - Qxx - Qzz
+ K[..., 2, 0] = Qzx + Qxz
+ K[..., 2, 1] = Qzy + Qyz
+ K[..., 2, 2] = Qzz - Qxx - Qyy
+ K[..., 3, 0] = Qyz - Qzy
+ K[..., 3, 1] = Qzx - Qxz
+ K[..., 3, 2] = Qxy - Qyx
+ K[..., 3, 3] = Qxx + Qyy + Qzz
+ K /= 3.0
+ # TODO: vectorize this -- probably could be made faster
+ q = np.empty(K.shape[:-2] + (4,))
+ it = np.nditer(q[..., 0], flags=['multi_index'])
+ while not it.finished:
+ # Use Hermitian eigenvectors, values for speed
+ vals, vecs = np.linalg.eigh(K[it.multi_index])
+ # Select largest eigenvector, reorder to w,x,y,z quaternion
+ q[it.multi_index] = vecs[[3, 0, 1, 2], np.argmax(vals)]
+ # Prefer quaternion with positive w
+ # (q * -1 corresponds to same rotation as q)
+ if q[it.multi_index][0] < 0:
+ q[it.multi_index] *= -1
+ it.iternext()
+ return q
+
+
+def quat2euler(quat):
+ """ Convert Quaternion to Euler Angles. See rotation.py for notes """
+ return mat2euler(quat2mat(quat))
+
+
+def quat2mat(quat):
+ """ Convert Quaternion to Euler Angles. See rotation.py for notes """
+ quat = np.asarray(quat, dtype=np.float64)
+ assert quat.shape[-1] == 4, "Invalid shape quat {}".format(quat)
+
+ w, x, y, z = quat[..., 0], quat[..., 1], quat[..., 2], quat[..., 3]
+ Nq = np.sum(quat * quat, axis=-1)
+ s = 2.0 / Nq
+ X, Y, Z = x * s, y * s, z * s
+ wX, wY, wZ = w * X, w * Y, w * Z
+ xX, xY, xZ = x * X, x * Y, x * Z
+ yY, yZ, zZ = y * Y, y * Z, z * Z
+
+ mat = np.empty(quat.shape[:-1] + (3, 3), dtype=np.float64)
+ mat[..., 0, 0] = 1.0 - (yY + zZ)
+ mat[..., 0, 1] = xY - wZ
+ mat[..., 0, 2] = xZ + wY
+ mat[..., 1, 0] = xY + wZ
+ mat[..., 1, 1] = 1.0 - (xX + zZ)
+ mat[..., 1, 2] = yZ - wX
+ mat[..., 2, 0] = xZ - wY
+ mat[..., 2, 1] = yZ + wX
+ mat[..., 2, 2] = 1.0 - (xX + yY)
+ return np.where((Nq > _FLOAT_EPS)[..., np.newaxis, np.newaxis], mat, np.eye(3))
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/.gitignore b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..b8e86679feda2ccc2eee79b913f612a1419f85ad
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/.gitignore
@@ -0,0 +1,8 @@
+# General
+.DS_Store
+*.swp
+*.profraw
+
+# Editors
+.vscode
+.idea
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/CONTRIBUTING.public.md b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/CONTRIBUTING.public.md
new file mode 100644
index 0000000000000000000000000000000000000000..db177d4ac70f76c2172ce65ca33c830d6cbd3ac5
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/CONTRIBUTING.public.md
@@ -0,0 +1,28 @@
+# How to Contribute
+
+We'd love to accept your patches and contributions to this project. There are
+just a few small guidelines you need to follow.
+
+## Contributor License Agreement
+
+Contributions to this project must be accompanied by a Contributor License
+Agreement. You (or your employer) retain the copyright to your contribution;
+this simply gives us permission to use and redistribute your contributions as
+part of the project. Head over to to see
+your current agreements on file or to sign a new one.
+
+You generally only need to submit a CLA once, so if you've already submitted one
+(even if it was for a different project), you probably don't need to do it
+again.
+
+## Code reviews
+
+All submissions, including submissions by project members, require review. We
+use GitHub pull requests for this purpose. Consult
+[GitHub Help](https://help.github.com/articles/about-pull-requests/) for more
+information on using pull requests.
+
+## Community Guidelines
+
+This project follows
+[Google's Open Source Community Guidelines](https://opensource.google.com/conduct/).
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/LICENSE b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..9a644b9b462a13d4cbc3dfd2f97517b561033e8a
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/LICENSE
@@ -0,0 +1,203 @@
+Copyright 2019 The DSuite Authors. All rights reserved.
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/README.public.md b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/README.public.md
new file mode 100644
index 0000000000000000000000000000000000000000..da3fa5d951d3f0c3d63f258fbacf12a6f6e0ef3c
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/README.public.md
@@ -0,0 +1,10 @@
+# D'Suite Scenes
+
+This repository is based on a collection of [MuJoCo](http://www.mujoco.org/) simulation
+scenes and common assets for D'Suite environments. Based on code in the ROBEL suite
+https://github.com/google-research/robel
+
+## Disclaimer
+
+This is not an official Google product.
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9e1e39d64ee3dad5b99b7bc23f5ce51cce73e0d3
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_asset.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b76b0daf7bec909eebac89d57b113804a03187f1
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/backwall_chain.xml
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c3e28f8b7e3fa919674ccf9a06970cc404d22439
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_asset.xml
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..83e1791e42b896feeb3c401c7cf15d8121d3b88f
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/counters_chain.xml
@@ -0,0 +1,48 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..820281007da27103bc68f115b1849c5dba185a5d
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_asset.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7f935d32d4d62cc120de87b8ec698bad4fa91a6f
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/hingecabinet_chain.xml
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dbe8e9bf6b12d734d257168b0d38f6358dc1bdc7
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_asset.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..fb5f224898380f47c655ea23ca82b141b6459a06
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/kettle_chain.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..cc651ee3131de9849d9776019b5335b493e7ba98
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_asset.xml
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..fd88ab365072113c634752589ed1656fde92db46
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/microwave_chain.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ef1184e73a390315e49966ef30a196ba1b1e5a62
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_asset.xml
@@ -0,0 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f96f8c7492b360ec15cee7dacd071356673bab30
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/oven_chain.xml
@@ -0,0 +1,115 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_asset.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_asset.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f0f370a7710d9e48dd99d71800d9fa4ca637cb09
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_asset.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_chain.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_chain.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5aa820e6edf53a32539d7ed9bb9eb11f9e7b22b2
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/assets/slidecabinet_chain.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/counters.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/counters.xml
new file mode 100644
index 0000000000000000000000000000000000000000..69fb8895380e36de8fa9185d9684b62dae7479a4
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/counters.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/hingecabinet.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/hingecabinet.xml
new file mode 100644
index 0000000000000000000000000000000000000000..89b8db41a37809790882f353db262b7d71e4c343
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/hingecabinet.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kettle.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kettle.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a27e978755ca3427f4e70af531db6c5cfe6a72b3
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kettle.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kitchen.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kitchen.xml
new file mode 100644
index 0000000000000000000000000000000000000000..34813ca5257484cc49ccaeec21c30b8d6dba90aa
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/kitchen.xml
@@ -0,0 +1,38 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate.stl
new file mode 100644
index 0000000000000000000000000000000000000000..46740b572af7ae501a1e520460d8494af07d54f3
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate_mesh.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate_mesh.stl
new file mode 100644
index 0000000000000000000000000000000000000000..46740b572af7ae501a1e520460d8494af07d54f3
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/burnerplate_mesh.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetbase.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetbase.stl
new file mode 100644
index 0000000000000000000000000000000000000000..580a51ccefa2d85e85069a2186a8dc7f7ba3ce67
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetbase.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetdrawer.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetdrawer.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0932eeb5e2b92a9a37d933e646630fa48db372b2
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinetdrawer.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinethandle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinethandle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..960cd392716c68e979bbaffd589746ce514d87e3
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/cabinethandle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/countertop.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/countertop.stl
new file mode 100644
index 0000000000000000000000000000000000000000..16410d10f4a96885081464e046d61b324ff3035a
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/countertop.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/faucet.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/faucet.stl
new file mode 100644
index 0000000000000000000000000000000000000000..55404aff691b9e1175d52b1fa0ca7bb77ccb5555
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/faucet.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/handle2.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/handle2.stl
new file mode 100644
index 0000000000000000000000000000000000000000..09b7833e9ddf59d2cec084f8b90bd7d5e24582fc
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/handle2.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingecabinet.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingecabinet.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6693df8edb7986d08bff131061e085ff6d52c94a
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingecabinet.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingedoor.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingedoor.stl
new file mode 100644
index 0000000000000000000000000000000000000000..feecf23aafaa874bdfddb995dfe56db6c74c123a
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingedoor.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingehandle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingehandle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..fb855210eeac21a8ee1ae25d96298eba25485b9d
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hingehandle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hood.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hood.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6c0e3ad68880dd7acdddc2940c437ecdab426d0a
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/hood.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0e8d9e536cdc78df9dbbb0d4c0ec3bfb7d80ed05
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettlehandle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettlehandle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..83baef3a78dd11fd2307a0e9785663e7d6983e5c
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/kettlehandle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/knob.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/knob.stl
new file mode 100644
index 0000000000000000000000000000000000000000..90180b5cc7c75a3f5ba3f57652feafb3ee47ed47
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/knob.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitch.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitch.stl
new file mode 100644
index 0000000000000000000000000000000000000000..fa956c95ec7faa88308e745e4f287b875e4c696f
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitch.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitchbase.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitchbase.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e64b0596afcb818221ca0295e6a35390eccba2cf
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/lightswitchbase.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/micro.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/micro.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6ed68021611282d23877e12e459b069f1b870fb7
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/micro.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microbutton.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microbutton.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2d7f1e331fa0bebfb4d63bf9112544a494d726bc
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microbutton.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microdoor.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microdoor.stl
new file mode 100644
index 0000000000000000000000000000000000000000..fa8c5485aae408445e6cbe8b9d74d8296330f3b5
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microdoor.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microefeet.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microefeet.stl
new file mode 100644
index 0000000000000000000000000000000000000000..98e7069b093a1cee33c7cdd16543e279cf7228c7
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microefeet.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microfeet.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microfeet.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a5162995d483bb128115dc373b47474288adc03d
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microfeet.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microhandle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microhandle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ed31a707e48a7c97ab45ee06277372be436c4a6c
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microhandle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microwindow.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microwindow.stl
new file mode 100644
index 0000000000000000000000000000000000000000..07d3c85b182f6b25099a4303d23800859daac303
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/microwindow.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oven.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oven.stl
new file mode 100644
index 0000000000000000000000000000000000000000..04d3b6680437c46d7c0c1eb31f0410691684ad17
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oven.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenhandle.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenhandle.stl
new file mode 100644
index 0000000000000000000000000000000000000000..30250a753cd88c16f1b22f4b776d40c6902abbf0
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenhandle.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oventop.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oventop.stl
new file mode 100644
index 0000000000000000000000000000000000000000..fb6664d5feb95fb4e061b3acb0d51127f55c9d8f
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/oventop.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenwindow.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenwindow.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f0205a52cbdbdf71d57669aaab046d3b67b9d7a1
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/ovenwindow.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidecabinet.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidecabinet.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6249a14fbb83fd3c1d6f85cbc607e23b6928d2fb
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidecabinet.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidedoor.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidedoor.stl
new file mode 100644
index 0000000000000000000000000000000000000000..307d6c52ec79272c43fb69375c025fc05f02da65
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/slidedoor.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/stoverim.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/stoverim.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0f76bfc3a287474ca997e8a4d5850f6d805ca8a1
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/stoverim.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/tile.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/tile.stl
new file mode 100644
index 0000000000000000000000000000000000000000..12639cead29a24b6607a9357b2e3d2ab994cb876
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/tile.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/wall.stl b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/wall.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f5562e216deaa93e31b221140d14aa3f60ed72a1
Binary files /dev/null and b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/meshes/wall.stl differ
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/microwave.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/microwave.xml
new file mode 100644
index 0000000000000000000000000000000000000000..3946632506015267c085433b9cfa26909a45a5bc
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/microwave.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/oven.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/oven.xml
new file mode 100644
index 0000000000000000000000000000000000000000..68913856dcebeb461aa711b9eda4d8ec987495c5
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/oven.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/slidecabinet.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/slidecabinet.xml
new file mode 100644
index 0000000000000000000000000000000000000000..78fa599c212cbf14147bbade4f70bc505e15a330
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/kitchen/slidecabinet.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/scenes/basic_scene.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/scenes/basic_scene.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8d5356d7c7444fd74341383d7e3a14f8f6ea45fe
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/adept_models/scenes/basic_scene.xml
@@ -0,0 +1,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/basic_scene.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/basic_scene.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4bb7e70a60c67bb07a7d2dad8ffaf9f4f64ed4de
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/basic_scene.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain0.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain0.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e2e53a7dfe9e99d63f2b29cb6a33f69f463a8d57
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain0.xml
@@ -0,0 +1,103 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain1.xml b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain1.xml
new file mode 100644
index 0000000000000000000000000000000000000000..29a9524e2699522b53dbcdecfab954d2889190a5
--- /dev/null
+++ b/third_party/diffusion_policy/diffusion_policy/env/kitchen/relay_policy_learning/third_party/franka/assets/chain1.xml
@@ -0,0 +1,61 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file