Add files using upload-large-folder tool

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

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

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

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)

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')

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
+    )

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

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block.urdf

@@ -0,0 +1,31 @@
+<?xml version="0.0" ?>
+<robot name="box.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+         <box size="0.04 0.04 0.04"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1 0.3412 0.3490 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+             <box size="0.04 0.04 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/block2.urdf

@@ -0,0 +1,31 @@
+<?xml version="0.0" ?>
+<robot name="box2.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+         <box size="0.04 0.04 0.04"/>
+      </geometry>
+      <material name="red">
+        <color rgba="0.3412 1 0.3490 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+             <box size="0.04 0.04 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/blue_cube.urdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<robot name="blue_cube.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="0.5"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="cube.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+       <material name="blue">
+        <color rgba="0.4 0.4 1.0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="cube.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

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

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/green_star.urdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<robot name="green_star.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="0.5"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="star.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+       <material name="green">
+        <color rgba="0.4 1.0 0.4 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="star.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

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

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

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/red_moon.urdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<robot name="red_moon.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="0.5"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="moon.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+       <material name="red">
+        <color rgba="1 0.4 0.4 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="moon.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

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

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/blocks/yellow_pentagon.urdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<robot name="yellow_pentagon.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="0.5"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".01"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="pentagon.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+       <material name="yellow">
+        <color rgba="1 1 0.4 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="pentagon.obj" scale="1.0 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/insert.urdf

@@ -0,0 +1,66 @@
+<?xml version="0.0" ?>
+<robot name="ell.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="0.3"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+
+    <visual>
+      <origin rpy="0 0 0" xyz="0.025 0 0"/>
+      <geometry>
+        <box size=".08 .03 .04"/>
+      </geometry>
+      <material name="red">
+        <color rgba="0. 0.3412 0.3490 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.025 0 0"/>
+      <geometry>
+        <box size=".08 .03 .04"/>
+      </geometry>
+    </collision>
+
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0.025 0"/>
+      <geometry>
+        <box size=".03 .08 .0399"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0. 0.3412 0.3490 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0.025 0"/>
+      <geometry>
+        <box size=".03 .08 .0399"/>
+      </geometry>
+    </collision>
+
+
+    <visual>
+      <origin rpy="0 0 0" xyz="0.075 0.025 0"/>
+      <geometry>
+        <box size=".03 .08 .0399"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0. 0.3412 0.3490 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.075 0.025 0"/>
+      <geometry>
+        <box size=".03 .08 .0399"/>
+      </geometry>
+    </collision>
+
+
+  </link>
+</robot>

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

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
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder.urdf

@@ -0,0 +1,98 @@
+<?xml version="0.0" ?>
+<robot name="cylinder.urdf">
+  <link name="headLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="tipJoint" type="fixed">
+    <parent link="headLink"/>
+    <child link="tipLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.029"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint>
+
+  <link name="tipLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.005"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.18039216, 0.50588235, 0.77254902 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- <link name="asdfLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.028" radius="0.001"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <joint name="asdfoint" type="fixed">
+    <parent link="tipLink"/>
+    <child link="asdfLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint> -->
+
+
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/cylinder_real.urdf

@@ -0,0 +1,98 @@
+<?xml version="0.0" ?>
+<robot name="cylinder_real.urdf">
+  <link name="headLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="tipJoint" type="fixed">
+    <parent link="headLink"/>
+    <child link="tipLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.029"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint>
+
+  <link name="tipLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.135" radius="0.0127"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.5, 0.5, 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.135" radius="0.0127"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- <link name="asdfLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.028" radius="0.001"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <joint name="asdfoint" type="fixed">
+    <parent link="tipLink"/>
+    <child link="asdfLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint> -->
+
+
+</robot>
+

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
+

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
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-base.urdf

@@ -0,0 +1,69 @@
+<?xml version="0.0" ?>
+<robot name="suction-base.urdf">
+  <material name="DarkGrey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="base.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="base.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+
+  <joint name="headJoint" type="fixed">
+    <parent link="baseLink"/>
+    <child link="midLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.015"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint>
+
+  <link name="midLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="mid.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="mid.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head-long.urdf

@@ -0,0 +1,101 @@
+<?xml version="0.0" ?>
+<robot name="suction-head.urdf">
+  <link name="headLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="tipJoint" type="fixed">
+    <parent link="headLink"/>
+    <child link="tipLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.029"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint>
+
+  <link name="tipLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="tip.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.18039216, 0.50588235, 0.77254902 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <!-- <geometry>
+        <mesh filename="tip.obj" scale="0.001 0.001 0.001"/>
+      </geometry> -->
+      <geometry>
+        <cylinder length="0.028" radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- <link name="asdfLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.028" radius="0.001"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <joint name="asdfoint" type="fixed">
+    <parent link="tipLink"/>
+    <child link="asdfLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint> -->
+
+
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/suction/suction-head.urdf

@@ -0,0 +1,70 @@
+<?xml version="0.0" ?>
+<robot name="suction-head.urdf">
+  <link name="headLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="darkgrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="head.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="tipJoint" type="fixed">
+    <parent link="headLink"/>
+    <child link="tipLink"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.029"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="10.0" friction="0.0"/>
+  </joint>
+
+  <link name="tipLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0001"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="tip.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0.18039216, 0.50588235, 0.77254902 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="tip.obj" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace.urdf

