├── requirements.txt ├── configs ├── visualize.txt ├── visualize_merge.txt ├── visualize_hdf5.txt ├── tare_pyft.txt ├── hdf5.txt ├── collect.txt ├── merge.txt └── pose.py ├── assets └── teaser.jpg ├── .gitmodules ├── .gitignore ├── visualize_hdf5s.sh ├── create_hdf5s.sh ├── utils ├── annotation.py ├── process.py └── transformation.py ├── LICENSE ├── README.md ├── test_tare_pyft.py ├── collect_data.py ├── tare_pyft.py ├── merge_hdf5.py ├── visualize_hdf5.py ├── create_hdf5.py ├── visualize_data.py ├── visualize_merge.py └── objs └── only_peeler.obj /requirements.txt: -------------------------------------------------------------------------------- 1 | pynput 2 | h5py 3 | -------------------------------------------------------------------------------- /configs/visualize.txt: -------------------------------------------------------------------------------- 1 | data_path = ./peel_data/01 2 | fps = 30 3 | -------------------------------------------------------------------------------- /assets/teaser.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ForceMimic/forcecapture/HEAD/assets/teaser.jpg -------------------------------------------------------------------------------- /configs/visualize_merge.txt: -------------------------------------------------------------------------------- 1 | hdf5_path = ./post_peel_data/v1.0.s.hdf5 2 | sample = False 3 | -------------------------------------------------------------------------------- /configs/visualize_hdf5.txt: -------------------------------------------------------------------------------- 1 | hdf5_path = ./post_peel_data/03/shave_00.hdf5 2 | pc_mesh = False 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "r3kit"] 2 | path = r3kit 3 | url = git@github.com:ForceMimic/r3kit.git 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | 3 | .vscode/ 4 | .idea/ 5 | 6 | temp_data/ 7 | pre_peel_data 8 | peel_data 9 | post_peel_data 10 | -------------------------------------------------------------------------------- /configs/tare_pyft.txt: -------------------------------------------------------------------------------- 1 | save_path = ./pre_peel_data/20240830 2 | t265_id = 230322111188 3 | pyft_id = 192.168.1.10 4 | pyft_port = 49152 5 | -------------------------------------------------------------------------------- /visualize_hdf5s.sh: -------------------------------------------------------------------------------- 1 | hdf5_list=`ls ./post_peel_data/train/01/*.hdf5` 2 | for hdf5_file in $hdf5_list 3 | do 4 | echo $hdf5_file 5 | python visualize_hdf5.py --hdf5_path $hdf5_file --pc_mesh 6 | done 7 | -------------------------------------------------------------------------------- /configs/hdf5.txt: -------------------------------------------------------------------------------- 1 | data_path = ./peel_data/20 2 | save_path = ./post_peel_data/20 3 | clip_object = True 4 | clip_gripper = True 5 | clip_pyft = True 6 | clip_base = True 7 | render_gripper_num = 0 8 | render_pyft_num = 0 9 | voxel_size = 0.001 10 | pc_num = 10000 11 | -------------------------------------------------------------------------------- /configs/collect.txt: -------------------------------------------------------------------------------- 1 | save_path = ./peel_data/20 2 | l515_id = f0172289 3 | t265l_id = 230222110234 4 | t265r_id = 230322111188 5 | pyft_id = 192.168.1.10 6 | pyft_port = 49152 7 | pyft_tare_path = ./pre_peel_data/20240830 8 | angler_id = /dev/ttyUSB0 9 | angler_index = 2 10 | -------------------------------------------------------------------------------- /configs/merge.txt: -------------------------------------------------------------------------------- 1 | data_path = ./post_peel_data 2 | save_path = ./post_peel_data/v1.0.s.hdf5 3 | stage = shave 4 | pc_mesh = False 5 | ft_coord = False 6 | num_o = 1 7 | num_a = 20 8 | num_aa = 10 9 | pad_o = True 10 | pad_a = True 11 | pad_aa = True 12 | gripper_xyz_threshold = 0.003 13 | gripper_quat_threshold = 0 14 | pyft_xyz_threshold = 0.003 15 | pyft_quat_threshold = 0 16 | pyft_f_threshold = 3 17 | pyft_t_threshold = 0 18 | -------------------------------------------------------------------------------- /create_hdf5s.sh: -------------------------------------------------------------------------------- 1 | data_dir='./peel_data' 2 | save_dir='./post_peel_data' 3 | data_list=`ls $data_dir` 4 | for data_name in $data_list 5 | do 6 | data_path=$data_dir/$data_name 7 | save_path=$save_dir/$data_name 8 | echo $data_path, $save_path 9 | python create_hdf5.py --data_path $data_path --save_path $save_path --clip_object --clip_gripper --clip_pyft --clip_base --render_gripper_num 10000 --render_pyft_num 10000 --voxel_size 0.001 --pc_num 10000 10 | done 11 | -------------------------------------------------------------------------------- /utils/annotation.py: -------------------------------------------------------------------------------- 1 | from typing import List, Dict, Union 2 | 3 | 4 | def search_stage(current_timestamp:float, stages:List[Dict[str, Union[float, list, str]]]) -> int: 5 | current_stage_idx = 0 6 | for stage_idx in range(len(stages)): 7 | if stages[stage_idx]['timestamp_ms'] <= current_timestamp and stages[stage_idx]['timestamp_ms'] >= stages[current_stage_idx]['timestamp_ms']: 8 | current_stage_idx = stage_idx 9 | return current_stage_idx 10 | -------------------------------------------------------------------------------- /utils/process.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | import open3d as o3d 3 | import numpy as np 4 | 5 | 6 | def voxelize(pc_xyz:np.ndarray, pc_rgb:np.ndarray, voxel_size:float) -> Tuple[np.ndarray, np.ndarray]: 7 | pcd = o3d.geometry.PointCloud() 8 | pcd.points = o3d.utility.Vector3dVector(pc_xyz) 9 | pcd.colors = o3d.utility.Vector3dVector(pc_rgb) 10 | downsampled_pcd = pcd.voxel_down_sample(voxel_size=voxel_size) 11 | downsampled_pc_xyz = np.asarray(downsampled_pcd.points) 12 | downsampled_pc_rgb = np.asarray(downsampled_pcd.colors) 13 | return (downsampled_pc_xyz, downsampled_pc_rgb) 14 | 15 | 16 | def mesh2pc(obj_path:str, num_points:int) -> np.ndarray: 17 | mesh = o3d.io.read_triangle_mesh(obj_path) 18 | pc = mesh.sample_points_uniformly(number_of_points=num_points) 19 | pc = np.asarray(pc.points) 20 | return pc 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 ForceMimic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /utils/transformation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as Rot 3 | 4 | 5 | def transform_pc(pc_camera:np.ndarray, c2w:np.ndarray) -> np.ndarray: 6 | # pc_camera: (N, 3), c2w: (4, 4) 7 | pc_camera_hm = np.concatenate([pc_camera, np.ones((pc_camera.shape[0], 1), dtype=pc_camera.dtype)], axis=-1) # (N, 4) 8 | pc_world_hm = pc_camera_hm @ c2w.T # (N, 4) 9 | pc_world = pc_world_hm[:, :3] # (N, 3) 10 | return pc_world 11 | 12 | def transform_dir(dir_camera:np.ndarray, c2w:np.ndarray) -> np.ndarray: 13 | # dir_camera: (N, 3), c2w: (4, 4) 14 | dir_camera_hm = np.concatenate([dir_camera, np.zeros((dir_camera.shape[0], 1), dtype=dir_camera.dtype)], axis=-1) # (N, 4) 15 | dir_world_hm = dir_camera_hm @ c2w.T # (N, 4) 16 | dir_world = dir_world_hm[:, :3] # (N, 3) 17 | return dir_world 18 | 19 | 20 | def xyzquat2mat(xyz:np.ndarray, quat:np.ndarray) -> np.ndarray: 21 | pose_4x4 = np.eye(4) 22 | pose_4x4[:3, :3] = Rot.from_quat(quat).as_matrix() 23 | pose_4x4[:3, 3] = xyz 24 | return pose_4x4 25 | 26 | 27 | def delta_xyz(xyz1:np.ndarray, xyz2:np.ndarray) -> float: 28 | delta = xyz1 - xyz2 29 | delta_value = np.linalg.norm(delta) 30 | return delta_value 31 | 32 | def delta_quat(quat1:np.ndarray, quat2:np.ndarray) -> float: 33 | delta = Rot.from_quat(quat1).inv() * Rot.from_quat(quat2) 34 | delta_value = delta.magnitude() 35 | return delta_value 36 | -------------------------------------------------------------------------------- /configs/pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | L515_2_BASE = np.array([[1,0,0,0], 4 | [0,-np.sin(70 / 180 * np.pi),np.cos(70 / 180 * np.pi),0], 5 | [0,-np.cos(70 / 180 * np.pi),-np.sin(70 / 180 * np.pi),0.59], 6 | [0,0,0,1]]) 7 | L515_2_T265l = np.array([[1,0,0,8.43/1000], 8 | [0,-np.cos(70 / 180 * np.pi),-np.sin(70 / 180 * np.pi),30.12/1000], 9 | [0,np.sin(70 / 180 * np.pi),-np.cos(70 / 180 * np.pi),-95.66/1000], 10 | [0,0,0,1]]) 11 | T265l_2_L515 = np.linalg.inv(L515_2_T265l) 12 | T265l_2_BASE = L515_2_BASE @ T265l_2_L515 13 | T265r_2_T265l = np.array([[1,0,0,0], 14 | [0,1,0,-32/1000], 15 | [0,0,1,0], 16 | [0,0,0,1]]) 17 | T265r_2_L515 = T265l_2_L515 @ T265r_2_T265l 18 | L515_2_T265r = np.linalg.inv(T265r_2_L515) 19 | T265r_2_BASE = L515_2_BASE @ T265r_2_L515 20 | T265l_2_GRIPPER = np.array([[-1,0,0,0], 21 | [0,np.cos(45 / 180 * np.pi),-np.sin(45 / 180 * np.pi),38.53/1000], 22 | [0,-np.sin(45 / 180 * np.pi),-np.cos(45 / 180 * np.pi),-19.47/1000], 23 | [0,0,0,1]]) 24 | T265r_2_PYFT = np.array([[-1,0,0,0], 25 | [0,np.cos(45 / 180 * np.pi),-np.sin(45 / 180 * np.pi),81.81/1000], 26 | [0,-np.sin(45 / 180 * np.pi),-np.cos(45 / 180 * np.pi),-19.5/1000], 27 | [0,0,0,1]]) 28 | 29 | ANGLE_2_WIDTH = 17 * 3.14 / 1000.0 * 2 / 360.0 30 | 31 | OBJECT_SPACE = ((-0.15, 0.1), (0.135, 0.385), (0.035, 0.335)) 32 | GRIPPER_SPACE = ((-0.095-0.01, 0.095+0.01), (-0.0165-0.005, 0.0165+0.005), (0.-0.005, 0.12+0.005)) 33 | PYFT_SPACE = ((-0.035-0.005, 0.035+0.005), (-0.035-0.005, 0.035+0.005), (0.-0.005, 0.14+0.005)) 34 | BASE_SPACE = ((-0.4, 0.4), (0.035, 0.585), (0.035, 0.835)) 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ForceCapture 2 | 3 | 4 | Website 5 | 6 | 7 | Paper 8 | 9 | 10 | ICRA 11 | 12 |
13 | 14 | 15 | ![teaser](assets/teaser.jpg) 16 | 17 | Official implementation for ForceCapture in the paper [ForceMimic: Force-Centric Imitation Learning with Force-Motion Capture System for Contact-Rich Manipulation](https://arxiv.org/abs/2410.07554), accepted by [ICRA 2025](https://2025.ieee-icra.org). 18 | 19 | For more information, please visit our [project website](https://forcemimic.github.io/). 20 | 21 | --- 22 | 23 | ## Hardware 24 | Download the fixed-tool version assets from [Onshape](https://cad.onshape.com/documents/a8dc2f65bd1ae816592e5c51/w/cf0c50fb046b5fe4d6c55a08/e/2a83c3cca3f56ae2baac3a15) and print them as needed. 25 | The force-torque sensor used here is an inner product by [Flexiv](https://www.flexiv.com/), but you can easily replace it with a commonly-used alternative, like [ATI](https://www.ati-ia.com/Products/ft/sensors.aspx). 26 | The SLAM camera used here is [Intel RealSense T265](https://www.intelrealsense.com/tracking-camera-t265/), and the observation camera used here is [Intel RealSense L515](https://www.intelrealsense.com/lidar-camera-l515/), while they are [discontinued](https://www.intelrealsense.com/message-to-customers/). And the angle encoder used here is by [pdcd](http://www.dgpdcdkj.com/). 27 | 28 | ## Installation 29 | ```bash 30 | git clone git@github.com:ForceMimic/forcecapture.git --recurse-submodules 31 | cd forcecapture 32 | # `main` branch for fixed-tool version, `git checkout gripper` to `gripper` branch for gripper version 33 | 34 | conda create -n focap python=3.10 35 | conda activate focap 36 | 37 | cd r3kit 38 | pip install -e . 39 | cd .. 40 | 41 | pip install -r requirements.txt 42 | ``` 43 | 44 | ## Pre-collection 45 | ```bash 46 | python tare_pyft.py --config configs/tare_pyft.txt 47 | python test_tare_pyft.py --config configs/tare_pyft.txt 48 | ``` 49 | 50 | ## Collection 51 | ```bash 52 | python collect_data.py --config configs/collect.txt 53 | ``` 54 | 55 | > NOTE: During collection, use `htop` to watch memory usage, and use `sudo sync` and `sudo sysctl -w vm.drop_caches=3` to free memory. 56 | 57 | ## Post-collection 58 | ```bash 59 | python visualize_data.py --config configs/visualize.txt # also be used as annotation 60 | python create_hdf5.py --config configs/hdf5.txt 61 | python visualize_hdf5.py --config configs/visualize_hdf5.txt 62 | python merge_hdf5.py --config configs/merge.txt 63 | python visualize_merge.py --config configs/visualize_merge.txt 64 | ``` 65 | 66 | ## Example Data 67 | You can download our processed dataset from [Google Drive](https://drive.google.com/drive/folders/1sKqOj9tO-luddM8XIaJvTFbRSEZet7C3?usp=sharing). 68 | 69 | --- 70 | 71 | ## Acknowledgement 72 | Our design and implementation are inspired by [DexCap](https://github.com/j96w/DexCap) and [UMI](https://github.com/real-stanford/universal_manipulation_interface). Kudos to the authors for their amazing contributions. 73 | 74 | ## Citation 75 | If you find our work useful, please consider citing: 76 | ```bibtex 77 | @inproceedings{liu2025forcemimic, 78 | author={Liu, Wenhai and Wang, Junbo and Wang, Yiming and Wang, Weiming and Lu, Cewu}, 79 | booktitle={2025 IEEE International Conference on Robotics and Automation (ICRA)}, 80 | title={ForceMimic: Force-Centric Imitation Learning with Force-Motion Capture System for Contact-Rich Manipulation}, 81 | year={2025} 82 | } 83 | ``` 84 | 85 | ## License 86 | This repository is released under the [MIT](https://mit-license.org/) license. 87 | -------------------------------------------------------------------------------- /test_tare_pyft.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import configargparse 4 | import json 5 | from copy import deepcopy 6 | from pynput import keyboard 7 | import numpy as np 8 | 9 | from r3kit.devices.camera.realsense.t265 import T265 10 | from r3kit.devices.ftsensor.ati.pyati import PyATI as Pyft 11 | from configs.pose import * 12 | 13 | 14 | def config_parse() -> configargparse.Namespace: 15 | parser = configargparse.ArgumentParser() 16 | 17 | parser.add_argument('--config', is_config_file=True, help='config file path') 18 | parser.add_argument('--save_path', type=str) 19 | parser.add_argument('--t265_id', type=str) 20 | parser.add_argument('--pyft_id', type=str) 21 | parser.add_argument('--pyft_port', type=int) 22 | 23 | args = parser.parse_args() 24 | return args 25 | 26 | 27 | def main(args): 28 | # read tare data 29 | with open(os.path.join(args.save_path, 'tare_pyft.json'), 'r') as f: 30 | tare_data = json.load(f) 31 | for key, value in tare_data.items(): 32 | if isinstance(value, list): 33 | tare_data[key] = np.array(value) 34 | 35 | # create devices 36 | t265 = T265(id=args.t265_id, image=False, name='t265') 37 | pyft = Pyft(id=args.pyft_id, port=args.pyft_port, fps=200, name='pyft') 38 | 39 | # special prepare for t265 to construct map 40 | start = input('Press enter to start T265 construct map') 41 | if start != '': 42 | del t265, pyft 43 | return 44 | print("Start T265 construct map") 45 | t265.collect_streaming(False) 46 | t265.start_streaming() 47 | 48 | # stable t265 pose 49 | stop_start = input('Press enter to stop T265 construct map and start stabling T265 pose') 50 | if stop_start != '': 51 | del t265, pyft 52 | return 53 | print("Stop T265 construct map and start stabling T265 pose") 54 | t265.collect_streaming(True) 55 | t265_pose_start_timestamp_ms = time.time() * 1000 56 | 57 | # mounting t265 58 | stop_start = input('Press enter to stop stabling T265 pose and start mounting T265') 59 | if stop_start != '': 60 | del t265, pyft 61 | return 62 | print("Stop stabling T265 pose and start mounting T265") 63 | t265_pose_end_timestamp_ms = time.time() * 1000 64 | 65 | # NOTE: urgly code to get t265 initial pose 66 | t265.pose_streaming_mutex.acquire() 67 | t265_all_poses = deepcopy(t265.pose_streaming_data) 68 | t265.pose_streaming_mutex.release() 69 | t265_initial_pose_mask = np.logical_and(np.array(t265_all_poses["timestamp_ms"]) > t265_pose_start_timestamp_ms, 70 | np.array(t265_all_poses["timestamp_ms"]) < t265_pose_end_timestamp_ms) 71 | t265_initial_xyzs = np.array(t265_all_poses["xyz"])[t265_initial_pose_mask] 72 | t265_initial_quats = np.array(t265_all_poses["quat"])[t265_initial_pose_mask] 73 | t265_initial_xyz = np.median(t265_initial_xyzs, axis=0) 74 | t265_initial_quat = np.median(t265_initial_quats, axis=0) 75 | t265_initial_pose = T265.raw2pose(t265_initial_xyz, t265_initial_quat) # c02w 76 | 77 | # start streaming 78 | stop_start = input('Press enter to stop mounting T265 and start streaming') 79 | if stop_start != '': 80 | del t265, pyft 81 | return 82 | print("Stop mounting T265 and start streaming") 83 | print("Press enter to stop streaming") 84 | stop = False 85 | def _on_press(key): 86 | nonlocal stop 87 | if key == keyboard.Key.enter: 88 | stop = True 89 | def _on_release(key): 90 | pass 91 | listener = keyboard.Listener(on_press=_on_press, on_release=_on_release) 92 | listener.start() 93 | while not stop: 94 | pyft_ft = pyft.get_mean_data(n=10, name='ft') 95 | _, _, t265_xyz, t265_quat = t265.get() 96 | t265_pose = T265.raw2pose(t265_xyz, t265_quat) # c2w 97 | t265_pose = np.linalg.inv(t265_initial_pose) @ t265_pose # c2c0 = w2c0 @ c2w 98 | t265_pose = T265r_2_BASE @ t265_pose # c2b = c02b @ c2c0 99 | pyft_pose = t265_pose @ np.linalg.inv(T265r_2_PYFT) # f2b = c2b @ f2c 100 | pyft_ft = Pyft.raw2tare(raw_ft=pyft_ft, tare=tare_data, pose=pyft_pose[:3, :3]) 101 | print(np.linalg.norm(pyft_ft[:3]), np.linalg.norm(pyft_ft[3:])) 102 | print(pyft_ft[:3], pyft_ft[3:]) 103 | time.sleep(0.1) 104 | 105 | # stop streaming 106 | print("Stop streaming") 107 | t265.stop_streaming() 108 | listener.stop() 109 | 110 | # destroy devices 111 | del t265, pyft 112 | 113 | 114 | if __name__ == '__main__': 115 | args = config_parse() 116 | main(args) 117 | -------------------------------------------------------------------------------- /collect_data.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | import time 4 | import configargparse 5 | import json 6 | 7 | from r3kit.devices.camera.realsense.t265 import T265 8 | from r3kit.devices.camera.realsense.l515 import L515 9 | from r3kit.devices.ftsensor.ati.pyati import PyATI as Pyft 10 | from r3kit.devices.encoder.pdcd.angler import Angler 11 | 12 | 13 | def config_parse() -> configargparse.Namespace: 14 | parser = configargparse.ArgumentParser() 15 | 16 | parser.add_argument('--config', is_config_file=True, help='config file path') 17 | parser.add_argument('--save_path', type=str) 18 | parser.add_argument('--l515_id', type=str) 19 | parser.add_argument('--t265r_id', type=str) 20 | parser.add_argument('--t265l_id', type=str) 21 | parser.add_argument('--pyft_id', type=str) 22 | parser.add_argument('--pyft_port', type=int) 23 | parser.add_argument('--pyft_tare_path', type=str) 24 | parser.add_argument('--angler_id', type=str) 25 | parser.add_argument('--angler_index', type=int) 26 | 27 | args = parser.parse_args() 28 | return args 29 | 30 | 31 | def main(args): 32 | # create devices 33 | l515 = L515(id=args.l515_id, name='l515') 34 | t265r = T265(id=args.t265r_id, image=False, name='t265r') 35 | t265l = T265(id=args.t265l_id, image=False, name='t265l') 36 | pyft = Pyft(id=args.pyft_id, port=args.pyft_port, fps=200, name='pyft') 37 | angler = Angler(id=args.angler_id, index=args.angler_index, fps=30, name='angler') 38 | 39 | # special prepare to close the gripper for angler to know bias 40 | start = input('Press enter to start closing gripper') 41 | if start != '': 42 | del l515, t265r, t265l, pyft, angler 43 | return 44 | print("Start closing gripper") 45 | 46 | # special prepare for t265 to construct map 47 | stop_start = input('Press enter to stop closing gripper and start T265 construct map') 48 | if stop_start != '': 49 | del l515, t265r, t265l, pyft, angler 50 | return 51 | print("Stop closing gripper and Start T265 construct map") 52 | t265r.collect_streaming(False) 53 | t265l.collect_streaming(False) 54 | t265r.start_streaming() 55 | t265l.start_streaming() 56 | 57 | # stable t265 pose 58 | stop_start = input('Press enter to stop T265 construct map and start stabling T265 pose') 59 | if stop_start != '': 60 | del l515, t265r, t265l, pyft, angler 61 | return 62 | print("Stop T265 construct map and start stabling T265 pose") 63 | t265r.collect_streaming(True) 64 | t265l.collect_streaming(True) 65 | t265_pose_start_timestamp_ms = time.time() * 1000 66 | 67 | # mounting t265 68 | stop_start = input('Press enter to stop stabling T265 pose and start mounting T265') 69 | if stop_start != '': 70 | del l515, t265r, t265l, pyft, angler 71 | return 72 | print("Stop stabling T265 pose and start mounting T265") 73 | t265_pose_end_timestamp_ms = time.time() * 1000 74 | 75 | # start streaming 76 | stop_start = input('Press enter to stop mounting T265 and start streaming') 77 | if stop_start != '': 78 | del l515, t265r, t265l, pyft, angler 79 | return 80 | print("Stop mounting T265 and start streaming") 81 | start_timestamp_ms = time.time() * 1000 82 | l515.start_streaming() 83 | pyft.start_streaming() 84 | angler.start_streaming() 85 | 86 | # collect data 87 | stop = input('Press enter to stop streaming') 88 | if stop != '': 89 | del l515, t265r, t265l, pyft, angler 90 | return 91 | print("Stop streaming") 92 | 93 | # stop streaming 94 | l515_data = l515.stop_streaming() 95 | t265r_data = t265r.stop_streaming() 96 | t265l_data = t265l.stop_streaming() 97 | pyft_data = pyft.stop_streaming() 98 | angler_data = angler.stop_streaming() 99 | 100 | # save data 101 | print("Start saving data") 102 | os.makedirs(args.save_path, exist_ok=True) 103 | with open(os.path.join(args.save_path, 'stage_timestamp_ms.json'), 'w') as f: 104 | json.dump({ 105 | 't265_pose_start_timestamp_ms': t265_pose_start_timestamp_ms, 106 | 't265_pose_end_timestamp_ms': t265_pose_end_timestamp_ms, 107 | 'start_timestamp_ms': start_timestamp_ms 108 | }, f, indent=4) 109 | os.makedirs(os.path.join(args.save_path, 'l515'), exist_ok=True) 110 | l515.save_streaming(os.path.join(args.save_path, 'l515'), l515_data) 111 | os.makedirs(os.path.join(args.save_path, 't265r'), exist_ok=True) 112 | t265r.save_streaming(os.path.join(args.save_path, 't265r'), t265r_data) 113 | os.makedirs(os.path.join(args.save_path, 't265l'), exist_ok=True) 114 | t265l.save_streaming(os.path.join(args.save_path, 't265l'), t265l_data) 115 | os.makedirs(os.path.join(args.save_path, 'pyft'), exist_ok=True) 116 | shutil.copyfile(os.path.join(args.pyft_tare_path, "tare_pyft.json"), os.path.join(args.save_path, 'pyft', "tare_pyft.json")) 117 | pyft.save_streaming(os.path.join(args.save_path, 'pyft'), pyft_data) 118 | os.makedirs(os.path.join(args.save_path, 'angler'), exist_ok=True) 119 | angler.save_streaming(os.path.join(args.save_path, 'angler'), angler_data) 120 | print("Stop saving data") 121 | 122 | # destroy devices 123 | del l515, t265r, t265l, pyft, angler 124 | 125 | 126 | if __name__ == '__main__': 127 | args = config_parse() 128 | main(args) 129 | -------------------------------------------------------------------------------- /tare_pyft.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import configargparse 4 | import json 5 | from pynput import keyboard 6 | import numpy as np 7 | import open3d as o3d 8 | 9 | from r3kit.devices.camera.realsense.t265 import T265 10 | from r3kit.devices.ftsensor.ati.pyati import PyATI as Pyft 11 | from r3kit.algos.tare.linear import LinearMFTarer, LinearFTarer, LinearCTTarer 12 | from configs.pose import * 13 | 14 | 15 | def config_parse() -> configargparse.Namespace: 16 | parser = configargparse.ArgumentParser() 17 | 18 | parser.add_argument('--config', is_config_file=True, help='config file path') 19 | parser.add_argument('--save_path', type=str) 20 | parser.add_argument('--t265_id', type=str) 21 | parser.add_argument('--pyft_id', type=str) 22 | parser.add_argument('--pyft_port', type=int) 23 | 24 | args = parser.parse_args() 25 | return args 26 | 27 | 28 | def main(args): 29 | if not os.path.exists(args.save_path): 30 | # create devices 31 | t265 = T265(id=args.t265_id, image=False, name='t265') 32 | pyft = Pyft(id=args.pyft_id, port=args.pyft_port, fps=200, name='pyft') 33 | 34 | # special prepare for t265 to construct map 35 | start = input('Press enter to start T265 construct map') 36 | if start != '': 37 | del t265, pyft 38 | return 39 | print("Start T265 construct map") 40 | t265.collect_streaming(False) 41 | t265.start_streaming() 42 | 43 | # stable t265 pose 44 | stop_start = input('Press enter to stop T265 construct map and start stabling T265 pose') 45 | if stop_start != '': 46 | del t265, pyft 47 | return 48 | print("Stop T265 construct map and start stabling T265 pose") 49 | t265.collect_streaming(True) 50 | t265_pose_start_timestamp_ms = time.time() * 1000 51 | 52 | # mounting t265 53 | stop_start = input('Press enter to stop stabling T265 pose and start mounting T265') 54 | if stop_start != '': 55 | del t265, pyft 56 | return 57 | print("Stop stabling T265 pose and start mounting T265") 58 | t265_pose_end_timestamp_ms = time.time() * 1000 59 | 60 | # start streaming 61 | stop_start = input('Press enter to stop mounting T265 and start streaming') 62 | if stop_start != '': 63 | del t265, pyft 64 | return 65 | print("Stop mounting T265 and start streaming") 66 | pyft_fts, t265_poses = [], [] 67 | print("Press enter to stop streaming") 68 | stop = False 69 | def _on_press(key): 70 | nonlocal stop 71 | if key == keyboard.Key.enter: 72 | stop = True 73 | def _on_release(key): 74 | pass 75 | listener = keyboard.Listener(on_press=_on_press, on_release=_on_release) 76 | listener.start() 77 | while not stop: 78 | pyft_ft = pyft.get_mean_data(n=10, name='ft') 79 | _, _, t265_xyz, t265_quat = t265.get() 80 | t265_pose = T265.raw2pose(t265_xyz, t265_quat) 81 | pyft_fts.append(pyft_ft) 82 | t265_poses.append(t265_pose) 83 | 84 | # stop streaming 85 | print("Stop streaming") 86 | t265_data = t265.stop_streaming() 87 | listener.stop() 88 | 89 | # destroy devices 90 | del t265, pyft 91 | 92 | # NOTE: urgly code to save data 93 | os.makedirs(args.save_path, exist_ok=True) 94 | np.save(os.path.join(args.save_path, 't265_timestamp.npy'), np.array(t265_data['pose']["timestamp_ms"])) 95 | np.savetxt(os.path.join(args.save_path, 't265_stage_timestamps.txt'), np.array([t265_pose_start_timestamp_ms, t265_pose_end_timestamp_ms])) 96 | np.save(os.path.join(args.save_path, 't265_xyz.npy'), np.array(t265_data['pose']["xyz"])) 97 | np.save(os.path.join(args.save_path, 't265_quat.npy'), np.array(t265_data['pose']["quat"])) 98 | np.save(os.path.join(args.save_path, 't265_poses.npy'), np.array(t265_poses)) 99 | np.save(os.path.join(args.save_path, 'pyft_fts.npy'), np.array(pyft_fts)) 100 | else: 101 | # reload data 102 | t265_data = { 103 | 'pose': { 104 | 'timestamp_ms': np.load(os.path.join(args.save_path, 't265_timestamp.npy')), 105 | 'xyz': np.load(os.path.join(args.save_path, 't265_xyz.npy')), 106 | 'quat': np.load(os.path.join(args.save_path, 't265_quat.npy')) 107 | } 108 | } 109 | t265_pose_start_timestamp_ms, t265_pose_end_timestamp_ms = np.loadtxt(os.path.join(args.save_path, 't265_stage_timestamps.txt')) 110 | t265_poses = np.load(os.path.join(args.save_path, 't265_poses.npy')) 111 | pyft_fts = np.load(os.path.join(args.save_path, 'pyft_fts.npy')) 112 | 113 | # tare 114 | t265_all_poses = t265_data['pose'] 115 | t265_initial_pose_mask = np.logical_and(np.array(t265_all_poses["timestamp_ms"]) > t265_pose_start_timestamp_ms, 116 | np.array(t265_all_poses["timestamp_ms"]) < t265_pose_end_timestamp_ms) 117 | t265_initial_xyzs = np.array(t265_all_poses["xyz"])[t265_initial_pose_mask] 118 | t265_initial_quats = np.array(t265_all_poses["quat"])[t265_initial_pose_mask] 119 | t265_initial_xyz = np.median(t265_initial_xyzs, axis=0) 120 | t265_initial_quat = np.median(t265_initial_quats, axis=0) 121 | t265_initial_pose = T265.raw2pose(t265_initial_xyz, t265_initial_quat) # c02w 122 | pyft_poses = [] 123 | for t265_pose in t265_poses: # c2w 124 | t265_pose = np.linalg.inv(t265_initial_pose) @ t265_pose # c2c0 = w2c0 @ c2w 125 | t265_pose = T265r_2_BASE @ t265_pose # c2b = c02b @ c2c0 126 | pyft_pose = t265_pose @ np.linalg.inv(T265r_2_PYFT) # f2b = c2b @ f2c 127 | pyft_poses.append(pyft_pose) 128 | mftarer = LinearMFTarer() 129 | for pyft_ft, pyft_pose in zip(pyft_fts, pyft_poses): 130 | mftarer.add_data(pyft_ft[:3], pyft_pose[:3, :3]) 131 | result = mftarer.run() 132 | ftarer = LinearFTarer() 133 | ftarer.set_m(result['m']) 134 | for pyft_ft, pyft_pose in zip(pyft_fts, pyft_poses): 135 | ftarer.add_data(pyft_ft[:3], pyft_pose[:3, :3]) 136 | result.update(ftarer.run()) 137 | ctarer = LinearCTTarer() 138 | ctarer.set_m(result['m']) 139 | for pyft_ft, pyft_pose in zip(pyft_fts, pyft_poses): 140 | ctarer.add_data(pyft_ft[3:], pyft_pose[:3, :3]) 141 | result.update(ctarer.run()) 142 | for key, value in result.items(): 143 | if isinstance(value, np.ndarray): 144 | result[key] = value.tolist() 145 | print(result) 146 | 147 | # save data 148 | os.makedirs(args.save_path, exist_ok=True) 149 | with open(os.path.join(args.save_path, 'tare_pyft.json'), 'w') as f: 150 | json.dump(result, f, indent=4) 151 | 152 | # visualize 153 | geometries = [] 154 | base_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1, origin=[0, 0, 0]) 155 | geometries.append(base_frame) 156 | for pyft_pose in pyft_poses[::(len(pyft_poses) // 200)]: 157 | pyft_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.02, origin=[0, 0, 0]) 158 | pyft_frame.transform(pyft_pose) 159 | geometries.append(pyft_frame) 160 | o3d.visualization.draw_geometries(geometries) 161 | 162 | 163 | if __name__ == '__main__': 164 | args = config_parse() 165 | main(args) 166 | -------------------------------------------------------------------------------- /merge_hdf5.py: -------------------------------------------------------------------------------- 1 | import os 2 | import configargparse 3 | import numpy as np 4 | import tqdm 5 | import h5py 6 | 7 | from utils.transformation import delta_xyz, delta_quat 8 | 9 | 10 | def config_parse() -> configargparse.Namespace: 11 | parser = configargparse.ArgumentParser() 12 | 13 | parser.add_argument('--config', is_config_file=True, help='config file path') 14 | parser.add_argument('--data_path', type=str) 15 | parser.add_argument('--save_path', type=str) 16 | parser.add_argument('--stage', type=str) 17 | parser.add_argument('--pc_mesh', action='store_true') 18 | parser.add_argument('--ft_coord', action='store_true') 19 | parser.add_argument('--num_o', type=int) 20 | parser.add_argument('--num_a', type=int) 21 | parser.add_argument('--num_aa', type=int) 22 | parser.add_argument('--pad_o', action='store_true') 23 | parser.add_argument('--pad_a', action='store_true') 24 | parser.add_argument('--pad_aa', action='store_true') 25 | parser.add_argument('--gripper_xyz_threshold', type=float) 26 | parser.add_argument('--gripper_quat_threshold', type=float) 27 | parser.add_argument('--pyft_xyz_threshold', type=float) 28 | parser.add_argument('--pyft_quat_threshold', type=float) 29 | parser.add_argument('--pyft_f_threshold', type=float) 30 | parser.add_argument('--pyft_t_threshold', type=float) 31 | 32 | args = parser.parse_args() 33 | return args 34 | 35 | 36 | def main(args): 37 | # general config 38 | data_path = args.data_path 39 | save_path = args.save_path 40 | stage = args.stage 41 | pc_mesh = args.pc_mesh 42 | ft_coord = args.ft_coord 43 | num_o = args.num_o 44 | num_a = args.num_a 45 | num_aa = args.num_aa 46 | pad_o = args.pad_o 47 | pad_a = args.pad_a 48 | pad_aa = args.pad_aa 49 | filter_thresholds = { 50 | 'gripper_xyz_threshold': args.gripper_xyz_threshold, 51 | 'gripper_quat_threshold': args.gripper_quat_threshold, 52 | 'pyft_xyz_threshold': args.pyft_xyz_threshold, 53 | 'pyft_quat_threshold': args.pyft_quat_threshold, 54 | 'pyft_f_threshold': args.pyft_f_threshold, 55 | 'pyft_t_threshold': args.pyft_t_threshold 56 | } 57 | hdf5_paths = [] 58 | for trial_name in sorted(os.listdir(data_path)): 59 | if not os.path.isdir(os.path.join(data_path, trial_name)): 60 | continue 61 | for file_name in sorted(os.listdir(os.path.join(data_path, trial_name))): 62 | if not file_name.endswith('.hdf5'): 63 | continue 64 | if stage == 'all' or stage in file_name: 65 | hdf5_paths.append(os.path.join(data_path, trial_name, file_name)) 66 | 67 | # hdf5 loop 68 | with h5py.File(save_path, 'w') as save_hdf5: 69 | save_hdf5_data_group = save_hdf5.create_group('data') 70 | save_hdf5_data_group.attrs['pc_mesh'] = pc_mesh 71 | save_hdf5_data_group.attrs['ft_coord'] = ft_coord 72 | save_hdf5_data_group.attrs['num_o'] = num_o 73 | save_hdf5_data_group.attrs['num_a'] = num_a 74 | save_hdf5_data_group.attrs['num_aa'] = num_aa 75 | save_hdf5_data_group.attrs['pad_o'] = pad_o 76 | save_hdf5_data_group.attrs['pad_a'] = pad_a 77 | save_hdf5_data_group.attrs['pad_aa'] = pad_aa 78 | for k, v in filter_thresholds.items(): 79 | save_hdf5_data_group.attrs[k] = v 80 | 81 | total_samples = 0 82 | for hdf5_path in tqdm.tqdm(hdf5_paths): 83 | whole_demo_name = os.path.basename(os.path.dirname(hdf5_path)) 84 | stage_name = os.path.splitext(os.path.basename(hdf5_path))[0] 85 | 86 | # load hdf5 87 | with h5py.File(hdf5_path, 'r') as data_hdf5: 88 | data_group = data_hdf5['data'] 89 | num_samples = data_group.attrs['num_samples'] 90 | 91 | data_o_group = data_group['o'] 92 | data_o_attrs = dict(data_o_group.attrs) 93 | if pc_mesh: 94 | l515_pc_xyzs_l515_o = data_o_group['l515_pc_xyzs_l515_mesh'][:].astype(np.float32) 95 | l515_pc_rgbs_o = data_o_group['l515_pc_rgbs_mesh'][:].astype(np.float32) 96 | else: 97 | l515_pc_xyzs_l515_o = data_o_group['l515_pc_xyzs_l515'][:].astype(np.float32) 98 | l515_pc_rgbs_o = data_o_group['l515_pc_rgbs'][:].astype(np.float32) 99 | gripper_xyzs_l515_o = data_o_group['gripper_xyzs_l515'][:].astype(np.float32) 100 | gripper_quats_l515_o = data_o_group['gripper_quats_l515'][:].astype(np.float32) 101 | pyft_xyzs_l515_o = data_o_group['pyft_xyzs_l515'][:].astype(np.float32) 102 | pyft_quats_l515_o = data_o_group['pyft_quats_l515'][:].astype(np.float32) 103 | if ft_coord: 104 | pyft_fs_o = data_o_group['pyft_fs_l515'][:].astype(np.float32) 105 | pyft_ts_o = data_o_group['pyft_ts_l515'][:].astype(np.float32) 106 | else: 107 | pyft_fs_o = data_o_group['pyft_fs_pyft'][:].astype(np.float32) 108 | pyft_ts_o = data_o_group['pyft_ts_pyft'][:].astype(np.float32) 109 | angler_widths_o = data_o_group['angler_widths'][:].astype(np.float32) 110 | 111 | # delta filter 112 | if sum(filter_thresholds.values()) != 0: 113 | lidx, ridx = 0, 0 114 | selected_idxs = [0] 115 | while ridx < num_samples: 116 | delta_gripper_xyz = delta_xyz(gripper_xyzs_l515_o[ridx], gripper_xyzs_l515_o[lidx]) 117 | delta_gripper_quat = delta_quat(gripper_quats_l515_o[ridx], gripper_quats_l515_o[lidx]) / np.pi * 180 118 | delta_pyft_xyz = delta_xyz(pyft_xyzs_l515_o[ridx], pyft_xyzs_l515_o[lidx]) 119 | delta_pyft_quat = delta_quat(pyft_quats_l515_o[ridx], pyft_quats_l515_o[lidx]) / np.pi * 180 120 | delta_pyft_f = delta_xyz(pyft_fs_o[ridx], pyft_fs_o[lidx]) 121 | delta_pyft_t = delta_xyz(pyft_ts_o[ridx], pyft_ts_o[lidx]) 122 | if (delta_gripper_xyz > filter_thresholds['gripper_xyz_threshold'] and filter_thresholds['gripper_xyz_threshold'] != 0) or \ 123 | (delta_gripper_quat > filter_thresholds['gripper_quat_threshold'] and filter_thresholds['gripper_quat_threshold'] != 0) or \ 124 | (delta_pyft_xyz > filter_thresholds['pyft_xyz_threshold'] and filter_thresholds['pyft_xyz_threshold'] != 0) or \ 125 | (delta_pyft_quat > filter_thresholds['pyft_quat_threshold'] and filter_thresholds['pyft_quat_threshold'] != 0) or \ 126 | (delta_pyft_f > filter_thresholds['pyft_f_threshold'] and filter_thresholds['pyft_f_threshold'] != 0) or \ 127 | (delta_pyft_t > filter_thresholds['pyft_t_threshold'] and filter_thresholds['pyft_t_threshold'] != 0): 128 | selected_idxs.append(ridx) 129 | lidx = ridx 130 | ridx += 1 131 | l515_pc_xyzs_l515_o = l515_pc_xyzs_l515_o[selected_idxs] 132 | l515_pc_rgbs_o = l515_pc_rgbs_o[selected_idxs] 133 | gripper_xyzs_l515_o = gripper_xyzs_l515_o[selected_idxs] 134 | gripper_quats_l515_o = gripper_quats_l515_o[selected_idxs] 135 | pyft_xyzs_l515_o = pyft_xyzs_l515_o[selected_idxs] 136 | pyft_quats_l515_o = pyft_quats_l515_o[selected_idxs] 137 | pyft_fs_o = pyft_fs_o[selected_idxs] 138 | pyft_ts_o = pyft_ts_o[selected_idxs] 139 | angler_widths_o = angler_widths_o[selected_idxs] 140 | filter_ratio = len(selected_idxs) / num_samples 141 | print(filter_ratio) 142 | num_samples = len(selected_idxs) 143 | 144 | # save data 145 | demo_name = f'demo_{whole_demo_name}_{stage_name}' 146 | save_hdf5_demo_group = save_hdf5_data_group.create_group(demo_name) 147 | save_hdf5_demo_group.create_dataset('l515_pc_xyzs_l515', data=l515_pc_xyzs_l515_o, dtype=np.float32) 148 | save_hdf5_demo_group.create_dataset('l515_pc_rgbs', data=l515_pc_rgbs_o, dtype=np.float32) 149 | save_hdf5_demo_group.create_dataset('gripper_xyzs_l515', data=gripper_xyzs_l515_o, dtype=np.float32) 150 | save_hdf5_demo_group.create_dataset('gripper_quats_l515', data=gripper_quats_l515_o, dtype=np.float32) 151 | save_hdf5_demo_group.create_dataset('pyft_xyzs_l515', data=pyft_xyzs_l515_o, dtype=np.float32) 152 | save_hdf5_demo_group.create_dataset('pyft_quats_l515', data=pyft_quats_l515_o, dtype=np.float32) 153 | save_hdf5_demo_group.create_dataset('pyft_fs', data=pyft_fs_o, dtype=np.float32) 154 | save_hdf5_demo_group.create_dataset('pyft_ts', data=pyft_ts_o, dtype=np.float32) 155 | save_hdf5_demo_group.create_dataset('angler_widths', data=angler_widths_o, dtype=np.float32) 156 | 157 | # save sample 158 | o_idxs, a_idxs = [], [] 159 | for current_idx in range(num_samples - 1): 160 | selected = True 161 | 162 | o_begin_idx = max(0, current_idx - num_o + 1) 163 | o_end_idx = min(num_samples, current_idx + 1) 164 | o_padding = num_o - (o_end_idx - o_begin_idx) 165 | if o_padding > 0: 166 | if pad_o: 167 | o_selected_idxs = [0] * o_padding + list(range(o_begin_idx, o_end_idx)) 168 | else: 169 | selected = False 170 | else: 171 | o_selected_idxs = list(range(o_begin_idx, o_end_idx)) 172 | 173 | a_begin_idx = min(num_samples - 1, current_idx + 1) 174 | a_end_idx = min(num_samples, current_idx + 1 + num_a) 175 | a_padding = num_a - (a_end_idx - a_begin_idx) 176 | if a_padding > 0: 177 | if pad_a and pad_aa: 178 | if a_padding > num_a - num_aa: 179 | selected = False 180 | else: 181 | a_selected_idxs = list(range(a_begin_idx, a_end_idx)) + [num_samples - 1] * a_padding 182 | elif pad_a and not pad_aa: 183 | a_selected_idxs = list(range(a_begin_idx, a_end_idx)) + [num_samples - 1] * a_padding 184 | else: 185 | selected = False 186 | else: 187 | a_selected_idxs = list(range(a_begin_idx, a_end_idx)) 188 | 189 | if selected: 190 | o_idxs.append(o_selected_idxs) 191 | a_idxs.append(a_selected_idxs) 192 | save_hdf5_demo_group.create_dataset('o', data=np.array(o_idxs, dtype=int), dtype=int) 193 | save_hdf5_demo_group.create_dataset('a', data=np.array(a_idxs, dtype=int), dtype=int) 194 | 195 | # save attributes 196 | save_hdf5_demo_group.attrs['num_samples'] = len(o_idxs) 197 | for k, v in data_o_attrs.items(): 198 | save_hdf5_demo_group.attrs[k] = v 199 | total_samples += len(o_idxs) 200 | if sum(filter_thresholds.values()) != 0: 201 | save_hdf5_demo_group.attrs['filter_ratio'] = filter_ratio 202 | 203 | # save attributes 204 | save_hdf5_data_group.attrs['num_samples'] = total_samples 205 | print(f'{total_samples = }') 206 | 207 | 208 | if __name__ == '__main__': 209 | args = config_parse() 210 | main(args) 211 | -------------------------------------------------------------------------------- /visualize_hdf5.py: -------------------------------------------------------------------------------- 1 | import os 2 | import configargparse 3 | import numpy as np 4 | import open3d as o3d 5 | from pynput import keyboard 6 | import tqdm 7 | import h5py 8 | 9 | from r3kit.utils.vis import rotation_vec2mat 10 | from utils.transformation import xyzquat2mat 11 | 12 | ''' 13 | Synchronize with `visualize_merge.py` some part 14 | ''' 15 | 16 | 17 | def config_parse() -> configargparse.Namespace: 18 | parser = configargparse.ArgumentParser() 19 | 20 | parser.add_argument('--config', is_config_file=True, help='config file path') 21 | parser.add_argument('--hdf5_path', type=str) 22 | parser.add_argument('--pc_mesh', action='store_true') 23 | 24 | args = parser.parse_args() 25 | return args 26 | 27 | 28 | def main(args): 29 | # general config 30 | hdf5_path = args.hdf5_path 31 | pc_mesh = args.pc_mesh 32 | 33 | # load hdf5 34 | with h5py.File(hdf5_path, 'r') as data_hdf5: 35 | data_group = data_hdf5['data'] 36 | num_samples = data_group.attrs['num_samples'] 37 | data_o_group = data_group['o'] 38 | if pc_mesh: 39 | l515_pc_xyzs_l515 = data_o_group['l515_pc_xyzs_l515_mesh'][:].astype(np.float32) 40 | l515_pc_rgbs = data_o_group['l515_pc_rgbs_mesh'][:].astype(np.float32) 41 | else: 42 | l515_pc_xyzs_l515 = data_o_group['l515_pc_xyzs_l515'][:].astype(np.float32) 43 | l515_pc_rgbs = data_o_group['l515_pc_rgbs'][:].astype(np.float32) 44 | gripper_xyzs_l515 = data_o_group['gripper_xyzs_l515'][:].astype(np.float32) 45 | gripper_quats_l515 = data_o_group['gripper_quats_l515'][:].astype(np.float32) 46 | pyft_xyzs_l515 = data_o_group['pyft_xyzs_l515'][:].astype(np.float32) 47 | pyft_quats_l515 = data_o_group['pyft_quats_l515'][:].astype(np.float32) 48 | pyft_fs_pyft = data_o_group['pyft_fs_pyft'][:].astype(np.float32) 49 | pyft_ts_pyft = data_o_group['pyft_ts_pyft'][:].astype(np.float32) 50 | angler_widths = data_o_group['angler_widths'][:].astype(np.float32) 51 | 52 | # create keyboard listener 53 | quit = False 54 | reset = False 55 | pause = True 56 | zero = False 57 | forward = False 58 | backward = False 59 | speed = 1 60 | def _on_press(key): 61 | nonlocal quit, reset, pause, zero, forward, backward, speed 62 | if hasattr(key, 'char') and key.char == 'q': 63 | quit = True 64 | print("quit") 65 | if hasattr(key, 'char') and key.char == 'r': 66 | reset = True 67 | print("reset") 68 | if hasattr(key, 'char') and key.char == 'p': 69 | pause = not pause 70 | forward = False 71 | backward = False 72 | print("pause" if pause else "continue") 73 | if key == keyboard.Key.backspace: 74 | zero = True 75 | print("zero") 76 | if pause and key == keyboard.Key.right: 77 | forward = True 78 | print("forward") 79 | if pause and key == keyboard.Key.left: 80 | backward = True 81 | print("backward") 82 | if key == keyboard.Key.up: 83 | speed *= 2 84 | print(f"speed {speed}") 85 | if key == keyboard.Key.down: 86 | speed //= 2 87 | speed = max(speed, 1) 88 | print(f"speed {speed}") 89 | def _on_release(key): 90 | pass 91 | listener = keyboard.Listener(on_press=_on_press, on_release=_on_release) 92 | listener.start() 93 | 94 | # process variables 95 | current_idx = 0 96 | 97 | # create visualizer 98 | visualizer = o3d.visualization.Visualizer() 99 | visualizer.create_window(width=1280, height=720, left=200, top=200, visible=True, window_name='data') 100 | 101 | # add l515 elements 102 | l515_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 103 | visualizer.add_geometry(l515_frame) 104 | l515_pc_xyz_l515, l515_pc_rgb = l515_pc_xyzs_l515[current_idx], l515_pc_rgbs[current_idx] 105 | l515_pcd = o3d.geometry.PointCloud() 106 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 107 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 108 | visualizer.add_geometry(l515_pcd) 109 | # add gripper elements 110 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 111 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 112 | gripper_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 113 | gripper_frame.transform(gripper_pose_l515) 114 | visualizer.add_geometry(gripper_frame) 115 | gripper = o3d.io.read_triangle_mesh(os.path.join("objs", "gripper.obj")) 116 | gripper.transform(gripper_pose_l515) 117 | visualizer.add_geometry(gripper) 118 | # add pyft elements 119 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 120 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 121 | pyft_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 122 | pyft_frame.transform(pyft_pose_l515) 123 | visualizer.add_geometry(pyft_frame) 124 | pyft_f_pyft, pyft_t_pyft = pyft_fs_pyft[current_idx], pyft_ts_pyft[current_idx] 125 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 126 | pyft_f_value = np.linalg.norm(pyft_f_l515) 127 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 128 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 129 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 130 | pyft_t_value = np.linalg.norm(pyft_t_l515) 131 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 132 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 133 | pyft_f_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.04 * 0.025, cone_radius=0.04 * 0.05, cylinder_height=0.04 * 0.875, cone_height=0.04 * 0.125, 134 | resolution=20, cylinder_split=4, cone_split=1) 135 | pyft_f_arrow.paint_uniform_color([1., 1., 0.]) 136 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 137 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 138 | pyft_f_arrow.translate(pyft_f_translation_l515) 139 | visualizer.add_geometry(pyft_f_arrow) 140 | pyft_t_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.4 * 0.025, cone_radius=0.4 * 0.05, cylinder_height=0.4 * 0.875, cone_height=0.4 * 0.125, 141 | resolution=20, cylinder_split=4, cone_split=1) 142 | pyft_t_arrow.paint_uniform_color([0., 1., 1.]) 143 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 144 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 145 | pyft_t_arrow.translate(pyft_t_translation_l515) 146 | visualizer.add_geometry(pyft_t_arrow) 147 | pyft_peeler = o3d.io.read_triangle_mesh(os.path.join("objs", "peeler.obj")) 148 | pyft_peeler.transform(pyft_pose_l515) 149 | visualizer.add_geometry(pyft_peeler) 150 | # add angler elements 151 | angler_width = angler_widths[current_idx] 152 | angler_right_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "right_finger.obj")) 153 | angler_left_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "left_finger.obj")) 154 | angler_finger_pose_gripper = np.identity(4) 155 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 156 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 157 | angler_right_finger.transform(gripper_right_finger_pose_l515) 158 | visualizer.add_geometry(angler_right_finger) 159 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 160 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 161 | angler_left_finger.transform(gripper_left_finger_pose_l515) 162 | visualizer.add_geometry(angler_left_finger) 163 | 164 | # visualizer setup 165 | view_control = visualizer.get_view_control() 166 | visualizer.get_render_option().background_color = [0, 0, 0] 167 | params = view_control.convert_to_pinhole_camera_parameters() 168 | params.extrinsic = np.array([[1, 0, 0, 0], 169 | [0, 1, 0, 0], 170 | [0, 0, 1, 0], 171 | [0, 0, 0, 1]]) 172 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 173 | 174 | # visualize loop 175 | gripper_xyz_deltas, gripper_quat_deltas, pyft_xyz_deltas, pyft_quat_deltas, pyft_f_deltas, pyft_t_deltas, angler_width_deltas = [], [], [], [], [], [], [] 176 | with tqdm.tqdm(total=num_samples) as pbar: 177 | while current_idx < num_samples: 178 | # update l515 elements 179 | l515_pc_xyz_l515, l515_pc_rgb = l515_pc_xyzs_l515[current_idx], l515_pc_rgbs[current_idx] 180 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 181 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 182 | visualizer.update_geometry(l515_pcd) 183 | # update gripper elements 184 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 185 | gripper_pose_l515_last = gripper_pose_l515.copy() 186 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 187 | gripper_frame.transform(np.linalg.inv(gripper_pose_l515_last)) 188 | gripper_frame.transform(gripper_pose_l515) 189 | visualizer.update_geometry(gripper_frame) 190 | gripper.transform(np.linalg.inv(gripper_pose_l515_last)) 191 | gripper.transform(gripper_pose_l515) 192 | gripper_delta_pose = np.dot(np.linalg.inv(gripper_pose_l515_last), gripper_pose_l515) 193 | visualizer.update_geometry(gripper) 194 | # update pyft elements 195 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 196 | pyft_pose_l515_last = pyft_pose_l515.copy() 197 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 198 | pyft_frame.transform(np.linalg.inv(pyft_pose_l515_last)) 199 | pyft_frame.transform(pyft_pose_l515) 200 | pyft_delta_pose = np.dot(np.linalg.inv(pyft_pose_l515_last), pyft_pose_l515) 201 | visualizer.update_geometry(pyft_frame) 202 | pyft_f_pyft_last, pyft_t_pyft_last = pyft_f_pyft.copy(), pyft_t_pyft.copy() 203 | pyft_f_pyft, pyft_t_pyft = pyft_fs_pyft[current_idx], pyft_ts_pyft[current_idx] 204 | pyft_delta_f, pyft_delta_t = pyft_f_pyft - pyft_f_pyft_last, pyft_t_pyft - pyft_t_pyft_last 205 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 206 | pyft_f_value_last = pyft_f_value.copy() 207 | pyft_f_value = np.linalg.norm(pyft_f_l515) 208 | pyft_f_rotation_l515_last = pyft_f_rotation_l515.copy() 209 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 210 | pyft_f_translation_l515_last = pyft_f_translation_l515.copy() 211 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 212 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 213 | pyft_t_value_last = pyft_t_value.copy() 214 | pyft_t_value = np.linalg.norm(pyft_t_l515) 215 | pyft_t_rotation_l515_last = pyft_t_rotation_l515.copy() 216 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 217 | pyft_t_translation_l515_last = pyft_t_translation_l515.copy() 218 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 219 | pyft_f_arrow.translate(-pyft_f_translation_l515_last) 220 | pyft_f_arrow.rotate(np.linalg.inv(pyft_f_rotation_l515_last), np.array([[0], [0], [0]])) 221 | pyft_f_arrow.scale(1/pyft_f_value_last, np.array([[0], [0], [0]])) 222 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 223 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 224 | pyft_f_arrow.translate(pyft_f_translation_l515) 225 | visualizer.update_geometry(pyft_f_arrow) 226 | pyft_t_arrow.translate(-pyft_t_translation_l515_last) 227 | pyft_t_arrow.rotate(np.linalg.inv(pyft_t_rotation_l515_last), np.array([[0], [0], [0]])) 228 | pyft_t_arrow.scale(1/pyft_t_value_last, np.array([[0], [0], [0]])) 229 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 230 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 231 | pyft_t_arrow.translate(pyft_t_translation_l515) 232 | visualizer.update_geometry(pyft_t_arrow) 233 | pyft_peeler.transform(np.linalg.inv(pyft_pose_l515_last)) 234 | pyft_peeler.transform(pyft_pose_l515) 235 | visualizer.update_geometry(pyft_peeler) 236 | # update angler elements 237 | angler_width_last = angler_width.copy() 238 | angler_width = angler_widths[current_idx] 239 | angler_finger_pose_gripper = np.identity(4) 240 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 241 | gripper_right_finger_pose_l515_last = gripper_right_finger_pose_l515.copy() 242 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 243 | angler_right_finger.transform(np.linalg.inv(gripper_right_finger_pose_l515_last)) 244 | angler_right_finger.transform(gripper_right_finger_pose_l515) 245 | visualizer.update_geometry(angler_right_finger) 246 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 247 | gripper_left_finger_pose_l515_last = gripper_left_finger_pose_l515.copy() 248 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 249 | angler_left_finger.transform(np.linalg.inv(gripper_left_finger_pose_l515_last)) 250 | angler_left_finger.transform(gripper_left_finger_pose_l515) 251 | visualizer.update_geometry(angler_left_finger) 252 | 253 | # visualizer update 254 | visualizer.poll_events() 255 | visualizer.update_renderer() 256 | 257 | # pbar update 258 | current_idx_last = current_idx 259 | if not pause: 260 | current_idx += speed 261 | pbar.update(speed) 262 | else: 263 | if forward: 264 | current_idx += speed 265 | pbar.update(speed) 266 | forward = False 267 | elif backward: 268 | current_idx -= speed 269 | pbar.update(-speed) 270 | backward = False 271 | else: 272 | pass 273 | pbar.set_postfix(f=pyft_f_value, t=pyft_t_value) 274 | 275 | # keyboard quit 276 | if quit: 277 | break 278 | # keyboard reset 279 | if reset: 280 | view_control = visualizer.get_view_control() 281 | params = view_control.convert_to_pinhole_camera_parameters() 282 | params.extrinsic = np.array([[1, 0, 0, 0], 283 | [0, 1, 0, 0], 284 | [0, 0, 1, 0], 285 | [0, 0, 0, 1]]) 286 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 287 | reset = False 288 | # keyboard zero 289 | if zero: 290 | pbar.update(-current_idx) 291 | current_idx = 0 292 | zero = False 293 | 294 | if current_idx != current_idx_last: 295 | gripper_xyz_delta_mm = np.linalg.norm(gripper_delta_pose[:3, 3]) * 1000 296 | gripper_quat_delta_deg = np.arccos(np.clip((np.trace(gripper_delta_pose[:3, :3]) - 1) / 2, -1, 1)) / np.pi * 180 297 | pyft_xyz_delta_mm = np.linalg.norm(pyft_delta_pose[:3, 3]) * 1000 298 | pyft_quat_delta_deg = np.arccos(np.clip((np.trace(pyft_delta_pose[:3, :3]) - 1) / 2, -1, 1)) / np.pi * 180 299 | angler_width_delta_mm = abs(angler_width - angler_width_last) * 1000 300 | pyft_f_delta_n = np.linalg.norm(pyft_delta_f) 301 | pyft_t_delta = np.linalg.norm(pyft_delta_t) 302 | print(gripper_xyz_delta_mm) 303 | 304 | gripper_xyz_deltas.append(gripper_xyz_delta_mm) 305 | gripper_quat_deltas.append(gripper_quat_delta_deg) 306 | pyft_xyz_deltas.append(pyft_xyz_delta_mm) 307 | pyft_quat_deltas.append(pyft_quat_delta_deg) 308 | angler_width_deltas.append(angler_width_delta_mm) 309 | pyft_f_deltas.append(pyft_f_delta_n) 310 | pyft_t_deltas.append(pyft_t_delta) 311 | 312 | visualizer.destroy_window() 313 | listener.stop() 314 | 315 | # import pdb; pdb.set_trace() 316 | 317 | 318 | if __name__ == '__main__': 319 | args = config_parse() 320 | main(args) 321 | -------------------------------------------------------------------------------- /create_hdf5.py: -------------------------------------------------------------------------------- 1 | import os 2 | import configargparse 3 | import json 4 | import numpy as np 5 | from scipy.spatial.transform import Rotation as Rot 6 | import cv2 7 | import tqdm 8 | import h5py 9 | 10 | from r3kit.devices.camera.realsense.t265 import T265 11 | from r3kit.devices.camera.realsense.l515 import L515 12 | from r3kit.devices.ftsensor.ati.pyati import PyATI as Pyft 13 | from r3kit.devices.encoder.pdcd.angler import Angler 14 | from configs.pose import * 15 | from utils.annotation import search_stage 16 | from utils.transformation import transform_pc 17 | from utils.process import voxelize, mesh2pc 18 | 19 | ''' 20 | Synchronize with `visualize_data.py` some part 21 | ''' 22 | 23 | 24 | def config_parse() -> configargparse.Namespace: 25 | parser = configargparse.ArgumentParser() 26 | 27 | parser.add_argument('--config', is_config_file=True, help='config file path') 28 | parser.add_argument('--data_path', type=str) 29 | parser.add_argument('--save_path', type=str) 30 | parser.add_argument('--clip_object', action='store_true') 31 | parser.add_argument('--clip_gripper', action='store_true') 32 | parser.add_argument('--clip_pyft', action='store_true') 33 | parser.add_argument('--clip_base', action='store_true') 34 | parser.add_argument('--render_gripper_num', type=int) 35 | parser.add_argument('--render_pyft_num', type=int) 36 | parser.add_argument('--voxel_size', type=float) 37 | parser.add_argument('--pc_num', type=int) 38 | 39 | args = parser.parse_args() 40 | return args 41 | 42 | 43 | def main(args): 44 | # general config 45 | data_path = args.data_path 46 | save_path = args.save_path 47 | clip_object = args.clip_object 48 | clip_gripper = args.clip_gripper 49 | clip_pyft = args.clip_pyft 50 | clip_base = args.clip_base 51 | render_gripper_num = args.render_gripper_num 52 | render_pyft_num = args.render_pyft_num 53 | voxel_size = args.voxel_size 54 | pc_num = args.pc_num 55 | 56 | # load stage data 57 | with open(os.path.join(data_path, 'stage_timestamp_ms.json'), 'r') as f: 58 | stage_timestamp_ms = json.load(f) 59 | t265_pose_start_timestamp_ms = stage_timestamp_ms['t265_pose_start_timestamp_ms'] 60 | t265_pose_end_timestamp_ms = stage_timestamp_ms['t265_pose_end_timestamp_ms'] 61 | start_timestamp_ms = stage_timestamp_ms['start_timestamp_ms'] 62 | # load l515 data 63 | l515_path = os.path.join(data_path, 'l515') 64 | l515_intrinsics = np.loadtxt(os.path.join(l515_path, 'intrinsics.txt')) # (4,), float64 65 | l515_depth_scale = np.loadtxt(os.path.join(l515_path, 'depth_scale.txt')).item() 66 | l515_timestamps = np.load(os.path.join(l515_path, 'timestamps.npy')) 67 | # load t265r data 68 | t265r_path = os.path.join(data_path, 't265r') 69 | t265r_pose_path = os.path.join(t265r_path, 'pose') 70 | t265r_pose_timestamps = np.load(os.path.join(t265r_pose_path, 'timestamps.npy')) 71 | t265r_xyzs = np.load(os.path.join(t265r_pose_path, 'xyz.npy')) 72 | t265r_quats = np.load(os.path.join(t265r_pose_path, 'quat.npy')) 73 | # load t265l data 74 | t265l_path = os.path.join(data_path, 't265l') 75 | t265l_pose_path = os.path.join(t265l_path, 'pose') 76 | t265l_pose_timestamps = np.load(os.path.join(t265l_pose_path, 'timestamps.npy')) 77 | t265l_xyzs = np.load(os.path.join(t265l_pose_path, 'xyz.npy')) 78 | t265l_quats = np.load(os.path.join(t265l_pose_path, 'quat.npy')) 79 | # load pyft data 80 | pyft_path = os.path.join(data_path, 'pyft') 81 | with open(os.path.join(pyft_path, 'tare_pyft.json'), 'r') as f: 82 | pyft_tare = json.load(f) 83 | pyft_timestamps = np.load(os.path.join(pyft_path, 'timestamps.npy')) 84 | pyft_fts = np.load(os.path.join(pyft_path, 'ft.npy')) 85 | # load angler data 86 | angler_path = os.path.join(data_path, 'angler') 87 | angler_timestamps = np.load(os.path.join(angler_path, 'timestamps.npy')) 88 | angler_angles = np.load(os.path.join(angler_path, 'angle.npy')) 89 | # load annotation data 90 | annotation_path = os.path.join(data_path, 'annotation.json') 91 | with open(annotation_path, 'r') as f: 92 | stages = json.load(f) 93 | 94 | # deal with t265 special prepare 95 | t265r_initial_pose_mask = np.logical_and(t265r_pose_timestamps > t265_pose_start_timestamp_ms, t265r_pose_timestamps < t265_pose_end_timestamp_ms) 96 | t265r_initial_xyz = np.median(t265r_xyzs[t265r_initial_pose_mask, :], axis=0) 97 | t265r_initial_quat = np.median(t265r_quats[t265r_initial_pose_mask, :], axis=0) 98 | t265r_initial_pose = T265.raw2pose(t265r_initial_xyz, t265r_initial_quat) # c02w 99 | t265l_initial_pose_mask = np.logical_and(t265l_pose_timestamps > t265_pose_start_timestamp_ms, t265l_pose_timestamps < t265_pose_end_timestamp_ms) 100 | t265l_initial_xyz = np.median(t265l_xyzs[t265l_initial_pose_mask, :], axis=0) 101 | t265l_initial_quat = np.median(t265l_quats[t265l_initial_pose_mask, :], axis=0) 102 | t265l_initial_pose = T265.raw2pose(t265l_initial_xyz, t265l_initial_quat) # c02w 103 | 104 | # deal with angler special prepare 105 | angler_angles = Angler.raw2angle(angler_angles) 106 | angler_angles[angler_angles < 0] = 0.0 107 | angler_widths = angler_angles * ANGLE_2_WIDTH 108 | 109 | # process loop 110 | data_dict = {} 111 | stage_idxs = {'grasp': 0, 'shave': 0, 'turn': 0} 112 | stage_idx_map = {} 113 | for l515_current_idx in tqdm.trange(len(l515_timestamps)): 114 | # process l515 variables 115 | l515_current_timestamp = l515_timestamps[l515_current_idx] 116 | # process t265r variables 117 | t265r_pose_current_idx = np.searchsorted(t265r_pose_timestamps, l515_current_timestamp) 118 | t265r_pose_current_idx = min(t265r_pose_current_idx, len(t265r_pose_timestamps)-1) 119 | # process t265l variables 120 | t265l_pose_current_idx = np.searchsorted(t265l_pose_timestamps, l515_current_timestamp) 121 | t265l_pose_current_idx = min(t265l_pose_current_idx, len(t265l_pose_timestamps)-1) 122 | # process pyft variables 123 | pyft_current_idx = np.searchsorted(pyft_timestamps, l515_current_timestamp) 124 | pyft_current_idx = min(pyft_current_idx, len(pyft_timestamps)-1) 125 | # process angler variables 126 | angler_current_idx = np.searchsorted(angler_timestamps, l515_current_timestamp) 127 | angler_current_idx = min(angler_current_idx, len(angler_timestamps)-1) 128 | 129 | # process stage 130 | stage_idx = search_stage(l515_current_timestamp, stages) 131 | stage = stages[stage_idx] 132 | t265r_xyz_t265rw_bias = np.array(stage['t265r_xyz_t265rw_bias']) 133 | t265l_xyz_t265lw_bias = np.array(stage['t265l_xyz_t265lw_bias']) 134 | if stage['stage'] == 'unrelated': 135 | continue 136 | if stage_idx not in stage_idx_map: 137 | stage_idx_map[stage_idx] = stage['stage'] + '_' + str(stage_idxs[stage['stage']]).zfill(2) 138 | stage_idxs[stage['stage']] = stage_idxs[stage['stage']] + 1 139 | data_dict[stage_idx_map[stage_idx]] = { 140 | 'l515_pc_xyzs_l515': [], 141 | 'l515_pc_rgbs': [], 142 | 'l515_pc_xyzs_l515_mesh': [], 143 | 'l515_pc_rgbs_mesh': [], 144 | 'gripper_xyzs_l515': [], 145 | 'gripper_quats_l515': [], 146 | 'pyft_xyzs_l515': [], 147 | 'pyft_quats_l515': [], 148 | 'pyft_fs_pyft': [], 149 | 'pyft_ts_pyft': [], 150 | 'pyft_fs_l515': [], 151 | 'pyft_ts_l515': [], 152 | 'angler_widths': [], 153 | } 154 | 155 | # process t265r elements 156 | t265r_xyz_t265rw, t265r_quat_t265rw = t265r_xyzs[t265r_pose_current_idx], t265r_quats[t265r_pose_current_idx] 157 | t265r_xyz_t265rw = t265r_xyz_t265rw + t265r_xyz_t265rw_bias 158 | t265r_pose_t265rw = T265.raw2pose(t265r_xyz_t265rw, t265r_quat_t265rw) # c2w 159 | t265r_pose_t265r0 = np.linalg.inv(t265r_initial_pose) @ t265r_pose_t265rw # c2c0 = w2c0 @ c2w 160 | t265r_pose_l515 = np.linalg.inv(L515_2_T265r) @ t265r_pose_t265r0 # c2l = c02l @ c2c0 161 | # process t265l elements 162 | t265l_xyz_t265lw, t265l_quat_t265lw = t265l_xyzs[t265l_pose_current_idx], t265l_quats[t265l_pose_current_idx] 163 | t265l_xyz_t265lw = t265l_xyz_t265lw + t265l_xyz_t265lw_bias 164 | t265l_pose_t265lw = T265.raw2pose(t265l_xyz_t265lw, t265l_quat_t265lw) # c2w 165 | t265l_pose_t265l0 = np.linalg.inv(t265l_initial_pose) @ t265l_pose_t265lw # c2c0 = w2c0 @ c2w 166 | t265l_pose_l515 = np.linalg.inv(L515_2_T265l) @ t265l_pose_t265l0 # c2l = c02l @ c2c0 167 | gripper_pose_l515 = t265l_pose_l515 @ np.linalg.inv(T265l_2_GRIPPER) # g2l = c2l @ g2c 168 | gripper_xyz_l515, gripper_quat_l515 = gripper_pose_l515[:3, 3], Rot.from_matrix(gripper_pose_l515[:3, :3]).as_quat() 169 | data_dict[stage_idx_map[stage_idx]]['gripper_xyzs_l515'].append(gripper_xyz_l515) 170 | data_dict[stage_idx_map[stage_idx]]['gripper_quats_l515'].append(gripper_quat_l515) 171 | # process pyft elements 172 | pyft_pose_l515 = t265r_pose_l515 @ np.linalg.inv(T265r_2_PYFT) # f2l = c2l @ f2c 173 | pyft_xyz_l515, pyft_quat_l515 = pyft_pose_l515[:3, 3], Rot.from_matrix(pyft_pose_l515[:3, :3]).as_quat() 174 | data_dict[stage_idx_map[stage_idx]]['pyft_xyzs_l515'].append(pyft_xyz_l515) 175 | data_dict[stage_idx_map[stage_idx]]['pyft_quats_l515'].append(pyft_quat_l515) 176 | pyft_ft_pyft = pyft_fts[pyft_current_idx] 177 | pyft_pose_base = L515_2_BASE @ pyft_pose_l515 # f2b = l2b @ f2l 178 | pyft_ft_pyft = Pyft.raw2tare(pyft_ft_pyft, pyft_tare, pyft_pose_base[:3, :3]) 179 | pyft_f_pyft, pyft_t_pyft = pyft_ft_pyft[:3], pyft_ft_pyft[3:] 180 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 181 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 182 | data_dict[stage_idx_map[stage_idx]]['pyft_fs_pyft'].append(pyft_f_pyft) 183 | data_dict[stage_idx_map[stage_idx]]['pyft_ts_pyft'].append(pyft_t_pyft) 184 | data_dict[stage_idx_map[stage_idx]]['pyft_fs_l515'].append(pyft_f_l515) 185 | data_dict[stage_idx_map[stage_idx]]['pyft_ts_l515'].append(pyft_t_l515) 186 | # process angler elements 187 | angler_width = angler_widths[angler_current_idx] 188 | data_dict[stage_idx_map[stage_idx]]['angler_widths'].append(angler_width) 189 | # process l515 elements 190 | l515_color_img = cv2.imread(os.path.join(l515_path, 'color', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_COLOR) 191 | l515_color_img = cv2.cvtColor(l515_color_img, cv2.COLOR_BGR2RGB) # (H, W, 3), uint8 192 | l515_color_img = l515_color_img / 255. # (H, W, 3), float64 193 | l515_depth_img = cv2.imread(os.path.join(l515_path, 'depth', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_ANYDEPTH) # (H, W), uint16 194 | l515_depth_img = l515_depth_img * l515_depth_scale # (H, W), float64 195 | l515_pc_xyz_l515, l515_pc_rgb = L515.img2pc(l515_depth_img, l515_intrinsics, l515_color_img) 196 | if clip_object: 197 | l515_pc_xyz_base = transform_pc(l515_pc_xyz_l515, L515_2_BASE) 198 | clip_object_mask = (l515_pc_xyz_base[:, 0] > OBJECT_SPACE[0][0]) & (l515_pc_xyz_base[:, 0] < OBJECT_SPACE[0][1]) & \ 199 | (l515_pc_xyz_base[:, 1] > OBJECT_SPACE[1][0]) & (l515_pc_xyz_base[:, 1] < OBJECT_SPACE[1][1]) & \ 200 | (l515_pc_xyz_base[:, 2] > OBJECT_SPACE[2][0]) & (l515_pc_xyz_base[:, 2] < OBJECT_SPACE[2][1]) 201 | else: 202 | clip_object_mask = np.zeros((l515_pc_xyz_l515.shape[0],), dtype=bool) 203 | if clip_gripper: 204 | l515_pc_xyz_gripper = transform_pc(l515_pc_xyz_l515, np.linalg.inv(gripper_pose_l515)) 205 | clip_gripper_mask = (l515_pc_xyz_gripper[:, 0] > GRIPPER_SPACE[0][0]) & (l515_pc_xyz_gripper[:, 0] < GRIPPER_SPACE[0][1]) & \ 206 | (l515_pc_xyz_gripper[:, 1] > GRIPPER_SPACE[1][0]) & (l515_pc_xyz_gripper[:, 1] < GRIPPER_SPACE[1][1]) & \ 207 | (l515_pc_xyz_gripper[:, 2] > GRIPPER_SPACE[2][0]) & (l515_pc_xyz_gripper[:, 2] < GRIPPER_SPACE[2][1]) 208 | else: 209 | clip_gripper_mask = np.zeros((l515_pc_xyz_l515.shape[0],), dtype=bool) 210 | if clip_pyft: 211 | l515_pc_xyz_pyft = transform_pc(l515_pc_xyz_l515, np.linalg.inv(pyft_pose_l515)) 212 | clip_pyft_mask = (l515_pc_xyz_pyft[:, 0] > PYFT_SPACE[0][0]) & (l515_pc_xyz_pyft[:, 0] < PYFT_SPACE[0][1]) & \ 213 | (l515_pc_xyz_pyft[:, 1] > PYFT_SPACE[1][0]) & (l515_pc_xyz_pyft[:, 1] < PYFT_SPACE[1][1]) & \ 214 | (l515_pc_xyz_pyft[:, 2] > PYFT_SPACE[2][0]) & (l515_pc_xyz_pyft[:, 2] < PYFT_SPACE[2][1]) 215 | else: 216 | clip_pyft_mask = np.zeros((l515_pc_xyz_l515.shape[0],), dtype=bool) 217 | if clip_base: 218 | l515_pc_xyz_base = transform_pc(l515_pc_xyz_l515, L515_2_BASE) 219 | clip_base_mask = (l515_pc_xyz_base[:, 0] > BASE_SPACE[0][0]) & (l515_pc_xyz_base[:, 0] < BASE_SPACE[0][1]) & \ 220 | (l515_pc_xyz_base[:, 1] > BASE_SPACE[1][0]) & (l515_pc_xyz_base[:, 1] < BASE_SPACE[1][1]) & \ 221 | (l515_pc_xyz_base[:, 2] > BASE_SPACE[2][0]) & (l515_pc_xyz_base[:, 2] < BASE_SPACE[2][1]) 222 | else: 223 | clip_base_mask = np.ones((l515_pc_xyz_l515.shape[0],), dtype=bool) 224 | valid_mask = np.logical_and(clip_base_mask, np.logical_or(clip_object_mask, np.logical_or(clip_gripper_mask, clip_pyft_mask))) 225 | # TODO: hardcode to throw out hands 226 | l515_pc_xyz_gripper = transform_pc(l515_pc_xyz_l515, np.linalg.inv(gripper_pose_l515)) 227 | valid_mask = np.logical_and(valid_mask, l515_pc_xyz_gripper[:, 2] > 0.) 228 | l515_pc_xyz_pyft = transform_pc(l515_pc_xyz_l515, np.linalg.inv(pyft_pose_l515)) 229 | valid_mask = np.logical_and(valid_mask, l515_pc_xyz_pyft[:, 2] > 0.) 230 | valid_mask = np.where(valid_mask)[0] 231 | l515_pc_xyz_l515 = l515_pc_xyz_l515[valid_mask] 232 | l515_pc_rgb = l515_pc_rgb[valid_mask] 233 | l515_pc_xyz_l515_mesh = l515_pc_xyz_l515.copy() 234 | l515_pc_rgb_mesh = l515_pc_rgb.copy() 235 | if render_gripper_num != 0: 236 | rfinger_pc_xyz_rfinger_mesh = mesh2pc(os.path.join("objs", "right_finger.obj"), num_points=render_gripper_num//2) 237 | lfinger_pc_xyz_lfinger_mesh = mesh2pc(os.path.join("objs", "left_finger.obj"), num_points=render_gripper_num//2) 238 | angler_finger_pose_gripper = np.identity(4) 239 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 240 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 241 | rfinger_pc_xyz_l515_mesh = transform_pc(rfinger_pc_xyz_rfinger_mesh, gripper_right_finger_pose_l515) 242 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 243 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 244 | lfinger_pc_xyz_l515_mesh = transform_pc(lfinger_pc_xyz_lfinger_mesh, gripper_left_finger_pose_l515) 245 | rfinger_pc_rgb_mesh = np.ones_like(rfinger_pc_xyz_l515_mesh) 246 | lfinger_pc_rgb_mesh = np.ones_like(lfinger_pc_xyz_l515_mesh) 247 | l515_pc_xyz_l515_mesh = np.concatenate([l515_pc_xyz_l515_mesh, rfinger_pc_xyz_l515_mesh, lfinger_pc_xyz_l515_mesh], axis=0) 248 | l515_pc_rgb_mesh = np.concatenate([l515_pc_rgb_mesh, rfinger_pc_rgb_mesh, lfinger_pc_rgb_mesh], axis=0) 249 | if render_pyft_num != 0: 250 | pyft_pc_xyz_pyft_mesh = mesh2pc(os.path.join("objs", "only_peeler.obj"), num_points=render_pyft_num) 251 | pyft_pc_xyz_l515_mesh = transform_pc(pyft_pc_xyz_pyft_mesh, pyft_pose_l515) 252 | pyft_pc_rgb_mesh = np.ones_like(pyft_pc_xyz_l515_mesh) 253 | l515_pc_xyz_l515_mesh = np.concatenate([l515_pc_xyz_l515_mesh, pyft_pc_xyz_l515_mesh], axis=0) 254 | l515_pc_rgb_mesh = np.concatenate([l515_pc_rgb_mesh, pyft_pc_rgb_mesh], axis=0) 255 | if voxel_size != 0: 256 | l515_pc_xyz_l515, l515_pc_rgb = voxelize(l515_pc_xyz_l515, l515_pc_rgb, voxel_size) 257 | l515_pc_xyz_l515_mesh, l515_pc_rgb_mesh = voxelize(l515_pc_xyz_l515_mesh, l515_pc_rgb_mesh, voxel_size) 258 | if pc_num != -1: 259 | if l515_pc_xyz_l515.shape[0] > pc_num: 260 | valid_mask = np.random.choice(l515_pc_xyz_l515.shape[0], pc_num, replace=False) 261 | elif l515_pc_xyz_l515.shape[0] < pc_num: 262 | print(f"Warning: {l515_pc_xyz_l515.shape[0] = }") 263 | valid_mask = np.concatenate([np.arange(l515_pc_xyz_l515.shape[0]), np.random.choice(l515_pc_xyz_l515.shape[0], pc_num - l515_pc_xyz_l515.shape[0], replace=False)], axis=0) 264 | l515_pc_xyz_l515 = l515_pc_xyz_l515[valid_mask] 265 | l515_pc_rgb = l515_pc_rgb[valid_mask] 266 | if l515_pc_xyz_l515_mesh.shape[0] > pc_num: 267 | valid_mask = np.random.choice(l515_pc_xyz_l515_mesh.shape[0], pc_num, replace=False) 268 | elif l515_pc_xyz_l515_mesh.shape[0] < pc_num: 269 | print(f"Warning: {l515_pc_xyz_l515_mesh.shape[0] = }") 270 | valid_mask = np.concatenate([np.arange(l515_pc_xyz_l515_mesh.shape[0]), np.random.choice(l515_pc_xyz_l515_mesh.shape[0], pc_num - l515_pc_xyz_l515_mesh.shape[0], replace=False)], axis=0) 271 | l515_pc_xyz_l515_mesh = l515_pc_xyz_l515_mesh[valid_mask] 272 | l515_pc_rgb_mesh = l515_pc_rgb_mesh[valid_mask] 273 | data_dict[stage_idx_map[stage_idx]]['l515_pc_xyzs_l515'].append(l515_pc_xyz_l515) 274 | data_dict[stage_idx_map[stage_idx]]['l515_pc_rgbs'].append(l515_pc_rgb) 275 | data_dict[stage_idx_map[stage_idx]]['l515_pc_xyzs_l515_mesh'].append(l515_pc_xyz_l515_mesh) 276 | data_dict[stage_idx_map[stage_idx]]['l515_pc_rgbs_mesh'].append(l515_pc_rgb_mesh) 277 | 278 | # hdf5 loop 279 | os.makedirs(save_path, exist_ok=True) 280 | for stage_name in tqdm.tqdm(data_dict.keys()): 281 | with h5py.File(os.path.join(save_path, stage_name + '.hdf5'), 'w') as stage_hdf5: 282 | stage_hdf5_data_group = stage_hdf5.create_group('data') 283 | 284 | # save observation 285 | stage_hdf5_o_group = stage_hdf5_data_group.create_group('o') 286 | stage_hdf5_o_group.create_dataset('l515_pc_xyzs_l515', data=np.array(data_dict[stage_name]['l515_pc_xyzs_l515'])) 287 | stage_hdf5_o_group.create_dataset('l515_pc_rgbs', data=np.array(data_dict[stage_name]['l515_pc_rgbs'])) 288 | stage_hdf5_o_group.create_dataset('l515_pc_xyzs_l515_mesh', data=np.array(data_dict[stage_name]['l515_pc_xyzs_l515_mesh'])) 289 | stage_hdf5_o_group.create_dataset('l515_pc_rgbs_mesh', data=np.array(data_dict[stage_name]['l515_pc_rgbs_mesh'])) 290 | stage_hdf5_o_group.create_dataset('gripper_xyzs_l515', data=np.array(data_dict[stage_name]['gripper_xyzs_l515'])) 291 | stage_hdf5_o_group.create_dataset('gripper_quats_l515', data=np.array(data_dict[stage_name]['gripper_quats_l515'])) 292 | stage_hdf5_o_group.create_dataset('pyft_xyzs_l515', data=np.array(data_dict[stage_name]['pyft_xyzs_l515'])) 293 | stage_hdf5_o_group.create_dataset('pyft_quats_l515', data=np.array(data_dict[stage_name]['pyft_quats_l515'])) 294 | stage_hdf5_o_group.create_dataset('pyft_fs_pyft', data=np.array(data_dict[stage_name]['pyft_fs_pyft'])) 295 | stage_hdf5_o_group.create_dataset('pyft_ts_pyft', data=np.array(data_dict[stage_name]['pyft_ts_pyft'])) 296 | stage_hdf5_o_group.create_dataset('pyft_fs_l515', data=np.array(data_dict[stage_name]['pyft_fs_l515'])) 297 | stage_hdf5_o_group.create_dataset('pyft_ts_l515', data=np.array(data_dict[stage_name]['pyft_ts_l515'])) 298 | stage_hdf5_o_group.create_dataset('angler_widths', data=np.array(data_dict[stage_name]['angler_widths'])) 299 | # save attributes 300 | stage_hdf5_data_group.attrs['num_samples'] = len(data_dict[stage_name]['l515_pc_xyzs_l515']) 301 | stage_hdf5_o_group.attrs['clip_object'] = clip_object 302 | stage_hdf5_o_group.attrs['clip_gripper'] = clip_gripper 303 | stage_hdf5_o_group.attrs['clip_pyft'] = clip_pyft 304 | stage_hdf5_o_group.attrs['clip_base'] = clip_base 305 | stage_hdf5_o_group.attrs['render_gripper_num'] = render_gripper_num 306 | stage_hdf5_o_group.attrs['render_pyft_num'] = render_pyft_num 307 | stage_hdf5_o_group.attrs['voxel_size'] = voxel_size 308 | stage_hdf5_o_group.attrs['pc_num'] = pc_num 309 | 310 | 311 | if __name__ == '__main__': 312 | args = config_parse() 313 | main(args) 314 | -------------------------------------------------------------------------------- /visualize_data.py: -------------------------------------------------------------------------------- 1 | import os 2 | import configargparse 3 | import json 4 | import numpy as np 5 | import cv2 6 | import open3d as o3d 7 | from pynput import keyboard 8 | import tqdm 9 | 10 | from r3kit.devices.camera.realsense.t265 import T265 11 | from r3kit.devices.camera.realsense.l515 import L515 12 | from r3kit.devices.ftsensor.ati.pyati import PyATI as Pyft 13 | from r3kit.devices.encoder.pdcd.angler import Angler 14 | from r3kit.utils.vis import rotation_vec2mat 15 | from configs.pose import * 16 | from utils.annotation import search_stage 17 | 18 | ''' 19 | Synchronize with `create_hdf5.py` some part 20 | ''' 21 | 22 | 23 | def config_parse() -> configargparse.Namespace: 24 | parser = configargparse.ArgumentParser() 25 | 26 | parser.add_argument('--config', is_config_file=True, help='config file path') 27 | parser.add_argument('--data_path', type=str) 28 | parser.add_argument('--fps', type=int, default=10) 29 | 30 | args = parser.parse_args() 31 | return args 32 | 33 | 34 | def main(args): 35 | # general config 36 | data_path = args.data_path 37 | fps = args.fps 38 | frame_interval_ms = 1000. / fps 39 | 40 | # load stage data 41 | with open(os.path.join(data_path, 'stage_timestamp_ms.json'), 'r') as f: 42 | stage_timestamp_ms = json.load(f) 43 | t265_pose_start_timestamp_ms = stage_timestamp_ms['t265_pose_start_timestamp_ms'] 44 | t265_pose_end_timestamp_ms = stage_timestamp_ms['t265_pose_end_timestamp_ms'] 45 | start_timestamp_ms = stage_timestamp_ms['start_timestamp_ms'] 46 | # load l515 data 47 | l515_path = os.path.join(data_path, 'l515') 48 | l515_intrinsics = np.loadtxt(os.path.join(l515_path, 'intrinsics.txt')) # (4,), float64 49 | l515_depth_scale = np.loadtxt(os.path.join(l515_path, 'depth_scale.txt')).item() 50 | l515_timestamps = np.load(os.path.join(l515_path, 'timestamps.npy')) 51 | ### l515_depth_img, l515_color_img loaded during iteration 52 | # load t265r data 53 | t265r_path = os.path.join(data_path, 't265r') 54 | ### t265r_image_path = os.path.join(t265r_path, 'image') 55 | ### t265r_image_timestamps = np.load(os.path.join(t265r_image_path, 'timestamps.npy')) 56 | ### t265r_left_img, t265r_right_img loaded during iteration 57 | t265r_pose_path = os.path.join(t265r_path, 'pose') 58 | t265r_pose_timestamps = np.load(os.path.join(t265r_pose_path, 'timestamps.npy')) 59 | t265r_xyzs = np.load(os.path.join(t265r_pose_path, 'xyz.npy')) 60 | t265r_quats = np.load(os.path.join(t265r_pose_path, 'quat.npy')) 61 | # load t265l data 62 | t265l_path = os.path.join(data_path, 't265l') 63 | ### t265l_image_path = os.path.join(t265l_path, 'image') 64 | ### t265l_image_timestamps = np.load(os.path.join(t265l_image_path, 'timestamps.npy')) 65 | ### t265l_left_img, t265l_right_img loaded during iteration 66 | t265l_pose_path = os.path.join(t265l_path, 'pose') 67 | t265l_pose_timestamps = np.load(os.path.join(t265l_pose_path, 'timestamps.npy')) 68 | t265l_xyzs = np.load(os.path.join(t265l_pose_path, 'xyz.npy')) 69 | t265l_quats = np.load(os.path.join(t265l_pose_path, 'quat.npy')) 70 | # load pyft data 71 | pyft_path = os.path.join(data_path, 'pyft') 72 | with open(os.path.join(pyft_path, 'tare_pyft.json'), 'r') as f: 73 | pyft_tare = json.load(f) 74 | pyft_timestamps = np.load(os.path.join(pyft_path, 'timestamps.npy')) 75 | pyft_fts = np.load(os.path.join(pyft_path, 'ft.npy')) 76 | # load angler data 77 | angler_path = os.path.join(data_path, 'angler') 78 | angler_timestamps = np.load(os.path.join(angler_path, 'timestamps.npy')) 79 | angler_angles = np.load(os.path.join(angler_path, 'angle.npy')) 80 | # load annotation data 81 | annotation_path = os.path.join(data_path, 'annotation.json') 82 | has_annotation = os.path.exists(annotation_path) 83 | if has_annotation: 84 | with open(annotation_path, 'r') as f: 85 | annotation = json.load(f) 86 | 87 | # deal with t265 special prepare 88 | t265r_initial_pose_mask = np.logical_and(t265r_pose_timestamps > t265_pose_start_timestamp_ms, t265r_pose_timestamps < t265_pose_end_timestamp_ms) 89 | t265r_initial_xyz = np.median(t265r_xyzs[t265r_initial_pose_mask, :], axis=0) 90 | t265r_initial_quat = np.median(t265r_quats[t265r_initial_pose_mask, :], axis=0) 91 | t265r_initial_pose = T265.raw2pose(t265r_initial_xyz, t265r_initial_quat) # c02w 92 | t265l_initial_pose_mask = np.logical_and(t265l_pose_timestamps > t265_pose_start_timestamp_ms, t265l_pose_timestamps < t265_pose_end_timestamp_ms) 93 | t265l_initial_xyz = np.median(t265l_xyzs[t265l_initial_pose_mask, :], axis=0) 94 | t265l_initial_quat = np.median(t265l_quats[t265l_initial_pose_mask, :], axis=0) 95 | t265l_initial_pose = T265.raw2pose(t265l_initial_xyz, t265l_initial_quat) # c02w 96 | 97 | # deal with angler special prepare 98 | angler_angles = Angler.raw2angle(angler_angles) 99 | angler_angles[angler_angles < 0] = 0.0 100 | angler_widths = angler_angles * ANGLE_2_WIDTH 101 | 102 | # process l515 variables 103 | l515_current_idx = 0 104 | l515_current_timestamp = l515_timestamps[l515_current_idx] 105 | l515_start_timestamp = l515_timestamps[0] 106 | l515_end_timestamp = l515_timestamps[-1] 107 | # process t265r variables 108 | ### t265r_image_current_idx = np.searchsorted(t265r_image_timestamps, l515_current_timestamp) 109 | t265r_pose_current_idx = np.searchsorted(t265r_pose_timestamps, l515_current_timestamp) 110 | # process t265l variables 111 | ### t265l_image_current_idx = np.searchsorted(t265l_image_timestamps, l515_current_timestamp) 112 | t265l_pose_current_idx = np.searchsorted(t265l_pose_timestamps, l515_current_timestamp) 113 | # process pyft variables 114 | pyft_current_idx = np.searchsorted(pyft_timestamps, l515_current_timestamp) 115 | # process angler variables 116 | angler_current_idx = np.searchsorted(angler_timestamps, l515_current_timestamp) 117 | 118 | # create keyboard listener 119 | quit = False 120 | reset = False 121 | pause = False 122 | zero = False 123 | forward = False 124 | backward = False 125 | speed = 1 126 | if not has_annotation: 127 | minus = False 128 | t265r_xyz_t265rw_bias = np.array([0., 0., 0.]) 129 | t265l_xyz_t265lw_bias = np.array([0., 0., 0.]) 130 | stages = [{'timestamp_ms': l515_current_timestamp, 131 | 't265r_xyz_t265rw_bias': t265r_xyz_t265rw_bias.tolist(), 132 | 't265l_xyz_t265lw_bias': t265l_xyz_t265lw_bias.tolist(), 133 | 'stage': 'unrelated'}] 134 | else: 135 | stages = annotation 136 | stage_idx = search_stage(l515_current_timestamp, stages) 137 | stage = stages[stage_idx] 138 | t265r_xyz_t265rw_bias = np.array(stage['t265r_xyz_t265rw_bias']) 139 | t265l_xyz_t265lw_bias = np.array(stage['t265l_xyz_t265lw_bias']) 140 | def _on_press(key): 141 | nonlocal quit, reset, pause, zero, forward, backward, speed 142 | nonlocal current_timestamp, stages, minus, t265r_xyz_t265rw_bias, t265l_xyz_t265lw_bias 143 | if hasattr(key, 'char') and key.char == 'q': 144 | quit = True 145 | print("quit") 146 | if hasattr(key, 'char') and key.char == 'r': 147 | reset = True 148 | print("reset") 149 | if hasattr(key, 'char') and key.char == 'p': 150 | pause = not pause 151 | forward = False 152 | backward = False 153 | print("pause" if pause else "continue") 154 | if key == keyboard.Key.backspace: 155 | zero = True 156 | print("zero") 157 | if pause and key == keyboard.Key.right: 158 | forward = True 159 | print("forward") 160 | if pause and key == keyboard.Key.left: 161 | backward = True 162 | print("backward") 163 | if pause and key == keyboard.Key.up: 164 | speed *= 2 165 | print(f"speed {speed}") 166 | if pause and key == keyboard.Key.down: 167 | speed //= 2 168 | speed = max(speed, 1) 169 | print(f"speed {speed}") 170 | if not has_annotation: 171 | if hasattr(key, 'char') and key.char == 'u': 172 | stages.append({'timestamp_ms': current_timestamp, 173 | 't265r_xyz_t265rw_bias': t265r_xyz_t265rw_bias.tolist(), 174 | 't265l_xyz_t265lw_bias': t265l_xyz_t265lw_bias.tolist(), 175 | 'stage': 'unrelated'}) 176 | print(f"unrelated from {current_timestamp}") 177 | if hasattr(key, 'char') and key.char == 'g': 178 | stages.append({'timestamp_ms': current_timestamp, 179 | 't265r_xyz_t265rw_bias': t265r_xyz_t265rw_bias.tolist(), 180 | 't265l_xyz_t265lw_bias': t265l_xyz_t265lw_bias.tolist(), 181 | 'stage': 'grasp'}) 182 | print(f"grasp from {current_timestamp}") 183 | if hasattr(key, 'char') and key.char == 's': 184 | stages.append({'timestamp_ms': current_timestamp, 185 | 't265r_xyz_t265rw_bias': t265r_xyz_t265rw_bias.tolist(), 186 | 't265l_xyz_t265lw_bias': t265l_xyz_t265lw_bias.tolist(), 187 | 'stage': 'shave'}) 188 | print(f"shave from {current_timestamp}") 189 | if hasattr(key, 'char') and key.char == 't': 190 | stages.append({'timestamp_ms': current_timestamp, 191 | 't265r_xyz_t265rw_bias': t265r_xyz_t265rw_bias.tolist(), 192 | 't265l_xyz_t265lw_bias': t265l_xyz_t265lw_bias.tolist(), 193 | 'stage': 'turn'}) 194 | print(f"turn from {current_timestamp}") 195 | if hasattr(key, 'char') and key.char == 'm': 196 | minus = not minus 197 | print("bias minus" if minus else "bias plus") 198 | if hasattr(key, 'char') and key.char == 'x': 199 | t265r_xyz_t265rw_bias = t265r_xyz_t265rw_bias + np.array([0.005 if not minus else -0.005, 0., 0.]) 200 | stage_idx = search_stage(current_timestamp, stages) 201 | stages[stage_idx]['t265r_xyz_t265rw_bias'] = t265r_xyz_t265rw_bias.tolist() 202 | if hasattr(key, 'char') and key.char == 'y': 203 | t265r_xyz_t265rw_bias = t265r_xyz_t265rw_bias + np.array([0., 0.005 if not minus else -0.005, 0.]) 204 | stage_idx = search_stage(current_timestamp, stages) 205 | stages[stage_idx]['t265r_xyz_t265rw_bias'] = t265r_xyz_t265rw_bias.tolist() 206 | if hasattr(key, 'char') and key.char == 'z': 207 | t265r_xyz_t265rw_bias = t265r_xyz_t265rw_bias + np.array([0., 0., 0.005 if not minus else -0.005]) 208 | stage_idx = search_stage(current_timestamp, stages) 209 | stages[stage_idx]['t265r_xyz_t265rw_bias'] = t265r_xyz_t265rw_bias.tolist() 210 | if hasattr(key, 'char') and key.char == 'a': 211 | t265l_xyz_t265lw_bias = t265l_xyz_t265lw_bias + np.array([0.005 if not minus else -0.005, 0., 0.]) 212 | stage_idx = search_stage(current_timestamp, stages) 213 | stages[stage_idx]['t265l_xyz_t265lw_bias'] = t265l_xyz_t265lw_bias.tolist() 214 | if hasattr(key, 'char') and key.char == 'b': 215 | t265l_xyz_t265lw_bias = t265l_xyz_t265lw_bias + np.array([0., 0.005 if not minus else -0.005, 0.]) 216 | stage_idx = search_stage(current_timestamp, stages) 217 | stages[stage_idx]['t265l_xyz_t265lw_bias'] = t265l_xyz_t265lw_bias.tolist() 218 | if hasattr(key, 'char') and key.char == 'c': 219 | t265l_xyz_t265lw_bias = t265l_xyz_t265lw_bias + np.array([0., 0., 0.005 if not minus else -0.005]) 220 | stage_idx = search_stage(current_timestamp, stages) 221 | stages[stage_idx]['t265l_xyz_t265lw_bias'] = t265l_xyz_t265lw_bias.tolist() 222 | if key == keyboard.Key.delete: 223 | stages.pop() 224 | print("delete") 225 | def _on_release(key): 226 | pass 227 | listener = keyboard.Listener(on_press=_on_press, on_release=_on_release) 228 | listener.start() 229 | 230 | # create visualizer 231 | visualizer = o3d.visualization.Visualizer() 232 | visualizer.create_window(width=1280, height=720, left=200, top=200, visible=True, window_name='data') 233 | 234 | # add l515 elements 235 | l515_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 236 | visualizer.add_geometry(l515_frame) 237 | l515_color_img = cv2.imread(os.path.join(l515_path, 'color', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_COLOR) 238 | l515_color_img = cv2.cvtColor(l515_color_img, cv2.COLOR_BGR2RGB) # (H, W, 3), uint8 239 | l515_color_img = l515_color_img / 255. # (H, W, 3), float64 240 | l515_depth_img = cv2.imread(os.path.join(l515_path, 'depth', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_ANYDEPTH) # (H, W), uint16 241 | l515_depth_img = l515_depth_img * l515_depth_scale # (H, W), float64 242 | l515_pc_xyz_l515, l515_pc_rgb = L515.img2pc(l515_depth_img, l515_intrinsics, l515_color_img) 243 | l515_pcd = o3d.geometry.PointCloud() 244 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 245 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 246 | visualizer.add_geometry(l515_pcd) 247 | # add t265r elements 248 | t265r_xyz_t265rw, t265r_quat_t265rw = t265r_xyzs[t265r_pose_current_idx], t265r_quats[t265r_pose_current_idx] 249 | t265r_xyz_t265rw = t265r_xyz_t265rw + t265r_xyz_t265rw_bias 250 | t265r_pose_t265rw = T265.raw2pose(t265r_xyz_t265rw, t265r_quat_t265rw) # c2w 251 | t265r_pose_t265r0 = np.linalg.inv(t265r_initial_pose) @ t265r_pose_t265rw # c2c0 = w2c0 @ c2w 252 | t265r_pose_l515 = np.linalg.inv(L515_2_T265r) @ t265r_pose_t265r0 # c2l = c02l @ c2c0 253 | ### t265r_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 254 | ### t265r_frame.transform(t265r_pose_l515) 255 | ### visualizer.add_geometry(t265r_frame) 256 | ### t265r_left_img = cv2.imread(os.path.join(t265r_image_path, 'left', f'{str(t265r_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 257 | ### cv2.namedWindow('t265r_left', cv2.WINDOW_NORMAL) 258 | ### cv2.imshow('t265r_left', t265r_left_img) 259 | ### cv2.waitKey(1) 260 | ### t265r_right_img = cv2.imread(os.path.join(t265r_image_path, 'right', f'{str(t265r_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 261 | ### cv2.namedWindow('t265r_right', cv2.WINDOW_NORMAL) 262 | ### cv2.imshow('t265r_right', t265r_right_img) 263 | ### cv2.waitKey(1) 264 | # add t265l elements 265 | t265l_xyz_t265lw, t265l_quat_t265lw = t265l_xyzs[t265l_pose_current_idx], t265l_quats[t265l_pose_current_idx] 266 | t265l_xyz_t265lw = t265l_xyz_t265lw + t265l_xyz_t265lw_bias 267 | t265l_pose_t265lw = T265.raw2pose(t265l_xyz_t265lw, t265l_quat_t265lw) # c2w 268 | t265l_pose_t265l0 = np.linalg.inv(t265l_initial_pose) @ t265l_pose_t265lw # c2c0 = w2c0 @ c2w 269 | t265l_pose_l515 = np.linalg.inv(L515_2_T265l) @ t265l_pose_t265l0 # c2l = c02l @ c2c0 270 | ### t265l_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 271 | ### t265l_frame.transform(t265l_pose_l515) 272 | ### visualizer.add_geometry(t265l_frame) 273 | ### t265l_left_img = cv2.imread(os.path.join(t265l_image_path, 'left', f'{str(t265l_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 274 | ### cv2.namedWindow('t265l_left', cv2.WINDOW_NORMAL) 275 | ### cv2.imshow('t265l_left', t265l_left_img) 276 | ### cv2.waitKey(1) 277 | ### t265l_right_img = cv2.imread(os.path.join(t265l_image_path, 'right', f'{str(t265l_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 278 | ### cv2.namedWindow('t265l_right', cv2.WINDOW_NORMAL) 279 | ### cv2.imshow('t265l_right', t265l_right_img) 280 | ### cv2.waitKey(1) 281 | gripper_pose_l515 = t265l_pose_l515 @ np.linalg.inv(T265l_2_GRIPPER) # g2l = c2l @ g2c 282 | gripper_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 283 | gripper_frame.transform(gripper_pose_l515) 284 | visualizer.add_geometry(gripper_frame) 285 | gripper = o3d.io.read_triangle_mesh(os.path.join("objs", "gripper.obj")) 286 | gripper.transform(gripper_pose_l515) 287 | visualizer.add_geometry(gripper) 288 | # add pyft elements 289 | pyft_pose_l515 = t265r_pose_l515 @ np.linalg.inv(T265r_2_PYFT) # f2l = c2l @ f2c 290 | pyft_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 291 | pyft_frame.transform(pyft_pose_l515) 292 | visualizer.add_geometry(pyft_frame) 293 | pyft_ft_pyft = pyft_fts[pyft_current_idx] 294 | pyft_pose_base = L515_2_BASE @ pyft_pose_l515 # f2b = l2b @ f2l 295 | pyft_ft_pyft = Pyft.raw2tare(pyft_ft_pyft, pyft_tare, pyft_pose_base[:3, :3]) 296 | pyft_f_pyft, pyft_t_pyft = pyft_ft_pyft[:3], pyft_ft_pyft[3:] 297 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 298 | pyft_f_value = np.linalg.norm(pyft_f_l515) 299 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 300 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 301 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 302 | pyft_t_value = np.linalg.norm(pyft_t_l515) 303 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 304 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 305 | pyft_f_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.04 * 0.025, cone_radius=0.04 * 0.05, cylinder_height=0.04 * 0.875, cone_height=0.04 * 0.125, 306 | resolution=20, cylinder_split=4, cone_split=1) 307 | pyft_f_arrow.paint_uniform_color([1., 1., 0.]) 308 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 309 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 310 | pyft_f_arrow.translate(pyft_f_translation_l515) 311 | visualizer.add_geometry(pyft_f_arrow) 312 | pyft_t_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.4 * 0.025, cone_radius=0.4 * 0.05, cylinder_height=0.4 * 0.875, cone_height=0.4 * 0.125, 313 | resolution=20, cylinder_split=4, cone_split=1) 314 | pyft_t_arrow.paint_uniform_color([0., 1., 1.]) 315 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 316 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 317 | pyft_t_arrow.translate(pyft_t_translation_l515) 318 | visualizer.add_geometry(pyft_t_arrow) 319 | pyft_peeler = o3d.io.read_triangle_mesh(os.path.join("objs", "peeler.obj")) 320 | pyft_peeler.transform(pyft_pose_l515) 321 | visualizer.add_geometry(pyft_peeler) 322 | # add angler elements 323 | angler_width = angler_widths[angler_current_idx] 324 | angler_right_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "right_finger.obj")) 325 | angler_left_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "left_finger.obj")) 326 | angler_finger_pose_gripper = np.identity(4) 327 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 328 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 329 | angler_right_finger.transform(gripper_right_finger_pose_l515) 330 | visualizer.add_geometry(angler_right_finger) 331 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 332 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 333 | angler_left_finger.transform(gripper_left_finger_pose_l515) 334 | visualizer.add_geometry(angler_left_finger) 335 | 336 | # visualizer setup 337 | view_control = visualizer.get_view_control() 338 | params = view_control.convert_to_pinhole_camera_parameters() 339 | params.extrinsic = np.array([[1, 0, 0, 0], 340 | [0, 1, 0, 0], 341 | [0, 0, 1, 0], 342 | [0, 0, 0, 1]]) 343 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 344 | 345 | # visualize loop 346 | show_timestamps = np.arange(l515_start_timestamp+frame_interval_ms, l515_end_timestamp+1e-3, frame_interval_ms) 347 | with tqdm.tqdm(total=len(show_timestamps)) as pbar: 348 | show_idx = 0 349 | while show_idx < len(show_timestamps): 350 | current_timestamp = show_timestamps[show_idx] 351 | print(current_timestamp) 352 | stage_idx = search_stage(current_timestamp, stages) 353 | stage = stages[stage_idx] 354 | t265r_xyz_t265rw_bias = np.array(stage['t265r_xyz_t265rw_bias']) 355 | t265l_xyz_t265lw_bias = np.array(stage['t265l_xyz_t265lw_bias']) 356 | 357 | # update l515 variables 358 | l515_current_idx = np.searchsorted(l515_timestamps, current_timestamp) 359 | l515_current_idx = min(l515_current_idx, len(l515_timestamps)-1) 360 | l515_current_time = (l515_timestamps[l515_current_idx] - l515_start_timestamp) / 1000. 361 | # update t265r variables 362 | ### t265r_image_current_idx = np.searchsorted(t265r_image_timestamps, current_timestamp) 363 | ### t265r_image_current_idx = min(t265r_image_current_idx, len(t265r_image_timestamps)-1) 364 | ### t265r_image_current_time = (t265r_image_timestamps[t265r_image_current_idx] - l515_start_timestamp) / 1000. 365 | t265r_pose_current_idx = np.searchsorted(t265r_pose_timestamps, current_timestamp) 366 | t265r_pose_current_idx = min(t265r_pose_current_idx, len(t265r_pose_timestamps)-1) 367 | t265r_pose_current_time = (t265r_pose_timestamps[t265r_pose_current_idx] - l515_start_timestamp) / 1000. 368 | # update t265l variables 369 | ### t265l_image_current_idx = np.searchsorted(t265l_image_timestamps, current_timestamp) 370 | ### t265l_image_current_idx = min(t265l_image_current_idx, len(t265l_image_timestamps)-1) 371 | ### t265l_image_current_time = (t265l_image_timestamps[t265l_image_current_idx] - l515_start_timestamp) / 1000. 372 | t265l_pose_current_idx = np.searchsorted(t265l_pose_timestamps, current_timestamp) 373 | t265l_pose_current_idx = min(t265l_pose_current_idx, len(t265l_pose_timestamps)-1) 374 | t265l_pose_current_time = (t265l_pose_timestamps[t265l_pose_current_idx] - l515_start_timestamp) / 1000. 375 | # update pyft variables 376 | pyft_current_idx = np.searchsorted(pyft_timestamps, current_timestamp) 377 | pyft_current_idx = min(pyft_current_idx, len(pyft_timestamps)-1) 378 | pyft_current_time = (pyft_timestamps[pyft_current_idx] - l515_start_timestamp) / 1000. 379 | # update angler variables 380 | angler_current_idx = np.searchsorted(angler_timestamps, current_timestamp) 381 | angler_current_idx = min(angler_current_idx, len(angler_timestamps)-1) 382 | angler_current_time = (angler_timestamps[angler_current_idx] - l515_start_timestamp) / 1000. 383 | 384 | # update l515 elements 385 | l515_color_img = cv2.imread(os.path.join(l515_path, 'color', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_COLOR) 386 | l515_color_img = cv2.cvtColor(l515_color_img, cv2.COLOR_BGR2RGB) # (H, W, 3), uint8 387 | l515_color_img = l515_color_img / 255. # (H, W, 3), float64 388 | l515_depth_img = cv2.imread(os.path.join(l515_path, 'depth', f'{str(l515_current_idx).zfill(16)}.png'), cv2.IMREAD_ANYDEPTH) # (H, W), uint16 389 | l515_depth_img = l515_depth_img * l515_depth_scale # (H, W), float64 390 | l515_pc_xyz_l515, l515_pc_rgb = L515.img2pc(l515_depth_img, l515_intrinsics, l515_color_img) 391 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 392 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 393 | visualizer.update_geometry(l515_pcd) 394 | # update t265r elements 395 | t265r_xyz_t265rw, t265r_quat_t265rw = t265r_xyzs[t265r_pose_current_idx], t265r_quats[t265r_pose_current_idx] 396 | t265r_xyz_t265rw = t265r_xyz_t265rw + t265r_xyz_t265rw_bias 397 | t265r_pose_t265rw = T265.raw2pose(t265r_xyz_t265rw, t265r_quat_t265rw) # c2w 398 | t265r_pose_t265r0 = np.linalg.inv(t265r_initial_pose) @ t265r_pose_t265rw # c2c0 = w2c0 @ c2w 399 | t265r_pose_l515_last = t265r_pose_l515.copy() 400 | t265r_pose_l515 = np.linalg.inv(L515_2_T265r) @ t265r_pose_t265r0 # c2l = c02l @ c2c0 401 | ### t265r_frame.transform(np.linalg.inv(t265r_pose_l515_last)) 402 | ### t265r_frame.transform(t265r_pose_l515) 403 | ### visualizer.update_geometry(t265r_frame) 404 | ### t265r_left_img = cv2.imread(os.path.join(t265r_image_path, 'left', f'{str(t265r_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 405 | ### cv2.imshow('t265r_left', t265r_left_img) 406 | ### cv2.waitKey(1) 407 | ### t265r_right_img = cv2.imread(os.path.join(t265r_image_path, 'right', f'{str(t265r_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 408 | ### cv2.imshow('t265r_right', t265r_right_img) 409 | ### cv2.waitKey(1) 410 | # update t265l elements 411 | t265l_xyz_t265lw, t265l_quat_t265lw = t265l_xyzs[t265l_pose_current_idx], t265l_quats[t265l_pose_current_idx] 412 | t265l_xyz_t265lw = t265l_xyz_t265lw + t265l_xyz_t265lw_bias 413 | t265l_pose_t265lw = T265.raw2pose(t265l_xyz_t265lw, t265l_quat_t265lw) # c2w 414 | t265l_pose_t265l0 = np.linalg.inv(t265l_initial_pose) @ t265l_pose_t265lw # c2c0 = w2c0 @ c2w 415 | t265l_pose_l515_last = t265l_pose_l515.copy() 416 | t265l_pose_l515 = np.linalg.inv(L515_2_T265l) @ t265l_pose_t265l0 # c2l = c02l @ c2c0 417 | ### t265l_frame.transform(np.linalg.inv(t265l_pose_l515_last)) 418 | ### t265l_frame.transform(t265l_pose_l515) 419 | ### visualizer.add_geometry(t265l_frame) 420 | ### t265l_left_img = cv2.imread(os.path.join(t265l_image_path, 'left', f'{str(t265l_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 421 | ### cv2.imshow('t265l_left', t265l_left_img) 422 | ### cv2.waitKey(1) 423 | ### t265l_right_img = cv2.imread(os.path.join(t265l_image_path, 'right', f'{str(t265l_image_current_idx).zfill(16)}.png'), cv2.IMREAD_GRAYSCALE) # (H, W), uint8 424 | ### cv2.imshow('t265l_right', t265l_right_img) 425 | ### cv2.waitKey(1) 426 | gripper_pose_l515_last = gripper_pose_l515.copy() 427 | gripper_pose_l515 = t265l_pose_l515 @ np.linalg.inv(T265l_2_GRIPPER) # g2l = c2l @ g2c 428 | gripper_frame.transform(np.linalg.inv(gripper_pose_l515_last)) 429 | gripper_frame.transform(gripper_pose_l515) 430 | visualizer.update_geometry(gripper_frame) 431 | gripper.transform(np.linalg.inv(gripper_pose_l515_last)) 432 | gripper.transform(gripper_pose_l515) 433 | visualizer.update_geometry(gripper) 434 | # update pyft elements 435 | pyft_pose_l515_last = pyft_pose_l515.copy() 436 | pyft_pose_l515 = t265r_pose_l515 @ np.linalg.inv(T265r_2_PYFT) 437 | pyft_frame.transform(np.linalg.inv(pyft_pose_l515_last)) 438 | pyft_frame.transform(pyft_pose_l515) 439 | visualizer.update_geometry(pyft_frame) 440 | pyft_ft_pyft = pyft_fts[pyft_current_idx] 441 | ### print(pyft_ft_pyft) 442 | pyft_pose_base = L515_2_BASE @ pyft_pose_l515 # f2b = l2b @ f2l 443 | pyft_ft_pyft = Pyft.raw2tare(pyft_ft_pyft, pyft_tare, pyft_pose_base[:3, :3]) 444 | pyft_f_pyft, pyft_t_pyft = pyft_ft_pyft[:3], pyft_ft_pyft[3:] 445 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 446 | pyft_f_value_last = pyft_f_value.copy() 447 | pyft_f_value = np.linalg.norm(pyft_f_l515) 448 | pyft_f_rotation_l515_last = pyft_f_rotation_l515.copy() 449 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 450 | pyft_f_translation_l515_last = pyft_f_translation_l515.copy() 451 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 452 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 453 | pyft_t_value_last = pyft_t_value.copy() 454 | pyft_t_value = np.linalg.norm(pyft_t_l515) 455 | pyft_t_rotation_l515_last = pyft_t_rotation_l515.copy() 456 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 457 | pyft_t_translation_l515_last = pyft_t_translation_l515.copy() 458 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 459 | pyft_f_arrow.translate(-pyft_f_translation_l515_last) 460 | pyft_f_arrow.rotate(np.linalg.inv(pyft_f_rotation_l515_last), np.array([[0], [0], [0]])) 461 | pyft_f_arrow.scale(1/pyft_f_value_last, np.array([[0], [0], [0]])) 462 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 463 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 464 | pyft_f_arrow.translate(pyft_f_translation_l515) 465 | visualizer.update_geometry(pyft_f_arrow) 466 | pyft_t_arrow.translate(-pyft_t_translation_l515_last) 467 | pyft_t_arrow.rotate(np.linalg.inv(pyft_t_rotation_l515_last), np.array([[0], [0], [0]])) 468 | pyft_t_arrow.scale(1/pyft_t_value_last, np.array([[0], [0], [0]])) 469 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 470 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 471 | pyft_t_arrow.translate(pyft_t_translation_l515) 472 | visualizer.update_geometry(pyft_t_arrow) 473 | pyft_peeler.transform(np.linalg.inv(pyft_pose_l515_last)) 474 | pyft_peeler.transform(pyft_pose_l515) 475 | visualizer.update_geometry(pyft_peeler) 476 | # update angler elements 477 | angler_width_last = angler_width.copy() 478 | angler_width = angler_widths[angler_current_idx] 479 | angler_finger_pose_gripper = np.identity(4) 480 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 481 | gripper_right_finger_pose_l515_last = gripper_right_finger_pose_l515.copy() 482 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 483 | angler_right_finger.transform(np.linalg.inv(gripper_right_finger_pose_l515_last)) 484 | angler_right_finger.transform(gripper_right_finger_pose_l515) 485 | visualizer.update_geometry(angler_right_finger) 486 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 487 | gripper_left_finger_pose_l515_last = gripper_left_finger_pose_l515.copy() 488 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 489 | angler_left_finger.transform(np.linalg.inv(gripper_left_finger_pose_l515_last)) 490 | angler_left_finger.transform(gripper_left_finger_pose_l515) 491 | visualizer.update_geometry(angler_left_finger) 492 | 493 | # visualizer update 494 | visualizer.poll_events() 495 | visualizer.update_renderer() 496 | 497 | # pbar update 498 | if not pause: 499 | show_idx += 1 500 | pbar.update(1) 501 | else: 502 | if forward: 503 | show_idx += speed 504 | pbar.update(speed) 505 | forward = False 506 | elif backward: 507 | show_idx -= speed 508 | pbar.update(-speed) 509 | backward = False 510 | else: 511 | pass 512 | pbar.set_postfix(f=pyft_f_value, t=pyft_t_value, s=stage['stage']) 513 | 514 | # keyboard quit 515 | if quit: 516 | break 517 | # keyboard reset 518 | if reset: 519 | view_control = visualizer.get_view_control() 520 | params = view_control.convert_to_pinhole_camera_parameters() 521 | params.extrinsic = np.array([[1, 0, 0, 0], 522 | [0, 1, 0, 0], 523 | [0, 0, 1, 0], 524 | [0, 0, 0, 1]]) 525 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 526 | reset = False 527 | # keyboard zero 528 | if zero: 529 | pbar.update(-show_idx) 530 | show_idx = 0 531 | zero = False 532 | 533 | visualizer.destroy_window() 534 | listener.stop() 535 | 536 | if not has_annotation: 537 | print(stages) 538 | with open(annotation_path, 'w') as f: 539 | json.dump(stages, f, indent=4) 540 | 541 | 542 | if __name__ == '__main__': 543 | args = config_parse() 544 | main(args) 545 | -------------------------------------------------------------------------------- /visualize_merge.py: -------------------------------------------------------------------------------- 1 | import os 2 | import configargparse 3 | import numpy as np 4 | import open3d as o3d 5 | from pynput import keyboard 6 | import tqdm 7 | import h5py 8 | 9 | from r3kit.utils.vis import rotation_vec2mat 10 | from utils.transformation import xyzquat2mat 11 | 12 | ''' 13 | Synchronize with `visualize_hdf5.py` some part 14 | ''' 15 | 16 | 17 | def config_parse() -> configargparse.Namespace: 18 | parser = configargparse.ArgumentParser() 19 | 20 | parser.add_argument('--config', is_config_file=True, help='config file path') 21 | parser.add_argument('--hdf5_path', type=str) 22 | parser.add_argument('--sample', action='store_true') 23 | 24 | args = parser.parse_args() 25 | return args 26 | 27 | 28 | def main(args): 29 | # general config 30 | hdf5_path = args.hdf5_path 31 | is_sample = args.sample 32 | 33 | # load hdf5 34 | with h5py.File(hdf5_path, 'r') as data_hdf5: 35 | data_group = data_hdf5['data'] 36 | data_attributes = dict(data_group.attrs) 37 | print(data_attributes) 38 | ft_coord = data_attributes['ft_coord'] 39 | 40 | for demo_idx, demo_name in enumerate(tqdm.tqdm(sorted(data_group.keys()))): 41 | print(demo_name) 42 | demo_group = data_group[demo_name] 43 | demo_attributes = dict(demo_group.attrs) 44 | print(demo_attributes) 45 | num_samples = demo_attributes['num_samples'] 46 | 47 | l515_pc_xyzs_l515 = demo_group['l515_pc_xyzs_l515'][:].astype(np.float32) 48 | l515_pc_rgbs = demo_group['l515_pc_rgbs'][:].astype(np.float32) 49 | gripper_xyzs_l515 = demo_group['gripper_xyzs_l515'][:].astype(np.float32) 50 | gripper_quats_l515 = demo_group['gripper_quats_l515'][:].astype(np.float32) 51 | pyft_xyzs_l515 = demo_group['pyft_xyzs_l515'][:].astype(np.float32) 52 | pyft_quats_l515 = demo_group['pyft_quats_l515'][:].astype(np.float32) 53 | pyft_fs = demo_group['pyft_fs'][:].astype(np.float32) 54 | pyft_ts = demo_group['pyft_ts'][:].astype(np.float32) 55 | angler_widths = demo_group['angler_widths'][:].astype(np.float32) 56 | len_seq = l515_pc_xyzs_l515.shape[0] 57 | print(len_seq) 58 | o_idxs = demo_group['o'][:].astype(int) 59 | a_idxs = demo_group['a'][:].astype(int) 60 | print(o_idxs[[0, num_samples//2, -1]]) 61 | print(a_idxs[[0, num_samples//2, -1]]) 62 | 63 | if is_sample: 64 | # create visualizer 65 | if demo_idx == 0: 66 | visualizer = o3d.visualization.Visualizer() 67 | visualizer.create_window(width=1280, height=720, left=200, top=200, visible=True, window_name='data') 68 | 69 | # loop samples 70 | for sample_idx in tqdm.trange(num_samples): 71 | o_idx = o_idxs[sample_idx] 72 | a_idx = a_idxs[sample_idx] 73 | 74 | for current_idx in o_idx: 75 | # add l515 elements 76 | l515_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 77 | visualizer.add_geometry(l515_frame) 78 | l515_pc_xyz_l515, l515_pc_rgb = l515_pc_xyzs_l515[current_idx], l515_pc_rgbs[current_idx] 79 | l515_pcd = o3d.geometry.PointCloud() 80 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 81 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 82 | visualizer.add_geometry(l515_pcd) 83 | # add gripper elements 84 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 85 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 86 | gripper_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 87 | gripper_frame.transform(gripper_pose_l515) 88 | visualizer.add_geometry(gripper_frame) 89 | gripper = o3d.io.read_triangle_mesh(os.path.join("objs", "gripper.obj")) 90 | gripper.transform(gripper_pose_l515) 91 | visualizer.add_geometry(gripper) 92 | # add pyft elements 93 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 94 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 95 | pyft_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 96 | pyft_frame.transform(pyft_pose_l515) 97 | visualizer.add_geometry(pyft_frame) 98 | if ft_coord: 99 | pyft_f_l515, pyft_t_l515 = pyft_fs[current_idx], pyft_ts[current_idx] 100 | else: 101 | pyft_f_pyft, pyft_t_pyft = pyft_fs[current_idx], pyft_ts[current_idx] 102 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 103 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 104 | pyft_f_value = np.linalg.norm(pyft_f_l515) 105 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 106 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 107 | pyft_t_value = np.linalg.norm(pyft_t_l515) 108 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 109 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 110 | pyft_f_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.04 * 0.025, cone_radius=0.04 * 0.05, cylinder_height=0.04 * 0.875, cone_height=0.04 * 0.125, 111 | resolution=20, cylinder_split=4, cone_split=1) 112 | pyft_f_arrow.paint_uniform_color([1., 1., 0.]) 113 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 114 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 115 | pyft_f_arrow.translate(pyft_f_translation_l515) 116 | visualizer.add_geometry(pyft_f_arrow) 117 | pyft_t_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.4 * 0.025, cone_radius=0.4 * 0.05, cylinder_height=0.4 * 0.875, cone_height=0.4 * 0.125, 118 | resolution=20, cylinder_split=4, cone_split=1) 119 | pyft_t_arrow.paint_uniform_color([0., 1., 1.]) 120 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 121 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 122 | pyft_t_arrow.translate(pyft_t_translation_l515) 123 | visualizer.add_geometry(pyft_t_arrow) 124 | pyft_peeler = o3d.io.read_triangle_mesh(os.path.join("objs", "peeler.obj")) 125 | pyft_peeler.transform(pyft_pose_l515) 126 | visualizer.add_geometry(pyft_peeler) 127 | # add angler elements 128 | angler_width = angler_widths[current_idx] 129 | angler_right_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "right_finger.obj")) 130 | angler_left_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "left_finger.obj")) 131 | angler_finger_pose_gripper = np.identity(4) 132 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 133 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 134 | angler_right_finger.transform(gripper_right_finger_pose_l515) 135 | visualizer.add_geometry(angler_right_finger) 136 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 137 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 138 | angler_left_finger.transform(gripper_left_finger_pose_l515) 139 | visualizer.add_geometry(angler_left_finger) 140 | 141 | # visualizer setup 142 | view_control = visualizer.get_view_control() 143 | visualizer.get_render_option().background_color = [0, 0, 0] 144 | params = view_control.convert_to_pinhole_camera_parameters() 145 | params.extrinsic = np.array([[1, 0, 0, 0], 146 | [0, 1, 0, 0], 147 | [0, 0, 1, 0], 148 | [0, 0, 0, 1]]) 149 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 150 | 151 | # visualize loop 152 | for current_idx in a_idx: 153 | # update gripper elements 154 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 155 | gripper_pose_l515_last = gripper_pose_l515.copy() 156 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 157 | gripper_frame.transform(np.linalg.inv(gripper_pose_l515_last)) 158 | gripper_frame.transform(gripper_pose_l515) 159 | visualizer.update_geometry(gripper_frame) 160 | gripper.transform(np.linalg.inv(gripper_pose_l515_last)) 161 | gripper.transform(gripper_pose_l515) 162 | gripper_delta_pose = np.dot(np.linalg.inv(gripper_pose_l515_last), gripper_pose_l515) 163 | visualizer.update_geometry(gripper) 164 | # update pyft elements 165 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 166 | pyft_pose_l515_last = pyft_pose_l515.copy() 167 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 168 | pyft_frame.transform(np.linalg.inv(pyft_pose_l515_last)) 169 | pyft_frame.transform(pyft_pose_l515) 170 | pyft_delta_pose = np.dot(np.linalg.inv(pyft_pose_l515_last), pyft_pose_l515) 171 | visualizer.update_geometry(pyft_frame) 172 | if ft_coord: 173 | pyft_f_l515_last, pyft_t_l515_last = pyft_f_l515.copy(), pyft_t_l515.copy() 174 | pyft_f_l515, pyft_t_l515 = pyft_fs[current_idx], pyft_ts[current_idx] 175 | pyft_delta_f, pyft_delta_t = pyft_f_l515 - pyft_f_l515_last, pyft_t_l515 - pyft_t_l515_last 176 | else: 177 | pyft_f_pyft_last, pyft_t_pyft_last = pyft_f_pyft.copy(), pyft_t_pyft.copy() 178 | pyft_f_pyft, pyft_t_pyft = pyft_fs[current_idx], pyft_ts[current_idx] 179 | pyft_delta_f, pyft_delta_t = pyft_f_pyft - pyft_f_pyft_last, pyft_t_pyft - pyft_t_pyft_last 180 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 181 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 182 | pyft_f_value_last = pyft_f_value.copy() 183 | pyft_f_value = np.linalg.norm(pyft_f_l515) 184 | pyft_f_rotation_l515_last = pyft_f_rotation_l515.copy() 185 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 186 | pyft_f_translation_l515_last = pyft_f_translation_l515.copy() 187 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 188 | pyft_t_value_last = pyft_t_value.copy() 189 | pyft_t_value = np.linalg.norm(pyft_t_l515) 190 | pyft_t_rotation_l515_last = pyft_t_rotation_l515.copy() 191 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 192 | pyft_t_translation_l515_last = pyft_t_translation_l515.copy() 193 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 194 | pyft_f_arrow.translate(-pyft_f_translation_l515_last) 195 | pyft_f_arrow.rotate(np.linalg.inv(pyft_f_rotation_l515_last), np.array([[0], [0], [0]])) 196 | pyft_f_arrow.scale(1/pyft_f_value_last, np.array([[0], [0], [0]])) 197 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 198 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 199 | pyft_f_arrow.translate(pyft_f_translation_l515) 200 | visualizer.update_geometry(pyft_f_arrow) 201 | pyft_t_arrow.translate(-pyft_t_translation_l515_last) 202 | pyft_t_arrow.rotate(np.linalg.inv(pyft_t_rotation_l515_last), np.array([[0], [0], [0]])) 203 | pyft_t_arrow.scale(1/pyft_t_value_last, np.array([[0], [0], [0]])) 204 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 205 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 206 | pyft_t_arrow.translate(pyft_t_translation_l515) 207 | visualizer.update_geometry(pyft_t_arrow) 208 | pyft_peeler.transform(np.linalg.inv(pyft_pose_l515_last)) 209 | pyft_peeler.transform(pyft_pose_l515) 210 | visualizer.update_geometry(pyft_peeler) 211 | # update angler elements 212 | angler_width_last = angler_width.copy() 213 | angler_width = angler_widths[current_idx] 214 | angler_finger_pose_gripper = np.identity(4) 215 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 216 | gripper_right_finger_pose_l515_last = gripper_right_finger_pose_l515.copy() 217 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 218 | angler_right_finger.transform(np.linalg.inv(gripper_right_finger_pose_l515_last)) 219 | angler_right_finger.transform(gripper_right_finger_pose_l515) 220 | visualizer.update_geometry(angler_right_finger) 221 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 222 | gripper_left_finger_pose_l515_last = gripper_left_finger_pose_l515.copy() 223 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 224 | angler_left_finger.transform(np.linalg.inv(gripper_left_finger_pose_l515_last)) 225 | angler_left_finger.transform(gripper_left_finger_pose_l515) 226 | visualizer.update_geometry(angler_left_finger) 227 | 228 | # visualizer update 229 | visualizer.poll_events() 230 | visualizer.update_renderer() 231 | 232 | visualizer.clear_geometries() 233 | else: 234 | # create keyboard listener 235 | quit = False 236 | reset = False 237 | pause = False 238 | zero = False 239 | forward = False 240 | backward = False 241 | speed = 1 242 | def _on_press(key): 243 | nonlocal quit, reset, pause, zero, forward, backward, speed 244 | if hasattr(key, 'char') and key.char == 'q': 245 | quit = True 246 | print("quit") 247 | if hasattr(key, 'char') and key.char == 'r': 248 | reset = True 249 | print("reset") 250 | if hasattr(key, 'char') and key.char == 'p': 251 | pause = not pause 252 | forward = False 253 | backward = False 254 | print("pause" if pause else "continue") 255 | if key == keyboard.Key.backspace: 256 | zero = True 257 | print("zero") 258 | if pause and key == keyboard.Key.right: 259 | forward = True 260 | print("forward") 261 | if pause and key == keyboard.Key.left: 262 | backward = True 263 | print("backward") 264 | if key == keyboard.Key.up: 265 | speed *= 2 266 | print(f"speed {speed}") 267 | if key == keyboard.Key.down: 268 | speed //= 2 269 | speed = max(speed, 1) 270 | print(f"speed {speed}") 271 | def _on_release(key): 272 | pass 273 | listener = keyboard.Listener(on_press=_on_press, on_release=_on_release) 274 | listener.start() 275 | 276 | # process variables 277 | current_idx = 0 278 | 279 | # create visualizer 280 | if demo_idx == 0: 281 | visualizer = o3d.visualization.Visualizer() 282 | visualizer.create_window(width=1280, height=720, left=200, top=200, visible=True, window_name='data') 283 | 284 | # add l515 elements 285 | l515_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 286 | visualizer.add_geometry(l515_frame) 287 | l515_pc_xyz_l515, l515_pc_rgb = l515_pc_xyzs_l515[current_idx], l515_pc_rgbs[current_idx] 288 | l515_pcd = o3d.geometry.PointCloud() 289 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 290 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 291 | visualizer.add_geometry(l515_pcd) 292 | # add gripper elements 293 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 294 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 295 | gripper_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 296 | gripper_frame.transform(gripper_pose_l515) 297 | visualizer.add_geometry(gripper_frame) 298 | gripper = o3d.io.read_triangle_mesh(os.path.join("objs", "gripper.obj")) 299 | gripper.transform(gripper_pose_l515) 300 | visualizer.add_geometry(gripper) 301 | # add pyft elements 302 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 303 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 304 | pyft_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05, origin=np.array([0., 0., 0.])) 305 | pyft_frame.transform(pyft_pose_l515) 306 | visualizer.add_geometry(pyft_frame) 307 | if ft_coord: 308 | pyft_f_l515, pyft_t_l515 = pyft_fs[current_idx], pyft_ts[current_idx] 309 | else: 310 | pyft_f_pyft, pyft_t_pyft = pyft_fs[current_idx], pyft_ts[current_idx] 311 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 312 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 313 | pyft_f_value = np.linalg.norm(pyft_f_l515) 314 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 315 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 316 | pyft_t_value = np.linalg.norm(pyft_t_l515) 317 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 318 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 319 | pyft_f_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.04 * 0.025, cone_radius=0.04 * 0.05, cylinder_height=0.04 * 0.875, cone_height=0.04 * 0.125, 320 | resolution=20, cylinder_split=4, cone_split=1) 321 | pyft_f_arrow.paint_uniform_color([1., 1., 0.]) 322 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 323 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 324 | pyft_f_arrow.translate(pyft_f_translation_l515) 325 | visualizer.add_geometry(pyft_f_arrow) 326 | pyft_t_arrow = o3d.geometry.TriangleMesh.create_arrow(cylinder_radius=0.4 * 0.025, cone_radius=0.4 * 0.05, cylinder_height=0.4 * 0.875, cone_height=0.4 * 0.125, 327 | resolution=20, cylinder_split=4, cone_split=1) 328 | pyft_t_arrow.paint_uniform_color([0., 1., 1.]) 329 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 330 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 331 | pyft_t_arrow.translate(pyft_t_translation_l515) 332 | visualizer.add_geometry(pyft_t_arrow) 333 | pyft_peeler = o3d.io.read_triangle_mesh(os.path.join("objs", "peeler.obj")) 334 | pyft_peeler.transform(pyft_pose_l515) 335 | visualizer.add_geometry(pyft_peeler) 336 | # add angler elements 337 | angler_width = angler_widths[current_idx] 338 | angler_right_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "right_finger.obj")) 339 | angler_left_finger = o3d.io.read_triangle_mesh(os.path.join("objs", "left_finger.obj")) 340 | angler_finger_pose_gripper = np.identity(4) 341 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 342 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 343 | angler_right_finger.transform(gripper_right_finger_pose_l515) 344 | visualizer.add_geometry(angler_right_finger) 345 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 346 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 347 | angler_left_finger.transform(gripper_left_finger_pose_l515) 348 | visualizer.add_geometry(angler_left_finger) 349 | 350 | # visualizer setup 351 | view_control = visualizer.get_view_control() 352 | visualizer.get_render_option().background_color = [0, 0, 0] 353 | params = view_control.convert_to_pinhole_camera_parameters() 354 | params.extrinsic = np.array([[1, 0, 0, 0], 355 | [0, 1, 0, 0], 356 | [0, 0, 1, 0], 357 | [0, 0, 0, 1]]) 358 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 359 | 360 | # visualize loop 361 | gripper_xyz_deltas, gripper_quat_deltas, pyft_xyz_deltas, pyft_quat_deltas, pyft_f_deltas, pyft_t_deltas, angler_width_deltas = [], [], [], [], [], [], [] 362 | with tqdm.tqdm(total=len_seq) as pbar: 363 | while current_idx < len_seq: 364 | # update l515 elements 365 | l515_pc_xyz_l515, l515_pc_rgb = l515_pc_xyzs_l515[current_idx], l515_pc_rgbs[current_idx] 366 | l515_pcd.points = o3d.utility.Vector3dVector(l515_pc_xyz_l515) 367 | l515_pcd.colors = o3d.utility.Vector3dVector(l515_pc_rgb) 368 | visualizer.update_geometry(l515_pcd) 369 | # update gripper elements 370 | gripper_xyz_l515, gripper_quat_l515 = gripper_xyzs_l515[current_idx], gripper_quats_l515[current_idx] 371 | gripper_pose_l515_last = gripper_pose_l515.copy() 372 | gripper_pose_l515 = xyzquat2mat(gripper_xyz_l515, gripper_quat_l515) 373 | gripper_frame.transform(np.linalg.inv(gripper_pose_l515_last)) 374 | gripper_frame.transform(gripper_pose_l515) 375 | visualizer.update_geometry(gripper_frame) 376 | gripper.transform(np.linalg.inv(gripper_pose_l515_last)) 377 | gripper.transform(gripper_pose_l515) 378 | gripper_delta_pose = np.dot(np.linalg.inv(gripper_pose_l515_last), gripper_pose_l515) 379 | visualizer.update_geometry(gripper) 380 | # update pyft elements 381 | pyft_xyz_l515, pyft_quat_l515 = pyft_xyzs_l515[current_idx], pyft_quats_l515[current_idx] 382 | pyft_pose_l515_last = pyft_pose_l515.copy() 383 | pyft_pose_l515 = xyzquat2mat(pyft_xyz_l515, pyft_quat_l515) 384 | pyft_frame.transform(np.linalg.inv(pyft_pose_l515_last)) 385 | pyft_frame.transform(pyft_pose_l515) 386 | pyft_delta_pose = np.dot(np.linalg.inv(pyft_pose_l515_last), pyft_pose_l515) 387 | visualizer.update_geometry(pyft_frame) 388 | if ft_coord: 389 | pyft_f_l515_last, pyft_t_l515_last = pyft_f_l515.copy(), pyft_t_l515.copy() 390 | pyft_f_l515, pyft_t_l515 = pyft_fs[current_idx], pyft_ts[current_idx] 391 | pyft_delta_f, pyft_delta_t = pyft_f_l515 - pyft_f_l515_last, pyft_t_l515 - pyft_t_l515_last 392 | else: 393 | pyft_f_pyft_last, pyft_t_pyft_last = pyft_f_pyft.copy(), pyft_t_pyft.copy() 394 | pyft_f_pyft, pyft_t_pyft = pyft_fs[current_idx], pyft_ts[current_idx] 395 | pyft_delta_f, pyft_delta_t = pyft_f_pyft - pyft_f_pyft_last, pyft_t_pyft - pyft_t_pyft_last 396 | pyft_f_l515 = pyft_pose_l515[:3, :3] @ pyft_f_pyft 397 | pyft_t_l515 = pyft_pose_l515[:3, :3] @ pyft_t_pyft 398 | pyft_f_value_last = pyft_f_value.copy() 399 | pyft_f_value = np.linalg.norm(pyft_f_l515) 400 | pyft_f_rotation_l515_last = pyft_f_rotation_l515.copy() 401 | pyft_f_rotation_l515 = rotation_vec2mat(pyft_f_l515 / pyft_f_value) 402 | pyft_f_translation_l515_last = pyft_f_translation_l515.copy() 403 | pyft_f_translation_l515 = pyft_pose_l515[:3, 3] 404 | pyft_t_value_last = pyft_t_value.copy() 405 | pyft_t_value = np.linalg.norm(pyft_t_l515) 406 | pyft_t_rotation_l515_last = pyft_t_rotation_l515.copy() 407 | pyft_t_rotation_l515 = rotation_vec2mat(pyft_t_l515 / pyft_t_value) 408 | pyft_t_translation_l515_last = pyft_t_translation_l515.copy() 409 | pyft_t_translation_l515 = pyft_pose_l515[:3, 3] 410 | pyft_f_arrow.translate(-pyft_f_translation_l515_last) 411 | pyft_f_arrow.rotate(np.linalg.inv(pyft_f_rotation_l515_last), np.array([[0], [0], [0]])) 412 | pyft_f_arrow.scale(1/pyft_f_value_last, np.array([[0], [0], [0]])) 413 | pyft_f_arrow.scale(pyft_f_value, np.array([[0], [0], [0]])) 414 | pyft_f_arrow.rotate(pyft_f_rotation_l515, np.array([[0], [0], [0]])) 415 | pyft_f_arrow.translate(pyft_f_translation_l515) 416 | visualizer.update_geometry(pyft_f_arrow) 417 | pyft_t_arrow.translate(-pyft_t_translation_l515_last) 418 | pyft_t_arrow.rotate(np.linalg.inv(pyft_t_rotation_l515_last), np.array([[0], [0], [0]])) 419 | pyft_t_arrow.scale(1/pyft_t_value_last, np.array([[0], [0], [0]])) 420 | pyft_t_arrow.scale(pyft_t_value, np.array([[0], [0], [0]])) 421 | pyft_t_arrow.rotate(pyft_t_rotation_l515, np.array([[0], [0], [0]])) 422 | pyft_t_arrow.translate(pyft_t_translation_l515) 423 | visualizer.update_geometry(pyft_t_arrow) 424 | pyft_peeler.transform(np.linalg.inv(pyft_pose_l515_last)) 425 | pyft_peeler.transform(pyft_pose_l515) 426 | visualizer.update_geometry(pyft_peeler) 427 | # update angler elements 428 | angler_width_last = angler_width.copy() 429 | angler_width = angler_widths[current_idx] 430 | angler_finger_pose_gripper = np.identity(4) 431 | angler_finger_pose_gripper[0, 3] = angler_width / 2. 432 | gripper_right_finger_pose_l515_last = gripper_right_finger_pose_l515.copy() 433 | gripper_right_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 434 | angler_right_finger.transform(np.linalg.inv(gripper_right_finger_pose_l515_last)) 435 | angler_right_finger.transform(gripper_right_finger_pose_l515) 436 | visualizer.update_geometry(angler_right_finger) 437 | angler_finger_pose_gripper[0, 3] = -angler_width / 2. 438 | gripper_left_finger_pose_l515_last = gripper_left_finger_pose_l515.copy() 439 | gripper_left_finger_pose_l515 = gripper_pose_l515 @ angler_finger_pose_gripper 440 | angler_left_finger.transform(np.linalg.inv(gripper_left_finger_pose_l515_last)) 441 | angler_left_finger.transform(gripper_left_finger_pose_l515) 442 | visualizer.update_geometry(angler_left_finger) 443 | 444 | # visualizer update 445 | visualizer.poll_events() 446 | visualizer.update_renderer() 447 | 448 | # pbar update 449 | current_idx_last = current_idx 450 | if not pause: 451 | current_idx += speed 452 | pbar.update(speed) 453 | else: 454 | if forward: 455 | current_idx += speed 456 | pbar.update(speed) 457 | forward = False 458 | elif backward: 459 | current_idx -= speed 460 | pbar.update(-speed) 461 | backward = False 462 | else: 463 | pass 464 | pbar.set_postfix(f=pyft_f_value, t=pyft_t_value) 465 | 466 | # keyboard quit 467 | if quit: 468 | break 469 | # keyboard reset 470 | if reset: 471 | view_control = visualizer.get_view_control() 472 | params = view_control.convert_to_pinhole_camera_parameters() 473 | params.extrinsic = np.array([[1, 0, 0, 0], 474 | [0, 1, 0, 0], 475 | [0, 0, 1, 0], 476 | [0, 0, 0, 1]]) 477 | view_control.convert_from_pinhole_camera_parameters(params, allow_arbitrary=True) 478 | reset = False 479 | # keyboard zero 480 | if zero: 481 | pbar.update(-current_idx) 482 | current_idx = 0 483 | zero = False 484 | 485 | if current_idx != current_idx_last: 486 | gripper_xyz_delta_mm = np.linalg.norm(gripper_delta_pose[:3, 3]) * 1000 487 | gripper_quat_delta_deg = np.arccos(np.clip((np.trace(gripper_delta_pose[:3, :3]) - 1) / 2, -1, 1)) / np.pi * 180 488 | pyft_xyz_delta_mm = np.linalg.norm(pyft_delta_pose[:3, 3]) * 1000 489 | pyft_quat_delta_deg = np.arccos(np.clip((np.trace(pyft_delta_pose[:3, :3]) - 1) / 2, -1, 1)) / np.pi * 180 490 | angler_width_delta_mm = abs(angler_width - angler_width_last) * 1000 491 | pyft_f_delta_n = np.linalg.norm(pyft_delta_f) 492 | pyft_t_delta = np.linalg.norm(pyft_delta_t) 493 | ### print(gripper_xyz_delta_mm) 494 | 495 | gripper_xyz_deltas.append(gripper_xyz_delta_mm) 496 | gripper_quat_deltas.append(gripper_quat_delta_deg) 497 | pyft_xyz_deltas.append(pyft_xyz_delta_mm) 498 | pyft_quat_deltas.append(pyft_quat_delta_deg) 499 | angler_width_deltas.append(angler_width_delta_mm) 500 | pyft_f_deltas.append(pyft_f_delta_n) 501 | pyft_t_deltas.append(pyft_t_delta) 502 | 503 | visualizer.clear_geometries() 504 | listener.stop() 505 | 506 | 507 | if __name__ == '__main__': 508 | args = config_parse() 509 | main(args) 510 | -------------------------------------------------------------------------------- /objs/only_peeler.obj: -------------------------------------------------------------------------------- 1 | # https://github.com/mikedh/trimesh 2 | v 0.01969576 -0.00950000 0.10450000 3 | v 0.00500000 -0.01025000 0.11200000 4 | v -0.01969576 -0.00950000 0.10450000 5 | v -0.00500000 -0.01025000 0.11200000 6 | v -0.01017638 -0.01031815 0.11268149 7 | v -0.01500000 -0.01051795 0.11467950 8 | v -0.02255965 -0.00971780 0.10667805 9 | v -0.01914214 -0.01083579 0.11785786 10 | v 0.03300000 -0.01400000 0.14950000 11 | v 0.02500000 -0.01400000 0.14950000 12 | v 0.03300000 -0.01199249 0.12942491 13 | v 0.02500000 -0.01225000 0.13200000 14 | v 0.03230834 -0.01135202 0.12302016 15 | v 0.02431852 -0.01173236 0.12682362 16 | v 0.03026527 -0.01074107 0.11691072 17 | v 0.02232051 -0.01125000 0.12200000 18 | v 0.02696498 -0.01018783 0.11137831 19 | v 0.01914214 -0.01083579 0.11785786 20 | v 0.02255965 -0.00971780 0.10667805 21 | v 0.01500000 -0.01051795 0.11467950 22 | v 0.01017638 -0.01031815 0.11268149 23 | v -0.02500000 -0.01400000 0.14950000 24 | v -0.03300000 -0.01400000 0.14950000 25 | v -0.02500000 -0.01225000 0.13200000 26 | v -0.03300000 -0.01199249 0.12942491 27 | v -0.02431852 -0.01173236 0.12682362 28 | v -0.03230834 -0.01135202 0.12302016 29 | v -0.02232051 -0.01125000 0.12200000 30 | v -0.03026527 -0.01074107 0.11691072 31 | v -0.02696498 -0.01018783 0.11137831 32 | v -0.02500000 -0.00800000 0.14950000 33 | v -0.03300000 -0.00800000 0.14950000 34 | v -0.02500000 -0.00877980 0.14950000 35 | v 0.02500000 -0.00877980 0.14950000 36 | v 0.03300000 -0.00800000 0.14950000 37 | v 0.02500000 -0.00800000 0.14950000 38 | v 0.00853250 0.00946319 0.10459465 39 | v 0.00753337 0.00950000 0.10450000 40 | v 0.00500000 0.00658333 0.11200000 41 | v -0.00500000 0.00658333 0.11200000 42 | v 0.01738246 0.00831713 0.10754166 43 | v 0.01556225 0.00855669 0.10692567 44 | v 0.01500000 0.00554131 0.11467950 45 | v -0.01842159 0.00815726 0.10795277 46 | v -0.01500000 0.00554131 0.11467950 47 | v -0.01556225 0.00855669 0.10692567 48 | v -0.01017638 0.00631831 0.11268149 49 | v -0.01153337 0.00911044 0.10550172 50 | v 0.01017638 0.00631831 0.11268149 51 | v -0.02110212 0.00765516 0.10924387 52 | v -0.02280672 0.00725679 0.11026824 53 | v -0.01914214 0.00430527 0.11785786 54 | v 0.01246556 0.00897500 0.10585001 55 | v 0.01153337 0.00911044 0.10550172 56 | v 0.00996015 0.00932302 0.10495509 57 | v 0.02548831 0.00646887 0.11229433 58 | v 0.02322510 0.00714799 0.11054803 59 | v 0.01914214 0.00430527 0.11785786 60 | v 0.02280672 0.00725679 0.11026824 61 | v 0.02019846 0.00784004 0.10876846 62 | v 0.02749138 0.00570433 0.11426028 63 | v 0.02232051 0.00269444 0.12200000 64 | v 0.02922651 0.00485466 0.11644517 65 | v 0.03300000 -0.00019302 0.12942491 66 | v 0.03268244 0.00150000 0.12507142 67 | v 0.02431852 0.00081859 0.12682362 68 | v -0.00753337 0.00950000 0.10450000 69 | v -0.00891782 0.00943325 0.10467164 70 | v -0.01107870 0.00917512 0.10533541 71 | v 0.03237303 0.00213148 0.12344762 72 | v 0.03159319 0.00311886 0.12090865 73 | v 0.03048399 0.00405953 0.11848978 74 | v 0.02500000 -0.00785280 0.14912148 75 | v 0.02500000 -0.00119444 0.13200000 76 | v -0.02509170 0.00660016 0.11195673 77 | v -0.02711819 0.00586141 0.11385636 78 | v -0.02232051 0.00269444 0.12200000 79 | v -0.02888007 0.00504285 0.11596124 80 | v -0.02922651 0.00485466 0.11644517 81 | v -0.02431852 0.00081859 0.12682362 82 | v -0.03268244 0.00150000 0.12507142 83 | v -0.02500000 -0.00119444 0.13200000 84 | v -0.03300000 -0.00019302 0.12942491 85 | v -0.02500000 -0.00785280 0.14912148 86 | v -0.03061928 0.00396031 0.11874491 87 | v -0.03168686 0.00302148 0.12115904 88 | v -0.03237303 0.00213148 0.12344762 89 | v -0.03300000 -0.00925000 0.14320096 90 | v -0.02500000 -0.00925000 0.14320096 91 | v -0.03300000 -0.00870096 0.14375000 92 | v -0.02500000 -0.00870096 0.14375000 93 | v -0.03300000 -0.00850000 0.14450000 94 | v -0.02500000 -0.00850000 0.14450000 95 | v -0.03300000 -0.00870096 0.14525000 96 | v -0.02500000 -0.00870096 0.14525000 97 | v -0.02500000 -0.00906090 0.14566965 98 | v -0.03300000 -0.00925000 0.14579904 99 | v -0.02500000 -0.00925000 0.14579904 100 | v -0.03300000 -0.01000000 0.14600000 101 | v -0.02500000 -0.01000000 0.14600000 102 | v -0.03300000 -0.01075000 0.14579904 103 | v -0.02500000 -0.01000476 0.14600000 104 | v -0.02500000 -0.01075000 0.14579904 105 | v -0.03300000 -0.01129904 0.14525000 106 | v -0.02500000 -0.01129904 0.14525000 107 | v -0.03300000 -0.01150000 0.14450000 108 | v -0.02500000 -0.01150000 0.14450000 109 | v -0.03300000 -0.01129904 0.14375000 110 | v -0.02500000 -0.01129904 0.14375000 111 | v -0.02500000 -0.01093910 0.14333035 112 | v -0.03300000 -0.01075000 0.14320096 113 | v -0.02500000 -0.01075000 0.14320096 114 | v -0.03300000 -0.01000000 0.14300000 115 | v -0.02500000 -0.01000000 0.14300000 116 | v -0.02500000 -0.00999524 0.14300000 117 | v 0.03300000 -0.00925000 0.14579904 118 | v 0.02500000 -0.00906090 0.14566965 119 | v 0.03300000 -0.00870096 0.14525000 120 | v 0.02500000 -0.00870096 0.14525000 121 | v 0.03300000 -0.00850000 0.14450000 122 | v 0.02500000 -0.00850000 0.14450000 123 | v 0.03300000 -0.00870096 0.14375000 124 | v 0.02500000 -0.00870096 0.14375000 125 | v 0.03300000 -0.00925000 0.14320096 126 | v 0.03300000 -0.01075000 0.14579904 127 | v 0.02500000 -0.01000476 0.14600000 128 | v 0.03300000 -0.01000000 0.14600000 129 | v 0.02500000 -0.01000000 0.14600000 130 | v 0.02500000 -0.00925000 0.14579904 131 | v 0.03300000 -0.01075000 0.14320096 132 | v 0.02500000 -0.01093910 0.14333035 133 | v 0.03300000 -0.01129904 0.14375000 134 | v 0.02500000 -0.01129904 0.14375000 135 | v 0.03300000 -0.01150000 0.14450000 136 | v 0.02500000 -0.01150000 0.14450000 137 | v 0.03300000 -0.01129904 0.14525000 138 | v 0.02500000 -0.01129904 0.14525000 139 | v 0.02500000 -0.01075000 0.14579904 140 | v 0.02500000 -0.00925000 0.14320096 141 | v 0.02500000 -0.00999524 0.14300000 142 | v 0.03300000 -0.01000000 0.14300000 143 | v 0.02500000 -0.01000000 0.14300000 144 | v 0.02500000 -0.01075000 0.14320096 145 | v -0.01250000 -0.00150000 0.09450000 146 | v -0.01250000 0.00150000 0.09450000 147 | v -0.01250000 -0.00150000 0.00450000 148 | v -0.01250000 0.00150000 0.00450000 149 | v 0.01725242 -0.00950000 0.10302665 150 | v 0.00521025 -0.00950000 0.09950645 151 | v -0.00521025 -0.00950000 0.09950645 152 | v -0.00490902 -0.00950000 0.09831542 153 | v -0.01128801 -0.00950000 0.10059249 154 | v -0.01725242 -0.00950000 0.10302665 155 | v 0.01128801 -0.00950000 0.10059249 156 | v -0.00450000 -0.00950000 0.00450000 157 | v 0.00450000 -0.00950000 0.00450000 158 | v -0.00450000 -0.00950000 0.09450000 159 | v 0.00450000 -0.00950000 0.09450000 160 | v 0.00490902 -0.00950000 0.09831542 161 | v 0.01250000 -0.00150000 0.00450000 162 | v 0.01250000 0.00150000 0.00450000 163 | v 0.01250000 -0.00150000 0.09450000 164 | v 0.01250000 0.00150000 0.09450000 165 | v -0.00609855 0.00950000 0.10191569 166 | v -0.00490418 0.00950000 0.09829301 167 | v 0.00450000 0.00950000 0.00450000 168 | v -0.00450000 0.00950000 0.00450000 169 | v 0.00450000 0.00950000 0.09450000 170 | v -0.00450000 0.00950000 0.09450000 171 | v 0.00490418 0.00950000 0.09829301 172 | v 0.00609855 0.00950000 0.10191569 173 | v -0.02500000 -0.01257186 0.13866518 174 | v -0.02500000 -0.01162799 0.13833482 175 | v 0.02500000 -0.01257186 0.13866518 176 | v 0.02500000 -0.01162799 0.13833482 177 | v -0.01272454 0.00150000 0.09660723 178 | v -0.01272723 -0.00150000 0.09661968 179 | v -0.01311957 0.00150000 0.09796519 180 | v -0.01311957 -0.00150000 0.09796519 181 | v -0.01338808 0.00150000 0.09861983 182 | v -0.01339861 -0.00150000 0.09864303 183 | v -0.01446082 0.00150000 0.10044741 184 | v -0.01448361 -0.00150000 0.10047809 185 | v -0.01490150 0.00150000 0.10100098 186 | v -0.01490150 -0.00150000 0.10100098 187 | v -0.01589459 0.00150000 0.10200790 188 | v -0.01593294 -0.00150000 0.10204146 189 | v -0.01762500 0.00150000 0.10323123 190 | v -0.01762500 -0.00150000 0.10323123 191 | v 0.01762500 0.00150000 0.10323123 192 | v 0.01762500 -0.00150000 0.10323123 193 | v 0.01589459 0.00150000 0.10200790 194 | v 0.01593294 -0.00150000 0.10204146 195 | v 0.01490150 0.00150000 0.10100098 196 | v 0.01490150 -0.00150000 0.10100098 197 | v 0.01446082 0.00150000 0.10044741 198 | v 0.01448361 -0.00150000 0.10047809 199 | v 0.01338808 0.00150000 0.09861983 200 | v 0.01339861 -0.00150000 0.09864303 201 | v 0.01311957 0.00150000 0.09796519 202 | v 0.01311957 -0.00150000 0.09796519 203 | v 0.01272454 0.00150000 0.09660723 204 | v 0.01272723 -0.00150000 0.09661968 205 | v 0.01438301 -0.00504671 0.10166835 206 | v 0.01571743 -0.00367635 0.10225385 207 | v 0.01678135 -0.00248586 0.10277769 208 | v 0.01761413 -0.00151285 0.10322517 209 | v 0.02145972 0.00150000 0.10577669 210 | v 0.00700902 -0.00932557 0.09969400 211 | v 0.00850957 -0.00891988 0.09993518 212 | v 0.01043559 -0.00805415 0.10036099 213 | v 0.01130796 -0.00753252 0.10059823 214 | v 0.01235594 -0.00679777 0.10092113 215 | v 0.02255965 0.00150000 0.10667805 216 | v 0.02485994 0.00150000 0.10887879 217 | v 0.02696498 0.00150000 0.11137831 218 | v 0.02774560 0.00150000 0.11246452 219 | v 0.03004880 0.00150000 0.11644946 220 | v 0.03171531 0.00150000 0.12073982 221 | v 0.03230834 0.00150000 0.12302016 222 | v -0.00677005 -0.00936851 0.09966275 223 | v -0.03230834 0.00150000 0.12302016 224 | v -0.03171531 0.00150000 0.12073982 225 | v -0.03004880 0.00150000 0.11644946 226 | v -0.02774560 0.00150000 0.11246452 227 | v -0.02696498 0.00150000 0.11137831 228 | v -0.02485994 0.00150000 0.10887879 229 | v -0.02145972 0.00150000 0.10577669 230 | v -0.02255965 0.00150000 0.10667805 231 | v -0.01761413 -0.00151285 0.10322517 232 | v -0.01566287 -0.00373531 0.10222838 233 | v -0.01370346 -0.00568087 0.10139928 234 | v -0.01235594 -0.00679777 0.10092113 235 | v -0.01130394 -0.00753511 0.10059708 236 | v -0.01061316 -0.00795458 0.10040700 237 | v -0.00878520 -0.00881987 0.09998801 238 | v -0.00850957 -0.00891988 0.09993518 239 | v -0.01210845 -0.00397214 0.09450000 240 | v -0.01189104 -0.00456147 0.00450000 241 | v -0.01189104 -0.00456147 0.09450000 242 | v -0.01097214 -0.00620228 0.09450000 243 | v -0.01015685 -0.00715685 0.00450000 244 | v -0.01015685 -0.00715685 0.09450000 245 | v -0.00920228 -0.00797214 0.09450000 246 | v -0.00756147 -0.00889104 0.00450000 247 | v -0.00756147 -0.00889104 0.09450000 248 | v -0.00697214 -0.00910845 0.09450000 249 | v -0.00950445 -0.00797214 0.09731870 250 | v -0.01416973 -0.00397214 0.10071216 251 | v -0.01200804 -0.00620228 0.09927603 252 | v -0.01039723 -0.00797214 0.10000928 253 | v -0.00732498 -0.00910845 0.09779141 254 | v -0.01123409 -0.00620228 0.09694354 255 | v -0.01234458 -0.00397214 0.09670267 256 | v -0.01304225 -0.00397214 0.09880525 257 | v 0.00697214 -0.00910845 0.09450000 258 | v 0.00756147 -0.00889104 0.00450000 259 | v 0.00756147 -0.00889104 0.09450000 260 | v 0.00920228 -0.00797214 0.09450000 261 | v 0.01015685 -0.00715685 0.00450000 262 | v 0.01015685 -0.00715685 0.09450000 263 | v 0.01097214 -0.00620228 0.09450000 264 | v 0.01189104 -0.00456147 0.00450000 265 | v 0.01189104 -0.00456147 0.09450000 266 | v 0.01210845 -0.00397214 0.09450000 267 | v 0.00732498 -0.00910845 0.09779141 268 | v 0.01416973 -0.00397214 0.10071216 269 | v 0.01039723 -0.00797214 0.10000928 270 | v 0.01200804 -0.00620228 0.09927603 271 | v 0.01304225 -0.00397214 0.09880525 272 | v 0.00950445 -0.00797214 0.09731870 273 | v 0.01123409 -0.00620228 0.09694354 274 | v 0.01234458 -0.00397214 0.09670267 275 | v 0.01210845 0.00397214 0.09450000 276 | v 0.01189104 0.00456147 0.00450000 277 | v 0.01189104 0.00456147 0.09450000 278 | v 0.01097214 0.00620228 0.09450000 279 | v 0.01015685 0.00715685 0.00450000 280 | v 0.01015685 0.00715685 0.09450000 281 | v 0.00920228 0.00797214 0.09450000 282 | v 0.00756147 0.00889104 0.00450000 283 | v 0.00756147 0.00889104 0.09450000 284 | v 0.00697214 0.00910845 0.09450000 285 | v 0.01234179 0.00397214 0.09668974 286 | v 0.01371632 0.00797214 0.10448380 287 | v 0.00835114 0.00910845 0.10089721 288 | v 0.00732080 0.00910845 0.09777208 289 | v 0.00950087 0.00797214 0.09730213 290 | v 0.01123098 0.00620228 0.09692918 291 | v 0.01001688 0.00910845 0.10373505 292 | v 0.01038323 0.00797214 0.09997843 293 | v 0.01180973 0.00797214 0.10240870 294 | v 0.01199591 0.00620228 0.09924928 295 | v 0.01323255 0.00620228 0.10135609 296 | v 0.01303131 0.00397214 0.09878114 297 | v 0.01414605 0.00397214 0.10068027 298 | v 0.01601736 0.00797214 0.10611054 299 | v 0.01639970 0.00732216 0.10542576 300 | v 0.01488538 0.00620228 0.10315501 301 | v 0.01688017 0.00620228 0.10456524 302 | v 0.01563596 0.00397214 0.10230187 303 | v 0.01743412 0.00397214 0.10357310 304 | v 0.02127916 0.00364687 0.10600801 305 | v 0.02402032 0.00563623 0.10966794 306 | v 0.02679516 0.00563623 0.11311594 307 | v 0.02978422 0.00364687 0.11657638 308 | v 0.02750355 0.00364687 0.11263042 309 | v 0.02464611 0.00364687 0.10907977 310 | v 0.02075071 0.00563623 0.10668499 311 | v 0.01991315 0.00732216 0.10775797 312 | v -0.00697214 0.00910845 0.09450000 313 | v -0.00756147 0.00889104 0.00450000 314 | v -0.00756147 0.00889104 0.09450000 315 | v -0.00920228 0.00797214 0.09450000 316 | v -0.01015685 0.00715685 0.00450000 317 | v -0.01015685 0.00715685 0.09450000 318 | v -0.01097214 0.00620228 0.09450000 319 | v -0.01189104 0.00456147 0.00450000 320 | v -0.01189104 0.00456147 0.09450000 321 | v -0.01210845 0.00397214 0.09450000 322 | v -0.01563596 0.00397214 0.10230187 323 | v -0.00732080 0.00910845 0.09777208 324 | v -0.00835114 0.00910845 0.10089721 325 | v -0.01180973 0.00797214 0.10240870 326 | v -0.01001688 0.00910845 0.10373505 327 | v -0.00950087 0.00797214 0.09730213 328 | v -0.01038323 0.00797214 0.09997843 329 | v -0.01323255 0.00620228 0.10135609 330 | v -0.01199591 0.00620228 0.09924928 331 | v -0.01414605 0.00397214 0.10068027 332 | v -0.01303131 0.00397214 0.09878114 333 | v -0.01123098 0.00620228 0.09692918 334 | v -0.01234179 0.00397214 0.09668974 335 | v -0.01743412 0.00397214 0.10357310 336 | v -0.01488538 0.00620228 0.10315501 337 | v -0.01688017 0.00620228 0.10456524 338 | v -0.01371632 0.00797214 0.10448380 339 | v -0.01639970 0.00732216 0.10542576 340 | v -0.01601736 0.00797214 0.10611054 341 | v -0.02075071 0.00563623 0.10668499 342 | v -0.02127916 0.00364687 0.10600801 343 | v -0.02464611 0.00364687 0.10907977 344 | v -0.02750355 0.00364687 0.11263042 345 | v -0.02679516 0.00563623 0.11311594 346 | v -0.02402032 0.00563623 0.10966794 347 | v -0.01991315 0.00732216 0.10775797 348 | v -0.02978422 0.00364687 0.11657638 349 | v 0.02500000 -0.00837201 0.15066516 350 | v -0.02500000 -0.00837201 0.15066516 351 | v 0.02500000 -0.00742814 0.15033484 352 | v -0.02500000 -0.00742814 0.15033484 353 | v -0.01507959 -0.01617767 0.00450000 354 | v -0.01467767 -0.01767767 0.00450000 355 | v 0.01358883 -0.01208883 0.00450000 356 | v -0.01467767 0.01767767 0.00450000 357 | v -0.01507959 0.01617767 0.00450000 358 | v -0.01917767 0.01507959 0.00450000 359 | v -0.03500000 0.00000000 0.00450000 360 | v -0.03479483 -0.00378417 0.00450000 361 | v -0.01617767 0.02027575 0.00450000 362 | v -0.01295484 0.03251418 0.00450000 363 | v -0.01767767 0.02067767 0.00450000 364 | v -0.01964155 0.02896911 0.00450000 365 | v -0.01917767 0.02027575 0.00450000 366 | v -0.03316786 -0.01117555 0.00450000 367 | v -0.03283133 -0.01212862 0.00450000 368 | v -0.02027575 0.01917767 0.00450000 369 | v -0.02540984 0.02406948 0.00450000 370 | v -0.02067767 0.01767767 0.00450000 371 | v -0.02999000 0.01804438 0.00450000 372 | v -0.02027575 0.01617767 0.00450000 373 | v -0.03316786 0.01117555 0.00450000 374 | v -0.03479483 0.00378417 0.00450000 375 | v 0.00936349 0.03372425 0.00450000 376 | v 0.01366169 0.03222357 0.00450000 377 | v -0.01767767 0.01467767 0.00450000 378 | v -0.01617767 0.01507959 0.00450000 379 | v -0.01507959 0.01917767 0.00450000 380 | v 0.00189486 0.03494867 0.00450000 381 | v -0.00566237 0.03453893 0.00450000 382 | v 0.01917767 0.01507959 0.00450000 383 | v 0.01767767 0.01467767 0.00450000 384 | v 0.01617767 0.01507959 0.00450000 385 | v 0.01507959 0.01617767 0.00450000 386 | v 0.01467767 0.01767767 0.00450000 387 | v 0.03500000 0.00000000 0.00450000 388 | v 0.02027575 0.01617767 0.00450000 389 | v 0.03418172 0.00752397 0.00450000 390 | v 0.02067767 0.01767767 0.00450000 391 | v 0.03176514 0.01469612 0.00450000 392 | v 0.02027575 0.01917767 0.00450000 393 | v 0.02786326 0.02118110 0.00450000 394 | v 0.01917767 0.02027575 0.00450000 395 | v 0.02265852 0.02667567 0.00450000 396 | v 0.01767767 0.02067767 0.00450000 397 | v 0.01639429 0.03092292 0.00450000 398 | v 0.01617767 0.02027575 0.00450000 399 | v 0.01507959 0.01917767 0.00450000 400 | v 0.01467767 -0.01767767 0.00450000 401 | v 0.01507959 -0.01617767 0.00450000 402 | v 0.01617767 -0.01507959 0.00450000 403 | v 0.01767767 -0.01467767 0.00450000 404 | v 0.03282362 -0.01214948 0.00450000 405 | v 0.03418172 -0.00752397 0.00450000 406 | v 0.01917767 -0.01507959 0.00450000 407 | v 0.02027575 -0.01617767 0.00450000 408 | v 0.03176514 -0.01469612 0.00450000 409 | v 0.02067767 -0.01767767 0.00450000 410 | v 0.02786326 -0.02118110 0.00450000 411 | v 0.02027575 -0.01917767 0.00450000 412 | v 0.02265852 -0.02667567 0.00450000 413 | v 0.01917767 -0.02027575 0.00450000 414 | v 0.01639429 -0.03092292 0.00450000 415 | v 0.01767767 -0.02067767 0.00450000 416 | v 0.01366169 -0.03222357 0.00450000 417 | v 0.01617767 -0.02027575 0.00450000 418 | v 0.01507959 -0.01917767 0.00450000 419 | v 0.00936349 -0.03372425 0.00450000 420 | v -0.01507959 -0.01917767 0.00450000 421 | v -0.02027575 -0.01617767 0.00450000 422 | v -0.01917767 -0.01507959 0.00450000 423 | v -0.01767767 -0.01467767 0.00450000 424 | v -0.01617767 -0.01507959 0.00450000 425 | v -0.02067767 -0.01767767 0.00450000 426 | v -0.02999000 -0.01804438 0.00450000 427 | v -0.02027575 -0.01917767 0.00450000 428 | v -0.02540984 -0.02406948 0.00450000 429 | v -0.01917767 -0.02027575 0.00450000 430 | v -0.01964155 -0.02896911 0.00450000 431 | v -0.01767767 -0.02067767 0.00450000 432 | v -0.01295484 -0.03251418 0.00450000 433 | v -0.01617767 -0.02027575 0.00450000 434 | v -0.00566237 -0.03453893 0.00450000 435 | v 0.00189486 -0.03494867 0.00450000 436 | v 0.03176514 -0.01469612 -0.00150000 437 | v 0.02786326 -0.02118110 -0.00150000 438 | v 0.02265852 -0.02667567 -0.00150000 439 | v 0.01639429 -0.03092292 -0.00150000 440 | v 0.00936349 -0.03372425 -0.00150000 441 | v 0.00189486 -0.03494867 -0.00150000 442 | v -0.00566237 -0.03453893 -0.00150000 443 | v -0.01295484 -0.03251418 -0.00150000 444 | v -0.01964155 -0.02896911 -0.00150000 445 | v -0.02540984 -0.02406948 -0.00150000 446 | v -0.02999000 -0.01804438 -0.00150000 447 | v -0.03316786 -0.01117555 -0.00150000 448 | v -0.03479483 -0.00378417 -0.00150000 449 | v -0.03500000 0.00000000 -0.00150000 450 | v -0.03479483 0.00378417 -0.00150000 451 | v -0.03316786 0.01117555 -0.00150000 452 | v -0.02999000 0.01804438 -0.00150000 453 | v -0.02540984 0.02406948 -0.00150000 454 | v -0.01964155 0.02896911 -0.00150000 455 | v -0.01295484 0.03251418 -0.00150000 456 | v -0.00566237 0.03453893 -0.00150000 457 | v 0.00189486 0.03494867 -0.00150000 458 | v 0.00936349 0.03372425 -0.00150000 459 | v 0.01639429 0.03092292 -0.00150000 460 | v 0.02265852 0.02667567 -0.00150000 461 | v 0.02786326 0.02118110 -0.00150000 462 | v 0.03176514 0.01469612 -0.00150000 463 | v 0.03418172 0.00752397 -0.00150000 464 | v 0.03500000 0.00000000 -0.00150000 465 | v 0.03418172 -0.00752397 -0.00150000 466 | v -0.01767767 -0.02067767 -0.00150000 467 | v -0.01917767 -0.02027575 -0.00150000 468 | v 0.01617767 -0.01507959 -0.00150000 469 | v 0.01507959 -0.01617767 -0.00150000 470 | v -0.01767767 0.01467767 -0.00150000 471 | v 0.01507959 -0.01917767 -0.00150000 472 | v 0.01467767 -0.01767767 -0.00150000 473 | v -0.01617767 -0.02027575 -0.00150000 474 | v 0.01917767 -0.01507959 -0.00150000 475 | v 0.01767767 -0.01467767 -0.00150000 476 | v 0.01617767 0.01507959 -0.00150000 477 | v -0.01507959 -0.01917767 -0.00150000 478 | v -0.01467767 -0.01767767 -0.00150000 479 | v -0.01507959 -0.01617767 -0.00150000 480 | v -0.01917767 -0.01507959 -0.00150000 481 | v 0.01507959 0.01617767 -0.00150000 482 | v 0.01467767 0.01767767 -0.00150000 483 | v 0.01507959 0.01917767 -0.00150000 484 | v -0.02027575 -0.01917767 -0.00150000 485 | v -0.02067767 -0.01767767 -0.00150000 486 | v -0.02027575 -0.01617767 -0.00150000 487 | v 0.01617767 0.02027575 -0.00150000 488 | v -0.01617767 0.01507959 -0.00150000 489 | v 0.01767767 0.02067767 -0.00150000 490 | v -0.01507959 0.01617767 -0.00150000 491 | v -0.01467767 0.01767767 -0.00150000 492 | v 0.01617767 -0.02027575 -0.00150000 493 | v 0.01767767 -0.02067767 -0.00150000 494 | v 0.01917767 -0.02027575 -0.00150000 495 | v 0.02027575 -0.01917767 -0.00150000 496 | v 0.02067767 -0.01767767 -0.00150000 497 | v 0.02027575 -0.01617767 -0.00150000 498 | v 0.01767767 0.01467767 -0.00150000 499 | v 0.01917767 0.01507959 -0.00150000 500 | v 0.02027575 0.01617767 -0.00150000 501 | v 0.02067767 0.01767767 -0.00150000 502 | v 0.02027575 0.01917767 -0.00150000 503 | v 0.01917767 0.02027575 -0.00150000 504 | v -0.01507959 0.01917767 -0.00150000 505 | v -0.01617767 -0.01507959 -0.00150000 506 | v -0.01767767 -0.01467767 -0.00150000 507 | v -0.01917767 0.01507959 -0.00150000 508 | v -0.02027575 0.01617767 -0.00150000 509 | v -0.02067767 0.01767767 -0.00150000 510 | v -0.02027575 0.01917767 -0.00150000 511 | v -0.01917767 0.02027575 -0.00150000 512 | v -0.01767767 0.02067767 -0.00150000 513 | v -0.01617767 0.02027575 -0.00150000 514 | f 1 2 3 515 | f 3 2 4 516 | f 4 5 3 517 | f 3 5 6 518 | f 3 6 7 519 | f 7 6 8 520 | f 9 10 11 521 | f 11 10 12 522 | f 11 12 13 523 | f 13 12 14 524 | f 13 14 15 525 | f 15 14 16 526 | f 15 16 17 527 | f 17 16 18 528 | f 17 18 19 529 | f 19 18 20 530 | f 19 20 1 531 | f 1 20 21 532 | f 1 21 2 533 | f 22 23 24 534 | f 24 23 25 535 | f 24 25 26 536 | f 26 25 27 537 | f 26 27 28 538 | f 28 27 29 539 | f 28 29 8 540 | f 8 29 30 541 | f 8 30 7 542 | f 31 32 33 543 | f 33 32 23 544 | f 33 23 22 545 | f 10 9 34 546 | f 34 9 35 547 | f 34 35 36 548 | f 37 38 39 549 | f 39 38 40 550 | f 41 42 43 551 | f 44 45 46 552 | f 46 45 47 553 | f 46 47 48 554 | f 43 42 49 555 | f 44 50 45 556 | f 45 50 51 557 | f 45 51 52 558 | f 42 53 49 559 | f 49 53 54 560 | f 49 54 39 561 | f 39 54 55 562 | f 39 55 37 563 | f 56 57 58 564 | f 58 57 59 565 | f 58 59 43 566 | f 43 59 60 567 | f 43 60 41 568 | f 56 58 61 569 | f 61 58 62 570 | f 61 62 63 571 | f 64 65 66 572 | f 38 67 40 573 | f 40 67 68 574 | f 40 68 47 575 | f 47 68 69 576 | f 47 69 48 577 | f 65 70 66 578 | f 66 70 71 579 | f 66 71 62 580 | f 62 71 72 581 | f 62 72 63 582 | f 36 35 73 583 | f 73 35 64 584 | f 73 64 74 585 | f 74 64 66 586 | f 51 75 52 587 | f 52 75 76 588 | f 52 76 77 589 | f 77 76 78 590 | f 77 78 79 591 | f 80 81 82 592 | f 82 81 83 593 | f 82 83 84 594 | f 84 83 32 595 | f 84 32 31 596 | f 79 85 77 597 | f 77 85 86 598 | f 77 86 80 599 | f 80 86 87 600 | f 80 87 81 601 | f 88 89 90 602 | f 90 89 91 603 | f 90 91 92 604 | f 92 91 93 605 | f 92 93 94 606 | f 93 95 94 607 | f 94 95 96 608 | f 94 96 97 609 | f 97 96 98 610 | f 97 98 99 611 | f 99 98 100 612 | f 99 100 101 613 | f 100 102 101 614 | f 101 102 103 615 | f 101 103 104 616 | f 104 103 105 617 | f 104 105 106 618 | f 106 105 107 619 | f 106 107 108 620 | f 107 109 108 621 | f 108 109 110 622 | f 108 110 111 623 | f 111 110 112 624 | f 111 112 113 625 | f 113 112 114 626 | f 113 114 88 627 | f 88 114 115 628 | f 88 115 89 629 | f 116 117 118 630 | f 118 117 119 631 | f 118 119 120 632 | f 120 119 121 633 | f 120 121 122 634 | f 122 121 123 635 | f 122 123 124 636 | f 125 126 127 637 | f 127 126 128 638 | f 127 128 116 639 | f 116 128 129 640 | f 116 129 117 641 | f 130 131 132 642 | f 132 131 133 643 | f 132 133 134 644 | f 134 133 135 645 | f 134 135 136 646 | f 136 135 137 647 | f 136 137 125 648 | f 125 137 138 649 | f 125 138 126 650 | f 123 139 124 651 | f 124 139 140 652 | f 124 140 141 653 | f 141 140 142 654 | f 141 142 130 655 | f 130 142 143 656 | f 130 143 131 657 | f 144 145 146 658 | f 146 145 147 659 | f 148 1 3 660 | f 149 150 151 661 | f 152 150 153 662 | f 153 150 149 663 | f 153 149 3 664 | f 3 149 154 665 | f 3 154 148 666 | f 155 156 157 667 | f 157 156 158 668 | f 157 158 151 669 | f 151 158 159 670 | f 151 159 149 671 | f 160 161 162 672 | f 162 161 163 673 | f 164 67 38 674 | f 164 38 165 675 | f 166 167 168 676 | f 168 167 169 677 | f 168 169 170 678 | f 170 169 165 679 | f 170 165 171 680 | f 171 165 38 681 | f 101 23 99 682 | f 99 23 32 683 | f 99 32 97 684 | f 88 83 113 685 | f 113 83 25 686 | f 113 25 111 687 | f 101 104 23 688 | f 23 104 106 689 | f 23 106 25 690 | f 25 106 108 691 | f 25 108 111 692 | f 88 90 83 693 | f 83 90 92 694 | f 83 92 32 695 | f 32 92 94 696 | f 32 94 97 697 | f 64 124 11 698 | f 11 124 141 699 | f 11 141 130 700 | f 9 125 35 701 | f 35 125 127 702 | f 35 127 116 703 | f 130 132 11 704 | f 11 132 134 705 | f 11 134 9 706 | f 9 134 136 707 | f 9 136 125 708 | f 116 118 35 709 | f 35 118 120 710 | f 35 120 64 711 | f 64 120 122 712 | f 64 122 124 713 | f 96 95 84 714 | f 84 95 93 715 | f 84 93 82 716 | f 82 93 91 717 | f 82 91 89 718 | f 172 110 109 719 | f 89 115 82 720 | f 82 115 173 721 | f 82 173 24 722 | f 24 173 172 723 | f 109 107 172 724 | f 172 107 22 725 | f 172 22 24 726 | f 102 33 103 727 | f 103 33 22 728 | f 103 22 105 729 | f 105 22 107 730 | f 119 117 73 731 | f 133 131 174 732 | f 175 140 139 733 | f 119 73 121 734 | f 121 73 74 735 | f 121 74 123 736 | f 133 174 135 737 | f 34 126 10 738 | f 10 126 138 739 | f 10 138 137 740 | f 137 135 10 741 | f 10 135 174 742 | f 10 174 12 743 | f 12 174 175 744 | f 12 175 74 745 | f 74 175 139 746 | f 74 139 123 747 | f 39 40 2 748 | f 2 40 4 749 | f 145 144 176 750 | f 176 144 177 751 | f 176 177 178 752 | f 178 177 179 753 | f 178 179 180 754 | f 180 179 181 755 | f 180 181 182 756 | f 182 181 183 757 | f 182 183 184 758 | f 184 183 185 759 | f 184 185 186 760 | f 186 185 187 761 | f 186 187 188 762 | f 188 187 189 763 | f 190 191 192 764 | f 192 191 193 765 | f 192 193 194 766 | f 194 193 195 767 | f 194 195 196 768 | f 196 195 197 769 | f 196 197 198 770 | f 198 197 199 771 | f 198 199 200 772 | f 200 199 201 773 | f 200 201 202 774 | f 202 201 203 775 | f 202 203 163 776 | f 163 203 162 777 | f 148 204 205 778 | f 205 206 148 779 | f 148 206 207 780 | f 148 207 1 781 | f 1 207 191 782 | f 1 191 19 783 | f 19 191 190 784 | f 19 190 208 785 | f 149 209 154 786 | f 154 209 210 787 | f 210 211 154 788 | f 154 211 212 789 | f 154 212 148 790 | f 148 212 213 791 | f 148 213 204 792 | f 208 214 19 793 | f 19 214 215 794 | f 19 215 17 795 | f 215 216 17 796 | f 17 216 217 797 | f 17 217 15 798 | f 217 218 15 799 | f 15 218 219 800 | f 15 219 13 801 | f 64 11 65 802 | f 65 11 13 803 | f 65 13 220 804 | f 220 13 219 805 | f 221 150 152 806 | f 3 7 189 807 | f 83 81 25 808 | f 25 81 27 809 | f 81 222 27 810 | f 27 222 223 811 | f 27 223 29 812 | f 223 224 29 813 | f 29 224 225 814 | f 29 225 30 815 | f 225 226 30 816 | f 30 226 227 817 | f 30 227 7 818 | f 188 189 228 819 | f 228 189 7 820 | f 228 7 229 821 | f 229 7 227 822 | f 3 189 153 823 | f 153 189 230 824 | f 153 230 231 825 | f 231 232 153 826 | f 153 232 233 827 | f 153 233 152 828 | f 152 233 234 829 | f 152 234 235 830 | f 235 236 152 831 | f 152 236 237 832 | f 152 237 221 833 | f 12 74 66 834 | f 12 66 14 835 | f 14 66 62 836 | f 14 62 16 837 | f 16 62 58 838 | f 16 58 18 839 | f 18 58 43 840 | f 18 43 20 841 | f 20 43 49 842 | f 20 49 21 843 | f 21 49 39 844 | f 21 39 2 845 | f 4 40 47 846 | f 4 47 5 847 | f 5 47 45 848 | f 5 45 6 849 | f 6 45 52 850 | f 6 52 8 851 | f 8 52 77 852 | f 8 77 28 853 | f 28 77 80 854 | f 28 80 26 855 | f 26 80 82 856 | f 26 82 24 857 | f 144 146 238 858 | f 238 146 239 859 | f 238 239 240 860 | f 240 239 241 861 | f 241 239 242 862 | f 241 242 243 863 | f 243 242 244 864 | f 244 242 245 865 | f 244 245 246 866 | f 246 245 247 867 | f 247 245 155 868 | f 247 155 157 869 | f 243 244 248 870 | f 230 189 187 871 | f 187 185 249 872 | f 235 234 250 873 | f 250 234 233 874 | f 250 233 232 875 | f 235 251 236 876 | f 236 251 248 877 | f 236 248 237 878 | f 237 248 252 879 | f 237 252 221 880 | f 221 252 150 881 | f 150 252 151 882 | f 151 252 247 883 | f 151 247 157 884 | f 248 244 252 885 | f 252 244 246 886 | f 252 246 247 887 | f 235 250 251 888 | f 251 250 253 889 | f 251 253 248 890 | f 248 253 241 891 | f 248 241 243 892 | f 144 238 254 893 | f 254 238 240 894 | f 230 187 231 895 | f 231 187 249 896 | f 231 249 232 897 | f 232 249 255 898 | f 232 255 250 899 | f 250 255 254 900 | f 250 254 253 901 | f 253 254 240 902 | f 253 240 241 903 | f 185 183 249 904 | f 249 183 181 905 | f 249 181 255 906 | f 255 181 179 907 | f 255 179 254 908 | f 254 179 177 909 | f 254 177 144 910 | f 158 156 256 911 | f 256 156 257 912 | f 256 257 258 913 | f 258 257 259 914 | f 259 257 260 915 | f 259 260 261 916 | f 261 260 262 917 | f 262 260 263 918 | f 262 263 264 919 | f 264 263 265 920 | f 265 263 160 921 | f 265 160 162 922 | f 266 159 158 923 | f 213 267 204 924 | f 149 159 209 925 | f 209 159 266 926 | f 209 266 210 927 | f 213 212 268 928 | f 268 212 211 929 | f 268 211 210 930 | f 193 191 207 931 | f 268 269 213 932 | f 213 269 270 933 | f 213 270 267 934 | f 267 270 197 935 | f 267 197 204 936 | f 204 197 195 937 | f 204 195 205 938 | f 205 195 193 939 | f 205 193 206 940 | f 206 193 207 941 | f 158 256 266 942 | f 266 256 258 943 | f 266 258 271 944 | f 258 259 271 945 | f 271 259 261 946 | f 271 261 272 947 | f 272 261 262 948 | f 272 262 273 949 | f 273 262 264 950 | f 273 264 265 951 | f 203 201 270 952 | f 270 201 199 953 | f 270 199 197 954 | f 210 266 268 955 | f 268 266 271 956 | f 268 271 269 957 | f 269 271 272 958 | f 269 272 270 959 | f 270 272 273 960 | f 270 273 203 961 | f 203 273 265 962 | f 203 265 162 963 | f 163 161 274 964 | f 274 161 275 965 | f 274 275 276 966 | f 276 275 277 967 | f 277 275 278 968 | f 277 278 279 969 | f 279 278 280 970 | f 280 278 281 971 | f 280 281 282 972 | f 282 281 283 973 | f 283 281 166 974 | f 283 166 168 975 | f 200 202 284 976 | f 54 53 285 977 | f 285 53 42 978 | f 286 171 38 979 | f 287 288 280 980 | f 168 170 283 981 | f 283 170 287 982 | f 283 287 282 983 | f 282 287 280 984 | f 284 276 289 985 | f 289 276 277 986 | f 289 277 288 987 | f 288 277 279 988 | f 288 279 280 989 | f 38 37 286 990 | f 286 37 290 991 | f 286 290 291 992 | f 291 290 292 993 | f 291 292 293 994 | f 293 292 294 995 | f 293 294 295 996 | f 295 294 296 997 | f 295 296 198 998 | f 42 297 285 999 | f 285 297 298 1000 | f 285 298 299 1001 | f 299 298 300 1002 | f 299 300 301 1003 | f 301 300 302 1004 | f 202 163 284 1005 | f 284 163 274 1006 | f 284 274 276 1007 | f 198 200 295 1008 | f 295 200 284 1009 | f 295 284 293 1010 | f 293 284 289 1011 | f 293 289 291 1012 | f 291 289 288 1013 | f 291 288 286 1014 | f 286 288 287 1015 | f 286 287 171 1016 | f 171 287 170 1017 | f 192 194 296 1018 | f 296 194 196 1019 | f 296 196 198 1020 | f 37 55 290 1021 | f 290 55 54 1022 | f 290 54 292 1023 | f 292 54 285 1024 | f 292 285 294 1025 | f 294 285 299 1026 | f 294 299 296 1027 | f 296 299 301 1028 | f 296 301 192 1029 | f 192 301 302 1030 | f 192 302 190 1031 | f 214 208 303 1032 | f 59 57 304 1033 | f 61 305 56 1034 | f 72 306 63 1035 | f 70 65 220 1036 | f 63 306 61 1037 | f 61 306 307 1038 | f 61 307 305 1039 | f 42 41 297 1040 | f 297 41 298 1041 | f 70 220 71 1042 | f 71 220 219 1043 | f 71 219 72 1044 | f 72 219 218 1045 | f 72 218 306 1046 | f 306 218 217 1047 | f 306 217 307 1048 | f 214 303 215 1049 | f 217 216 307 1050 | f 307 216 308 1051 | f 307 308 305 1052 | f 305 308 304 1053 | f 305 304 56 1054 | f 56 304 57 1055 | f 216 215 308 1056 | f 308 215 303 1057 | f 308 303 304 1058 | f 304 303 309 1059 | f 304 309 59 1060 | f 59 309 310 1061 | f 59 310 60 1062 | f 60 310 41 1063 | f 41 310 298 1064 | f 298 310 309 1065 | f 298 309 300 1066 | f 300 309 303 1067 | f 300 303 302 1068 | f 302 303 208 1069 | f 302 208 190 1070 | f 169 167 311 1071 | f 311 167 312 1072 | f 311 312 313 1073 | f 313 312 314 1074 | f 314 312 315 1075 | f 314 315 316 1076 | f 316 315 317 1077 | f 317 315 318 1078 | f 317 318 319 1079 | f 319 318 320 1080 | f 320 318 147 1081 | f 320 147 145 1082 | f 184 186 321 1083 | f 322 165 169 1084 | f 67 164 68 1085 | f 68 164 323 1086 | f 324 48 325 1087 | f 325 48 69 1088 | f 169 311 322 1089 | f 322 311 313 1090 | f 322 313 326 1091 | f 69 68 325 1092 | f 325 68 323 1093 | f 325 323 324 1094 | f 324 323 327 1095 | f 324 327 328 1096 | f 328 327 329 1097 | f 328 329 330 1098 | f 330 329 331 1099 | f 330 331 182 1100 | f 313 314 326 1101 | f 326 314 316 1102 | f 326 316 332 1103 | f 332 316 317 1104 | f 332 317 333 1105 | f 333 317 319 1106 | f 333 319 320 1107 | f 186 188 321 1108 | f 321 188 334 1109 | f 321 334 335 1110 | f 335 334 336 1111 | f 335 336 337 1112 | f 337 336 338 1113 | f 337 338 339 1114 | f 182 184 330 1115 | f 330 184 321 1116 | f 330 321 328 1117 | f 328 321 335 1118 | f 328 335 324 1119 | f 324 335 337 1120 | f 324 337 48 1121 | f 48 337 339 1122 | f 48 339 46 1123 | f 176 178 331 1124 | f 331 178 180 1125 | f 331 180 182 1126 | f 164 165 323 1127 | f 323 165 322 1128 | f 323 322 327 1129 | f 327 322 326 1130 | f 327 326 329 1131 | f 329 326 332 1132 | f 329 332 331 1133 | f 331 332 333 1134 | f 331 333 176 1135 | f 176 333 320 1136 | f 176 320 145 1137 | f 340 341 342 1138 | f 343 225 224 1139 | f 225 343 226 1140 | f 76 344 78 1141 | f 75 51 345 1142 | f 46 339 44 1143 | f 44 339 346 1144 | f 44 346 50 1145 | f 222 81 87 1146 | f 222 87 223 1147 | f 227 342 229 1148 | f 229 342 341 1149 | f 229 341 228 1150 | f 87 86 223 1151 | f 223 86 85 1152 | f 223 85 224 1153 | f 224 85 347 1154 | f 224 347 343 1155 | f 50 346 51 1156 | f 51 346 340 1157 | f 51 340 345 1158 | f 345 340 342 1159 | f 345 342 343 1160 | f 343 342 227 1161 | f 343 227 226 1162 | f 85 79 347 1163 | f 347 79 78 1164 | f 347 78 343 1165 | f 343 78 344 1166 | f 343 344 345 1167 | f 345 344 76 1168 | f 345 76 75 1169 | f 339 338 346 1170 | f 346 338 336 1171 | f 346 336 340 1172 | f 340 336 334 1173 | f 340 334 341 1174 | f 341 334 188 1175 | f 341 188 228 1176 | f 110 102 112 1177 | f 112 102 100 1178 | f 112 100 114 1179 | f 114 100 98 1180 | f 114 98 115 1181 | f 115 98 96 1182 | f 126 131 143 1183 | f 126 143 128 1184 | f 128 143 142 1185 | f 128 142 129 1186 | f 129 142 140 1187 | f 129 140 117 1188 | f 348 349 34 1189 | f 34 349 33 1190 | f 34 33 126 1191 | f 126 33 102 1192 | f 126 102 131 1193 | f 131 102 110 1194 | f 131 110 174 1195 | f 174 110 172 1196 | f 350 351 348 1197 | f 348 351 349 1198 | f 351 350 84 1199 | f 84 350 73 1200 | f 84 73 96 1201 | f 96 73 117 1202 | f 96 117 115 1203 | f 115 117 140 1204 | f 115 140 173 1205 | f 173 140 175 1206 | f 174 172 175 1207 | f 175 172 173 1208 | f 33 349 31 1209 | f 31 349 351 1210 | f 31 351 84 1211 | f 348 34 350 1212 | f 350 34 36 1213 | f 350 36 73 1214 | f 352 353 354 1215 | f 355 356 315 1216 | f 146 147 356 1217 | f 356 147 318 1218 | f 356 318 315 1219 | f 357 358 359 1220 | f 360 361 362 1221 | f 362 361 363 1222 | f 362 363 364 1223 | f 315 312 355 1224 | f 355 312 167 1225 | f 355 167 166 1226 | f 365 366 239 1227 | f 239 366 242 1228 | f 364 363 367 1229 | f 367 363 368 1230 | f 367 368 369 1231 | f 369 368 370 1232 | f 369 370 371 1233 | f 371 370 372 1234 | f 371 372 357 1235 | f 357 372 373 1236 | f 357 373 358 1237 | f 354 260 257 1238 | f 374 281 375 1239 | f 375 281 278 1240 | f 375 278 275 1241 | f 161 160 354 1242 | f 354 160 263 1243 | f 354 263 260 1244 | f 359 365 357 1245 | f 357 365 239 1246 | f 357 239 376 1247 | f 376 239 146 1248 | f 376 146 377 1249 | f 377 146 356 1250 | f 166 281 355 1251 | f 355 281 374 1252 | f 355 374 378 1253 | f 378 374 379 1254 | f 378 379 360 1255 | f 360 379 380 1256 | f 360 380 361 1257 | f 257 156 354 1258 | f 354 156 155 1259 | f 354 155 366 1260 | f 366 155 245 1261 | f 366 245 242 1262 | f 381 382 354 1263 | f 354 382 383 1264 | f 354 383 384 1265 | f 275 161 375 1266 | f 375 161 354 1267 | f 375 354 385 1268 | f 385 354 384 1269 | f 381 386 387 1270 | f 387 386 388 1271 | f 387 388 389 1272 | f 389 388 390 1273 | f 389 390 391 1274 | f 391 390 392 1275 | f 391 392 393 1276 | f 393 392 394 1277 | f 393 394 395 1278 | f 395 394 396 1279 | f 395 396 397 1280 | f 397 396 375 1281 | f 397 375 398 1282 | f 398 375 385 1283 | f 399 400 354 1284 | f 354 400 401 1285 | f 401 402 354 1286 | f 354 402 403 1287 | f 354 403 381 1288 | f 381 403 404 1289 | f 381 404 386 1290 | f 402 405 403 1291 | f 403 405 406 1292 | f 403 406 407 1293 | f 407 406 408 1294 | f 407 408 409 1295 | f 409 408 410 1296 | f 409 410 411 1297 | f 411 410 412 1298 | f 411 412 413 1299 | f 413 412 414 1300 | f 413 414 415 1301 | f 415 414 416 1302 | f 415 416 417 1303 | f 417 399 415 1304 | f 415 399 354 1305 | f 415 354 418 1306 | f 418 354 353 1307 | f 418 353 419 1308 | f 420 421 366 1309 | f 366 421 422 1310 | f 366 422 354 1311 | f 354 422 423 1312 | f 354 423 352 1313 | f 420 366 424 1314 | f 424 366 425 1315 | f 424 425 426 1316 | f 426 425 427 1317 | f 426 427 428 1318 | f 428 427 429 1319 | f 428 429 430 1320 | f 430 429 431 1321 | f 430 431 432 1322 | f 432 431 433 1323 | f 432 433 419 1324 | f 419 433 434 1325 | f 419 434 418 1326 | f 435 409 436 1327 | f 436 409 411 1328 | f 436 411 437 1329 | f 437 411 413 1330 | f 437 413 438 1331 | f 438 413 415 1332 | f 438 415 439 1333 | f 415 418 439 1334 | f 439 418 434 1335 | f 439 434 440 1336 | f 440 434 433 1337 | f 440 433 441 1338 | f 441 433 431 1339 | f 441 431 442 1340 | f 442 431 429 1341 | f 442 429 443 1342 | f 443 429 427 1343 | f 443 427 444 1344 | f 444 427 425 1345 | f 444 425 445 1346 | f 445 425 366 1347 | f 445 366 446 1348 | f 366 365 446 1349 | f 446 365 359 1350 | f 446 359 447 1351 | f 447 359 448 1352 | f 448 359 358 1353 | f 448 358 449 1354 | f 358 373 449 1355 | f 449 373 372 1356 | f 449 372 450 1357 | f 450 372 370 1358 | f 450 370 451 1359 | f 451 370 368 1360 | f 451 368 452 1361 | f 452 368 363 1362 | f 452 363 453 1363 | f 453 363 361 1364 | f 453 361 454 1365 | f 454 361 380 1366 | f 454 380 455 1367 | f 455 380 379 1368 | f 455 379 456 1369 | f 456 379 374 1370 | f 456 374 457 1371 | f 457 374 375 1372 | f 457 375 458 1373 | f 375 396 458 1374 | f 458 396 394 1375 | f 458 394 459 1376 | f 459 394 392 1377 | f 459 392 460 1378 | f 460 392 390 1379 | f 460 390 461 1380 | f 461 390 388 1381 | f 461 388 462 1382 | f 462 388 386 1383 | f 462 386 463 1384 | f 463 386 404 1385 | f 463 404 464 1386 | f 464 404 403 1387 | f 464 403 435 1388 | f 435 403 407 1389 | f 435 407 409 1390 | f 442 443 465 1391 | f 465 443 466 1392 | f 467 468 469 1393 | f 440 441 470 1394 | f 470 441 442 1395 | f 470 442 471 1396 | f 471 442 465 1397 | f 471 465 472 1398 | f 473 474 475 1399 | f 472 476 471 1400 | f 471 476 477 1401 | f 471 477 468 1402 | f 468 477 478 1403 | f 468 478 469 1404 | f 450 479 449 1405 | f 449 479 448 1406 | f 475 474 480 1407 | f 480 474 467 1408 | f 480 467 481 1409 | f 481 467 469 1410 | f 481 469 482 1411 | f 466 443 483 1412 | f 483 443 444 1413 | f 483 444 484 1414 | f 484 444 445 1415 | f 484 445 485 1416 | f 485 445 446 1417 | f 485 446 479 1418 | f 479 446 447 1419 | f 479 447 448 1420 | f 482 469 486 1421 | f 486 469 487 1422 | f 486 487 488 1423 | f 488 487 489 1424 | f 488 489 490 1425 | f 440 470 439 1426 | f 439 470 491 1427 | f 439 491 438 1428 | f 438 491 492 1429 | f 438 492 437 1430 | f 437 492 493 1431 | f 437 493 436 1432 | f 436 493 494 1433 | f 436 494 435 1434 | f 435 494 495 1435 | f 435 495 464 1436 | f 464 495 496 1437 | f 464 496 463 1438 | f 463 496 473 1439 | f 463 473 462 1440 | f 462 473 475 1441 | f 462 475 497 1442 | f 497 498 462 1443 | f 462 498 499 1444 | f 462 499 461 1445 | f 461 499 500 1446 | f 461 500 460 1447 | f 460 500 501 1448 | f 460 501 459 1449 | f 459 501 502 1450 | f 459 502 458 1451 | f 458 502 488 1452 | f 458 488 457 1453 | f 457 488 490 1454 | f 457 490 503 1455 | f 478 504 469 1456 | f 469 504 505 1457 | f 469 505 506 1458 | f 506 505 479 1459 | f 506 479 507 1460 | f 507 479 450 1461 | f 507 450 508 1462 | f 508 450 451 1463 | f 508 451 509 1464 | f 509 451 452 1465 | f 509 452 510 1466 | f 510 452 453 1467 | f 510 453 511 1468 | f 511 453 454 1469 | f 511 454 512 1470 | f 512 454 455 1471 | f 512 455 503 1472 | f 503 455 456 1473 | f 503 456 457 1474 | f 385 481 398 1475 | f 398 481 482 1476 | f 398 482 397 1477 | f 397 482 486 1478 | f 397 486 395 1479 | f 395 486 488 1480 | f 395 488 393 1481 | f 393 488 502 1482 | f 393 502 391 1483 | f 391 502 501 1484 | f 391 501 389 1485 | f 389 501 500 1486 | f 389 500 387 1487 | f 387 500 499 1488 | f 387 499 381 1489 | f 381 499 498 1490 | f 381 498 382 1491 | f 382 498 497 1492 | f 382 497 383 1493 | f 383 497 475 1494 | f 383 475 384 1495 | f 384 475 480 1496 | f 384 480 385 1497 | f 385 480 481 1498 | f 369 508 367 1499 | f 367 508 509 1500 | f 367 509 364 1501 | f 364 509 510 1502 | f 364 510 362 1503 | f 362 510 511 1504 | f 362 511 360 1505 | f 360 511 512 1506 | f 360 512 378 1507 | f 378 512 503 1508 | f 378 503 355 1509 | f 355 503 490 1510 | f 355 490 356 1511 | f 356 490 489 1512 | f 356 489 377 1513 | f 377 489 487 1514 | f 377 487 376 1515 | f 376 487 469 1516 | f 376 469 357 1517 | f 357 469 506 1518 | f 357 506 371 1519 | f 371 506 507 1520 | f 371 507 369 1521 | f 369 507 508 1522 | f 424 484 420 1523 | f 420 484 485 1524 | f 420 485 421 1525 | f 421 485 479 1526 | f 421 479 422 1527 | f 422 479 505 1528 | f 422 505 423 1529 | f 423 505 504 1530 | f 423 504 352 1531 | f 352 504 478 1532 | f 352 478 353 1533 | f 353 478 477 1534 | f 353 477 419 1535 | f 419 477 476 1536 | f 419 476 432 1537 | f 432 476 472 1538 | f 432 472 430 1539 | f 430 472 465 1540 | f 430 465 428 1541 | f 428 465 466 1542 | f 428 466 426 1543 | f 426 466 483 1544 | f 426 483 424 1545 | f 424 483 484 1546 | f 399 471 400 1547 | f 400 471 468 1548 | f 400 468 401 1549 | f 401 468 467 1550 | f 401 467 402 1551 | f 402 467 474 1552 | f 402 474 405 1553 | f 405 474 473 1554 | f 405 473 406 1555 | f 406 473 496 1556 | f 406 496 408 1557 | f 408 496 495 1558 | f 408 495 410 1559 | f 410 495 494 1560 | f 410 494 412 1561 | f 412 494 493 1562 | f 412 493 414 1563 | f 414 493 492 1564 | f 414 492 416 1565 | f 416 492 491 1566 | f 416 491 417 1567 | f 417 491 470 1568 | f 417 470 399 1569 | f 399 470 471 1570 | --------------------------------------------------------------------------------