@@ -0,0 +1,30 @@
+<?xml version="0.0" ?>
+<robot name="workspace.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="plane.obj" scale="0.0167 0.0333 1"/>
+      </geometry>
+      <material name="DarkGrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.005"/>
+      <geometry>
+        <box size="2.0 3.0 0.01"/>
+      </geometry>
+    </collision> -->
+  </link>
+</robot>
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/workspace_real.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="workspace.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="plane.obj" scale="0.01524 0.02032 1"/>
+      </geometry>
+      <material name="DarkGrey">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.005"/>
+      <geometry>
+        <box size="2.0 3.0 0.01"/>
+      </geometry>
+    </collision> -->
+  </link>
+</robot>

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
+

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone.urdf

@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<robot name="zone.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.01 0 0.02"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="zone.obj" scale="0.006 0.006 0.00005"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+</robot>

third_party/diffusion_policy/diffusion_policy/env/block_pushing/assets/zone2.urdf

@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<robot name="zone2.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.01 0 0.02"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="zone.obj" scale="0.006 0.006 0.00005"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1 0 0 1"/>
+      </material>
+    </visual>
+  </link>
+</robot>

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,
+)

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)),
+)

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)),
+)

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))

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))

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)
+        )

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

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))

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)

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

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
+            )

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*(# )?<?https?://\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=^(?:(?P<exempt>setUp|tearDown|setUpModule|tearDownModule)|(?P<camel_case>_?[A-Z][a-zA-Z0-9]*)|(?P<snake_case>_?[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<exempt>_[a-z0-9_]+__|runTest|setUp|tearDown|setUpTestCase|tearDownTestCase|setupSelf|tearDownClass|setUpClass|(test|assert)_*[A-Z0-9][a-zA-Z0-9_]*|next)|(?P<camel_case>_{0,2}[A-Z][a-zA-Z0-9_]*)|(?P<snake_case>_{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

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
+

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

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])
+

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,
+)

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 @@
+<!--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-->
+
+    <!--https://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.-->
+
+<mujoco model="franka_mocap_studyTable_buttons">
+
+    <size njmax='1000' nconmax='1000'/>
+
+    <include file="../../../../adept_models/scenes/basic_scene.xml"/>
+    <include file="../../../../third_party/franka/assets/assets.xml"/>
+    <include file="../../../../third_party/franka/assets/actuator0.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/oven_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/counters_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/backwall_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/slidecabinet_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/hingecabinet_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/microwave_asset.xml"/>
+    <include file="../../../../adept_models/kitchen/assets/kettle_asset.xml"/>
+
+    <visual>
+    <global offwidth="2560" offheight="1920" />
+    <quality shadowsize="4096" offsamples="8" />
+    <map force="0.1" fogend="5" />
+    </visual>
+
+    <compiler inertiafromgeom='auto' inertiagrouprange='3 5' angle="radian"
+              meshdir="../../../../adept_models/kitchen"
+              texturedir="../../../../adept_models/kitchen"/>
+
+    <equality>
+        <weld body1="vive_controller" body2="world" solref="0.02 1" solimp=".7 .95 0.050"/>
+    </equality>
+
+    <worldbody>
+
+        <!-- Mocap -->
+        <body name="vive_controller" mocap="true" pos="-0.440 -0.092 2.026" euler="-1.57 0 -.785">
+            <geom type="box" group="2" pos='0 0 .142' size="0.02 0.10 0.03" contype="0" conaffinity="0" rgba=".9 .7 .95 0" euler="0 0 -.785"/>
+        </body>
+
+        <site name='target' pos='0 0 0' size='0.1' rgba='0 2 0 .2'/>
+        <camera name='left_cap' pos='-1.2 -0.5 1.8' quat='0.78 0.49 -0.22 -0.32' />
+        <camera name='right_cap' pos='1.2 -0.5 1.8' quat='0.76 0.5 0.21 0.35'/>
+
+        <!-- Robot -->
+        <body pos='0. 0 1.8' euler='0 0 1.57'>
+            <geom type='cylinder' size='.120 .90' pos='-.04 0 -0.90' class='panda_viz'/>
+            <include file="../../../../third_party/franka/assets/chain0.xml"/>
+        </body>
+
+        <body name='desk' pos='-0.1 0.75 0'>
+
+            <body name="counters1" pos="0 0 0" >
+                <include file="../../../../adept_models/kitchen/assets/counters_chain.xml"/>
+            </body>
+            <body name="oven" pos="0 0 0" >
+                <include file="../../../../adept_models/kitchen/assets/oven_chain.xml"/>
+            </body>
+            <body name="backwall" pos="0 0 0" >
+                <include file="../../../../adept_models/kitchen/assets/backwall_chain.xml"/>
+            </body>
+            <body name="slidecabinet" pos="0.4 0.3 2.6" >
+                <include file="../../../../adept_models/kitchen/assets/slidecabinet_chain.xml"/>
+            </body>
+            <body name="hingecabinet" pos="-0.504 0.28 2.6" >
+                <include file="../../../../adept_models/kitchen/assets/hingecabinet_chain.xml"/>
+            </body>
+            <body name="microwave" pos="-0.750 -0.025 1.6" euler="0 0 0.3">
+                <include file="../../../../adept_models/kitchen/assets/microwave_chain.xml"/>
+            </body>
+        </body>
+        <body name="kettle" pos="-0.269 0.35 1.626">
+            <freejoint/>
+            <include file="../../../../adept_models/kitchen/assets/kettle_chain.xml"/>
+        </body>
+
+    </worldbody>
+
+
+    <keyframe>
+        <key qpos='0.16 -1.76 1.84 -2.51 0.36 0.79 1.55 0.00 0.0 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 8.24417e-05 9.48283e-05 0 0 0 0 -0.269 0.35 1.61523 1 1.34939e-19 -3.51612e-05 -7.50168e-19'/>
+    </keyframe>
+
+</mujoco>