├── .gitignore ├── LICENSE ├── README.md ├── configs ├── configs.json └── default.yaml ├── models ├── flexiv │ ├── flexivBot.xml │ ├── flexivCfg.xml │ ├── flexiv_rizon4.urdf │ ├── graphics │ │ ├── link0_body.mtl │ │ ├── link0_body.obj │ │ ├── link0_body.stl │ │ ├── link0_body_coll.mtl │ │ ├── link0_body_coll.obj │ │ ├── link0_body_pvt4.mtl │ │ ├── link0_body_pvt4.obj │ │ ├── link1_assemble.mtl │ │ ├── link1_assemble.obj │ │ ├── link1_assemble.stl │ │ ├── link1_assemble_coll.mtl │ │ ├── link1_assemble_coll.obj │ │ ├── link2_assemble.mtl │ │ ├── link2_assemble.obj │ │ ├── link2_assemble.stl │ │ ├── link2_assemble_coll.mtl │ │ ├── link2_assemble_coll.obj │ │ ├── link3_assemble.mtl │ │ ├── link3_assemble.obj │ │ ├── link3_assemble.stl │ │ ├── link3_assemble_coll.mtl │ │ ├── link3_assemble_coll.obj │ │ ├── link4_assemble.mtl │ │ ├── link4_assemble.obj │ │ ├── link4_assemble.stl │ │ ├── link4_assemble_coll.mtl │ │ ├── link4_assemble_coll.obj │ │ ├── link5_assemble.mtl │ │ ├── link5_assemble.obj │ │ ├── link5_assemble.stl │ │ ├── link5_assemble_coll.mtl │ │ ├── link5_assemble_coll.obj │ │ ├── link6_assemble.mtl │ │ ├── link6_assemble.obj │ │ ├── link6_assemble.stl │ │ ├── link6_assemble_coll.mtl │ │ ├── link6_assemble_coll.obj │ │ ├── link7_assemble.mtl │ │ ├── link7_assemble.obj │ │ ├── link7_assemble.stl │ │ ├── link7_assemble_coll.mtl │ │ └── link7_assemble_coll.obj │ ├── robot_pvt3.urdf │ └── robot_pvt4.urdf ├── franka │ ├── LICENSE.md │ ├── franka modified from panda.txt │ ├── franka.urdf │ ├── meshes │ │ ├── collision │ │ │ ├── finger.obj │ │ │ ├── hand.obj │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.obj │ │ │ └── link7.obj │ │ └── visual │ │ │ ├── finger.dae │ │ │ ├── hand.dae │ │ │ ├── link0.dae │ │ │ ├── link1.dae │ │ │ ├── link2.dae │ │ │ ├── link3.dae │ │ │ ├── link4.dae │ │ │ ├── link5.dae │ │ │ ├── link6.dae │ │ │ └── link7.dae │ └── panda.urdf ├── kuka │ ├── meshes │ │ ├── link_0.obj │ │ ├── link_1.obj │ │ ├── link_2.obj │ │ ├── link_3.obj │ │ ├── link_4.obj │ │ ├── link_5.obj │ │ ├── link_6.obj │ │ └── link_7.obj │ ├── model.urdf │ └── source.txt └── ur5 │ ├── LICENSE │ ├── meshes │ └── ur5 │ │ ├── collision │ │ ├── base.stl │ │ ├── forearm.stl │ │ ├── shoulder.stl │ │ ├── upperarm.stl │ │ ├── wrist1.stl │ │ ├── wrist2.stl │ │ └── wrist3.stl │ │ └── visual │ │ ├── base.obj │ │ ├── forearm.obj │ │ ├── shoulder.obj │ │ ├── upperarm.obj │ │ ├── wrist1.obj │ │ ├── wrist2.obj │ │ └── wrist3.obj │ ├── source.txt │ └── urdf │ ├── LICENSE │ ├── ur5 modified from ur5_robotiq_85.txt │ ├── ur5.urdf │ └── ur5_robotiq_85.urdf ├── requirements.txt ├── requirements_api.txt ├── rh20t_api ├── __init__.py ├── configurations.py ├── convert.py ├── extract.py ├── online.py ├── scene.py ├── search.py ├── transforms.py └── utils.py ├── scripts └── test_joint_angle_getter.py ├── utils ├── __init__.py ├── keyboard_listener.py ├── logger.py ├── point_cloud.py ├── robot.py └── stopwatch.py └── visualize.py /.gitignore: -------------------------------------------------------------------------------- 1 | *cache/ 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | pip-wheel-metadata/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | .python-version 88 | 89 | # pipenv 90 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 91 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 92 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 93 | # install all needed dependencies. 94 | #Pipfile.lock 95 | 96 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 97 | __pypackages__/ 98 | 99 | # Celery stuff 100 | celerybeat-schedule 101 | celerybeat.pid 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | 133 | # Data folder 134 | data/ 135 | action_check_gripper_res/ 136 | 137 | # VS Code 138 | .vscode/ 139 | .VSCodeCounter/ 140 | 141 | # Pytest cache 142 | .pytest_cache/ 143 | 144 | 145 | # results 146 | res/ 147 | 148 | # logs 149 | log/ -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Fang Hao-Shu and Fang Hongjie and Tang Zhenyu and Liu Jirong and Wang Junbo and Zhu Haoyi and Lu Cewu 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RH20T Dataset API and Visualizer Implementation 2 | 3 | ## Dataset API 4 | 5 | ### Getting Started 6 | 7 | The RH20T Python API module is implemented in `rh20t_api` folder. To install the dependencies, run the following command: 8 | 9 | ```bash 10 | pip install -r ./requirements_api.txt 11 | ``` 12 | 13 | The information for each robot configuration are in `configs/configs.json` file, which is usually required for using the API. 14 | 15 | ### Basic Usage 16 | 17 | Here presents basic usage of the dataset API, including a data extraction script, a scene data loader for loading preprocessed scene data from the dataset, as well as an online preprocessor for real robotic manipulation inference. You can also refer to our visualizer implementation for better understanding how to use the scene data loader. 18 | 19 | #### Data Extraction Script 20 | 21 | The RGB-D data is stored in mp4 format, we need to change the data into the original image format. In `rh20t_api/extract.py` we provide APIs and a multiprocessing script to convert the dataset into the original image version. Notice that the following scene data loader is based on the original image version of RH20T. 22 | 23 | #### Scene Data Loader 24 | 25 | The scene data loader should be initialized with a specific scene data folder and robot configurations. 26 | 27 | ```python 28 | from rh20t_api.configurations import load_conf 29 | from rh20t_api.scene import RH20TScene 30 | 31 | robot_configs = load_conf("configs/configs.json") 32 | scene = RH20TScene(scene_path, robot_configs) 33 | ``` 34 | 35 | Some of the methods/properties in the scene data loader that may be of use are listed in the following: 36 | 37 | |Method/Property|Comment| 38 | |---|---| 39 | |`RH20TScene.extrinsics_base_aligned`|The preprocessed extrinsics 4x4 matrices for each camera related to robot arm base| 40 | |`RH20TScene.folder`|The current scene folder (can be modified)| 41 | |`RH20TScene.is_high_freq`|Toggles reading high frequency data, default to False| 42 | |`RH20TScene.intrinsics`|Dict[str, np.ndarray] type of camera serial : calibrated 3x4 intrinsic matrices| 43 | |`RH20TScene.in_hand_serials`|The list of in-hand camera serials| 44 | |`RH20TScene.serials`|The list of all camera serials| 45 | |`RH20TScene.low_freq_timestamps`|The list of sorted low-frequency timestamps for each camera serial| 46 | |`RH20TScene.high_freq_timestamps`|The list of sorted high-frequency timestamps (different cameras share the same high-frequency timestamp list)| 47 | |`RH20TScene.start_timestamp`|The starting timestamp for the current scene| 48 | |`RH20TScene.end_timestamp`|The ending timestamp for the current scene| 49 | |`RH20TScene.get_audio_path()`|The audio path| 50 | |`RH20TScene.get_image_path_pairs(timestamp:int, image_types:List[str]=["color", "depth"])`|Query interpolated `Dict[str, List[str]]` type of color-depth image pairs paths for each camera given a timestamp| 51 | |`RH20TScene.get_image_path_pairs_period(time_interval:int, start_timestamp:int=None, end_timestamp:int=None)`|Query a list of interpolated `Dict[str, List[str]]` type of color-depth image pairs paths for each camera given a period of time in milliseconds (starting and ending timestamps will be set to the scene's if not specified)| 52 | |`RH20TScene.get_ft_aligned(timestamp:int, serial:str="base", zeroed:bool=True)`|Query interpolated preprocessed force-torque concatenated 6d vector given a timestamp and a camera serial (or "base" which reads data from all serials)| 53 | |`RH20TScene.get_tcp_aligned(timestamp:int, serial:str="base")`|Query interpolated preprocessed tcp 7d quaternion pose vector given a timestamp and a camera serial (or "base" which reads data from all serials)| 54 | |`RH20TScene.get_joint_angles_aligned(timestamp:int, serial:str="base")`|Query interpolated joint angles sequence given a timestamp and a camera serial (or "base" which reads data from all serials)| 55 | |`RH20TScene.get_gripper_command(timestamp:int)`|Query interpolated gripper command given a timestamp| 56 | |`RH20TScene.get_gripper_info(timestamp:int)`|Query interpolated gripper info given a timestamp| 57 | 58 | #### Online Preprocessor 59 | 60 | The force and torque concatenated vectors, as well as the tcp values are collected in robot arm base coordinate. The implemented online preprocessor can be used to project these values to a certain camera coordinate online for inferencing purpose. It should be initialized with calibration result path, and the specific camera to project to. 61 | 62 | ```python 63 | from rh20t_api.configurations import load_conf 64 | from rh20t_api.scene import RH20TScene 65 | from rh20t_api.online import RH20TOnline 66 | 67 | serial = "[The serial number of the camera to project to]" 68 | 69 | robot_configs = load_conf("configs/configs.json") 70 | scene = RH20TScene(scene_path, robot_configs) 71 | 72 | # before using the processor, it is recommended to collect the first several frames of data 73 | # when the robot arm is still, and update the sensor offsets considering the temperature drift 74 | sampled_raw_fts = # shaped (n, 6), raw force-torque vectors in the first several frames 75 | sampled_raw_tcps = # shaped (n, 6) or (n, 7), raw tcp values in the first several frames 76 | scene.configuration.update_offset(sampled_raw_fts, sampled_raw_tcps) 77 | 78 | # initialize the preprocessor 79 | processor = RH20TOnline(scene.calib_folder, scene.configuration, serial) 80 | 81 | # ... 82 | 83 | # online preprocessing ft_raw and tcp_raw, the processed values are 84 | # aligned, processed with sensor offsets (for force and torque) and 85 | # projected to the specified camera 86 | processed_ft = processor.project_raw_from_external_sensor(ft_raw, tcp_raw) 87 | processed_tcp, processed_ft_tcp = processor.project_raw_from_robot_sensor(tcp_raw) 88 | ``` 89 | 90 | ## Dataset Visualizer 91 | 92 | ### Getting Started 93 | 94 | The visualizer can be configured in `configs/default.yaml`. The dependencies are installed via: 95 | 96 | ```bash 97 | # Recommend running on Ubuntu 98 | pip install -r ./requirements.txt 99 | ``` 100 | 101 | The minimal files requirement for visualizing a scene will be a scene folder placed together with a calibration folder like: 102 | 103 | ```text 104 | - calib 105 | - [some timestamp] 106 | - ... 107 | - [scene folder] 108 | ``` 109 | 110 | Before visualizing a scene, we should first preprocess the images to point clouds and cache them to save time, for example: 111 | 112 | ```bash 113 | python visualize.py --scene_folder [SCENE_FOLDER] --cache_folder [CACHE_FOLDER] --preprocess 114 | ``` 115 | 116 | **NOTE: The RGB and depth images were originally 1280x720 and are now resized to 640x360. To get the visualization run normally, we either need to resize the images back to 1280x720 or modify the camera intrinsics. In this implementation, we explicitly scale the intrinsics by 0.5 at [here](https://github.com/rh20t/rh20t_api/blob/main/utils/point_cloud.py#L93-L94). If you are projecting the depth to point cloud in your own project, don't forget this step!** 117 | 118 | You can modify configurations including sampling time interval, screenshot saving path, and choices to enable visualizing etc in `configs/default.yaml`. If you would only like to view the first several frames of point clouds, you can run the above command for a while and then stop it with `Ctrl + C`. It is recommended to cache at least the first frame point cloud since it is used for adjusting the initial viewing direction. 119 | 120 | Running the following command will visualize the dynamic scene, with an Open3D visualizer viewport showing the 3D dynamic scene, and an OpenCV viewport showing the real-world video captured by your chosen camera (determined by `chosen_cam_idx` in `configs/default.yaml`). Note that the scene folder and cache folder should match the previous ones. 121 | 122 | ```bash 123 | python visualize.py --scene_folder [SCENE_FOLDER] --cache_folder [CACHE_FOLDER] 124 | ``` 125 | 126 | During the visualization, you can: 127 | 128 | 1. drag and view the scene with your mouse; 129 | 2. press `Alt` to pause, then press `←` and `→` to view previous and next frames respectively, drag and view them, press `C` to save a screenshot of the current frame, and press `Alt` again for continue playing; 130 | 3. press `Esc` to stop. 131 | -------------------------------------------------------------------------------- /configs/configs.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "conf_num": 1, 4 | "robot": "flexiv", 5 | "robot_urdf": "./models/flexiv/robot_pvt3.urdf", 6 | "robot_mesh": "./models/flexiv/", 7 | "calib_tcp_length": 7, 8 | "robot_tcp_field": [0, 7], 9 | "robot_ft_field": [7, 13], 10 | "robot_joint_field": [0, 7], 11 | "robot_joint_sequence": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], 12 | "sensor": "dahuan", 13 | "sensor_ft_field": [0, 6], 14 | "offset": [3.293999, 0.7040006, -8.3530004, -0.55881995, 0.14261997, 0.11335], 15 | "centroid": [0.0, 0.0, 0.084], 16 | "gravity": [0.0, 0.0, -12.65486, 0.0], 17 | "ts_rot": [[0,-1,0,0],[1,0,0,0],[0,0,1,0],[0,0,0,1]], 18 | "in_hand": ["043322070878"], 19 | "tc_mat": [[0,-1,0,0],[1,0,0,0.077],[0,0,1,0.2665],[0,0,0,1]], 20 | "align_mat_tcp": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 21 | "align_mat_base": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 22 | "tactile": false, 23 | "gripper": "Dahuan AG-95" 24 | }, 25 | { 26 | "conf_num": 2, 27 | "robot": "flexiv", 28 | "robot_urdf": "./models/flexiv/robot_pvt3.urdf", 29 | "robot_mesh": "./models/flexiv/", 30 | "calib_tcp_length": 7, 31 | "robot_tcp_field": [0, 7], 32 | "robot_ft_field": [7, 13], 33 | "robot_joint_field": [0, 7], 34 | "robot_joint_sequence": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], 35 | "sensor": "dahuan", 36 | "sensor_ft_field": [0, 6], 37 | "offset": [-0.10485816, 10.3766778, 9.84273631, -0.00359222313, 0.072519455, 0.193357165], 38 | "centroid": [0.0, 0.0, 0.084], 39 | "gravity": [0.0, 0.0, -11.65486, 0.0], 40 | "ts_rot": [[0,1,0,0],[-1,0,0,0],[0,0,1,0],[0,0,0,1]], 41 | "in_hand": ["104422070042"], 42 | "tc_mat": [[0,-1,0,0],[1,0,0,0.077],[0,0,1,0.2915],[0,0,0,1]], 43 | "align_mat_tcp": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 44 | "align_mat_base": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 45 | "tactile": false, 46 | "gripper": "Dahuan AG-95" 47 | }, 48 | { 49 | "conf_num": 3, 50 | "robot": "ur5", 51 | "robot_urdf": "./models/ur5/urdf/ur5.urdf", 52 | "robot_mesh": "./models/ur5/", 53 | "calib_tcp_length": 7, 54 | "robot_tcp_field": [0, 6], 55 | "robot_ft_field": [6, 12], 56 | "robot_joint_field": [0, 6], 57 | "robot_joint_sequence": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"], 58 | "sensor": "ati", 59 | "sensor_ft_field": [0, 6], 60 | "offset": [-34.6980467, 3.94564945, 12.6925057, -0.0647263426, 0.199003279, -0.405158842], 61 | "centroid": [0.0, 0.0, 0.05], 62 | "gravity": [0.0, 0.0, -15.3060632, 0.0], 63 | "ts_rot": [[-1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]], 64 | "in_hand": ["045322071843"], 65 | "tc_mat": [[1,0,0,0.027],[0,1,0,0.0775],[0,0,1,0.240],[0,0,0,1]], 66 | "align_mat_tcp": [[0,-1,0,0],[1,0,0,0],[0,0,1,0],[0,0,0,1]], 67 | "align_mat_base": [[-1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]], 68 | "tactile": false, 69 | "gripper": "WSG-50" 70 | }, 71 | { 72 | "conf_num": 4, 73 | "robot": "ur5", 74 | "robot_urdf": "./models/ur5/urdf/ur5.urdf", 75 | "robot_mesh": "./models/ur5/", 76 | "calib_tcp_length": 7, 77 | "robot_tcp_field": [0, 6], 78 | "robot_ft_field": [6, 12], 79 | "robot_joint_field": [0, 6], 80 | "robot_joint_sequence": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"], 81 | "sensor": "ati", 82 | "sensor_ft_field": [0, 6], 83 | "offset": [-46.4660726, 15.9144707, 1.5917947, -0.0372446672, 0.14495894, -0.523965284], 84 | "centroid": [0.0, 0.0, 0.065], 85 | "gravity": [0.0, 0.0, -10.4090632, 0.0], 86 | "ts_rot": [[-1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]], 87 | "in_hand": ["045322071843"], 88 | "tc_mat": [[1,0,0,0.027],[0,1,0,0.0775],[0,0,1,0.210],[0,0,0,1]], 89 | "align_mat_tcp": [[0,-1,0,0],[1,0,0,0],[0,0,1,0],[0,0,0,1]], 90 | "align_mat_base": [[-1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]], 91 | "tactile": false, 92 | "gripper": "Robotiq 2F-85" 93 | }, 94 | { 95 | "conf_num": 5, 96 | "robot": "franka", 97 | "robot_urdf": "./models/franka/franka.urdf", 98 | "robot_mesh": "./models/franka/", 99 | "calib_tcp_length": 6, 100 | "robot_tcp_field": [0, 6], 101 | "robot_ft_field": [6, 12], 102 | "robot_joint_field": [0, 7], 103 | "robot_joint_sequence": ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"], 104 | "sensor": "none", 105 | "sensor_ft_field": [], 106 | "offset": [0.45048516, -0.2524121, 6.83712617, -0.03164226, -2.0295747, -0.16243551], 107 | "centroid": [0.0, 0.0, 0.75328287], 108 | "gravity": [0.0, 0.0, 2.64042946, 0.0], 109 | "ts_rot": [[-1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]], 110 | "in_hand": ["104422070042", "135122079702"], 111 | "tc_mat": [[0,1,0,0.033], [-1,0,0,-0.05], [0,0,1,0.09], [0,0,0,1]], 112 | "align_mat_tcp": [[-1,0,0,0],[0,-1,0,0],[0,0,1,0],[0,0,0,1]], 113 | "align_mat_base": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 114 | "tactile": false, 115 | "gripper": "franka" 116 | }, 117 | { 118 | "conf_num": 6, 119 | "robot": "kuka", 120 | "robot_urdf": "./models/kuka/model.urdf", 121 | "robot_mesh": "./models/kuka/", 122 | "calib_tcp_length": 6, 123 | "robot_tcp_field": [0, 6], 124 | "robot_ft_field": [6, 12], 125 | "robot_joint_field": [0, 7], 126 | "robot_joint_sequence": ["lbr_iiwa_joint_1", "lbr_iiwa_joint_2", "lbr_iiwa_joint_3", "lbr_iiwa_joint_4", "lbr_iiwa_joint_5", "lbr_iiwa_joint_6", "lbr_iiwa_joint_7"], 127 | "sensor": "ati", 128 | "sensor_ft_field": [0, 6], 129 | "offset": [-56.233, 1.944, -4.433, 0.0, 0.202, -0.52], 130 | "centroid": [0.0, 0.0, 0.065], 131 | "gravity": [0.0, 0.0, -11.0652612, 0.0], 132 | "ts_rot": [[0,1,0,0],[-1,0,0,0],[0,0,1,0],[0,0,0,1]], 133 | "in_hand": ["135122075425", "135122070361"], 134 | "tc_mat": [[0,-1,0,0.035], [1,0,0,0.055], [0,0,1,0.20], [0,0,0,1]], 135 | "align_mat_tcp": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 136 | "align_mat_base": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 137 | "tactile": false, 138 | "gripper":"Robotiq 2F-85" 139 | }, 140 | { 141 | "conf_num": 7, 142 | "robot": "kuka", 143 | "robot_urdf": "./models/kuka/model.urdf", 144 | "robot_mesh": "./models/kuka/", 145 | "calib_tcp_length": 6, 146 | "robot_tcp_field": [0, 6], 147 | "robot_ft_field": [6, 12], 148 | "robot_joint_field": [0, 7], 149 | "robot_joint_sequence": ["lbr_iiwa_joint_1", "lbr_iiwa_joint_2", "lbr_iiwa_joint_3", "lbr_iiwa_joint_4", "lbr_iiwa_joint_5", "lbr_iiwa_joint_6", "lbr_iiwa_joint_7"], 150 | "sensor": "ati", 151 | "sensor_ft_field": [0, 6], 152 | "offset": [-56.233, 1.944, -4.433, 0.0, 0.202, -0.52], 153 | "centroid": [0.0, 0.0, 0.065], 154 | "gravity": [0.0, 0.0, -12.5347212, 0.0], 155 | "ts_rot": [[0,1,0,0],[-1,0,0,0],[0,0,1,0],[0,0,0,1]], 156 | "in_hand": ["135122075425", "135122070361"], 157 | "tc_mat": [[0,-1,0,0.035], [1,0,0,0.055], [0,0,1,0.20], [0,0,0,1]], 158 | "align_mat_tcp": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 159 | "align_mat_base": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], 160 | "tactile": false, 161 | "gripper":"Robotiq 2F-85" 162 | } 163 | ] -------------------------------------------------------------------------------- /configs/default.yaml: -------------------------------------------------------------------------------- 1 | # general visualization settings 2 | robot_configs: 'configs/configs.json' 3 | screenshot_path: './res' 4 | 5 | time_interval: 100 6 | max_traj_size: 15 7 | viewport_width: 1280 8 | viewport_height: 720 9 | chosen_cam_idx: -1 10 | 11 | enable_ft: true 12 | enable_pcd: false 13 | enable_model: true 14 | enable_traj: true 15 | 16 | # point cloud generation and display 17 | downsample_voxel_size_m: 0.005 18 | filter_num_neighbor: 10 19 | filter_radius_m: 0.01 20 | filter_std_ratio: 2.0 21 | normal_radius: 0.01 22 | normal_num_neighbor: 30 23 | min_depth_m: 0.4 24 | max_depth_m: 1.0 25 | resolution: !!python/tuple 26 | - 640 27 | - 360 28 | debug: false 29 | -------------------------------------------------------------------------------- /models/flexiv/flexivBot.xml: -------------------------------------------------------------------------------- 1 | 2 | 25 | 26 | 27 | ground 28 | 0.000000 0.000000 0.0000 29 | 0.0 0.0 0.0 1.0 30 | 2.368575 31 | 0.006163 0.006283 0.004321 -0.000025 -0.000067 -0.000048 32 | -0.002682 0.002022 0.073196 33 | ground 34 | none 35 | fixed 36 | -360 360 37 | 80 38 | 0 39 | -0.000000 40 | 0.0 41 | 1 42 | 1 43 | 0.0 44 | FREQ_NONE 45 | 0.5 46 | 1.0 47 | 1.0 48 | 49 | 50 | graphics/link0_body.stl 51 | graphics/link0_body.stl 52 | 0.0 0.0 0.0 53 | 1.0 0.0 0.0 0.0 54 | 1 1 1 55 | 56 | 57 | 58 | 59 | link1 60 | 0.0 0.0 0.113700 61 | 1.0 0.0 0.0 0.0 62 | 3.39008 63 | 0.027446 0.027415 0.005702 -0.000003 0.000022 -0.000866 64 | 0.000011 -0.004050 0.144322 65 | joint1 66 | ground 67 | rz 68 | -163.5 163.5 69 | 150 70 | 130 71 | 0 72 | 0.0 73 | 0.0 74 | -1 75 | -1 76 | 0.0 77 | FREQ_20 78 | 0.5 79 | 1.0 80 | 1.0 81 | 82 | 83 | graphics/link1_assemble.stl 84 | graphics/link1_assemble.stl 85 | 0.0 0.0 0.0 86 | 1.0 0.0 0.0 0.0 87 | 1 1 1 88 | 89 | 90 | 91 | 92 | link2 93 | 0.0 0.015 0.2113 94 | 1.0 0.0 0.0 0.0 95 | 2.502653 96 | 0.021277 0.021694 0.003095 0.000002 0.000010 -0.002064 97 | -0.000118 0.021132 0.111573 98 | joint2 99 | link1 100 | ry 101 | -120.5 120.5 102 | 150 103 | 130 104 | 0 105 | -40.0 106 | 0.0 107 | -1 108 | -1 109 | 180.0 110 | FREQ_20 111 | 0.0 112 | 1.0 113 | 1.0 114 | 115 | 116 | graphics/link2_assemble.stl 117 | graphics/link2_assemble.stl 118 | 0.0 0.0 0.0 119 | 1.0 0.0 0.0 0.0 120 | 1 1 1 121 | 122 | 123 | 124 | 125 | link3 126 | 0.0 0.015 0.205 127 | 1.0 0.0 0.0 0.0 128 | 2.202413 129 | 0.012855 0.012154 0.003370 0.000038 -0.000713 -0.000583 130 | -0.005534 -0.003869 0.140358 131 | joint3 132 | link2 133 | rz 134 | -165 165 135 | 150 136 | 70 137 | -0.0 138 | 0.0 139 | 0 140 | -1 141 | -1 142 | 0.0 143 | FREQ_40 144 | 0.0 145 | 1.0 146 | 1.0 147 | 148 | 149 | graphics/link3_assemble.stl 150 | graphics/link3_assemble.stl 151 | 0.0 0.0 0.0 152 | 1.0 0.0 0.0 0.0 153 | 1 1 1 154 | 155 | 156 | 157 | 158 | link4 159 | -0.01 -0.025 0.19 160 | 1.0 0.0 0.0 0.0 161 | 2.18366 162 | 0.015492 0.015639 0.002482 0.000055 0.000545 0.001025 163 | 0.006592 -0.024396 0.114736 164 | joint4 165 | link3 166 | ry 167 | -142 111 168 | 150 169 | 60 170 | -90.0 171 | 0.0 172 | 0 173 | 1 174 | 1 175 | -180.0 176 | FREQ_30 177 | 0.0 178 | 1.0 179 | 1.0 180 | 181 | 182 | graphics/link4_assemble.stl 183 | graphics/link4_assemble.stl 184 | 0.0 0.0 0.0 185 | 1.0 0.0 0.0 0.0 186 | 1 1 1 187 | 188 | 189 | 190 | 191 | link5 192 | 0.01 -0.02 0.195 193 | 1.0 0.0 0.0 0.0 194 | 2.158026 195 | 0.012199 0.011381 0.003241 -0.000013 0.000029 0.000461 196 | -0.000248 0.003520 0.136665 197 | joint5 198 | link4 199 | rz 200 | -165 165 201 | 480 202 | 20 203 | 0.000000 204 | 0.0 205 | 0 206 | -1 207 | -1 208 | 0.0 209 | FREQ_200 210 | 0.0 211 | 1.0 212 | 1.0 213 | 214 | 215 | graphics/link5_assemble.stl 216 | graphics/link5_assemble.stl 217 | 0.0 0.0 0.0 218 | 1.0 0.0 0.0 0.0 219 | 1 1 1 220 | 221 | 222 | 223 | 224 | link6 225 | 0.0 0.03 0.185 226 | 1.0 0.0 0.0 0.0 227 | 2.01581 228 | 0.005364 0.006365 0.003039 0.000111 -0.000699 -0.000432 229 | -0.011668 0.018814 0.067823 230 | joint6 231 | link5 232 | ry 233 | -89 105 234 | 480 235 | 20 236 | 40.0 237 | 22.0 238 | 0 239 | -1 240 | -1 241 | -90.0 242 | FREQ_200 243 | 0.0 244 | 1.0 245 | 1.0 246 | 247 | 248 | graphics/link6_assemble.stl 249 | graphics/link6_assemble.stl 250 | 0.0 0.0 0.0 251 | 1.0 0.0 0.0 0.0 252 | 1 1 1 253 | 254 | 255 | 256 | 257 | link7 258 | -0.058263 0.0145 0.096 259 | 0.7071068 0.0 -0.7071068 0.0 260 | 0.671925 261 | 0.000888 0.000886 0.000772 0.0 -0.0 0.0 262 | 0.000678 -0.000608 0.021929 263 | joint7 264 | link6 265 | rz 266 | -150 150 267 | 480 268 | 20 269 | 0 270 | -0.0 271 | 0.0 272 | -1 273 | -1 274 | 0.0 275 | FREQ_200 276 | 0.0 277 | 1.0 278 | 1.0 279 | 280 | 281 | graphics/link7_assemble.stl 282 | graphics/link7_assemble.stl 283 | 0.0 0.0 0.0 284 | 1.0 0.0 0.0 0.0 285 | 1 1 1 286 | 287 | 288 | 289 | 290 | -------------------------------------------------------------------------------- /models/flexiv/flexivCfg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | flexivBot 9 | flexivBot.xml 10 | controllers/ctrlBullet.xml 11 | Bullet 12 | 0.0 0.0 -9.81 13 | 0.0 0.0 14 | 0 15 | rbdl 16 | 17 | 21 | 22 | link7 23 | 0.0 0.0 0.074 24 | 0.0 0.0 0.0 1.0 25 | 26 | 32 | 33 | ../../../../flexiv_robots/app/ArmDriverApp 34 | ../../../../flexiv_robots/specs/A01F-003/robot_config.xml 35 | v1.l.5 36 | v3.m.5 37 | 38 | 50 | 62 | 66 | 67 | None 68 | 69 | 70 | 78 | 84 | 85 | calibration/jointTorqueSensorOffset.cali 86 | calibration/kinematics.cali 87 | calibration/jointPositionLinkEncoder.cali 88 | 89 | 90 | rules/RULES 91 | 98 | 99 | CAMERA_RGBD 100 | link7 101 | 0.07717 0.0325 0.0365 102 | 0.7071068 0.0 0.0 0.7071068 103 | 0.088 0.015 0.015 104 | -0.0325 0.0 0.0 105 | 106 | 107 | 108 | CAMERA_RGBD 109 | world 110 | -0.0 0.5 0.0 111 | 1.0 0.0 0.0 0.0 112 | 0.088 0.015 0.015 113 | -0.0325 0.0 0.0 114 | 115 | 116 | 117 | 118 | 1.8 0.0 0.35 119 | 0.0 0.0 0.35 120 | 0 0 1 121 | 0 10 122 | 123 | 124 | 50.0 0.0 0.0 125 | 0.0 0.0 0.1 126 | 127 | 128 | 10.0 5.0 1.0 129 | 0.0 0.0 0.5 130 | 131 | 132 | -10.0 -10.0 1.0 133 | 0.0 0.0 0.0 134 | 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /models/flexiv/flexiv_rizon4.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link0_body.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.002 5 | Ns 15.686281 6 | Ka 0.150000 0.150000 0.150000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link0_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link0_body.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link0_body_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.002 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link0_body_pvt4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 15.686281 6 | Ka 0.150000 0.150000 0.150000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link1_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.002 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.000 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link1_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link1_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link1_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.000 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link2_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.002 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.001 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link2_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link2_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link2_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.001 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link3_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.001 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.001 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link3_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link3_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link3_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.001 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link4_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.001 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.000 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link4_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link4_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link4_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.000 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link5_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.001 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.000 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link5_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link5_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link5_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.000 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link6_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl cover_white.001 5 | Ns 92.156872 6 | Ka 0.250000 0.250000 0.250000 7 | Kd 0.969921 0.969921 0.969921 8 | Ks 0.072304 0.072304 0.072304 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl robot.001 15 | Ns 15.686281 16 | Ka 0.150000 0.150000 0.150000 17 | Kd 0.487364 0.487364 0.527718 18 | Ks 0.600013 0.600013 0.600013 19 | Ke 0.0 0.0 0.0 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link6_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link6_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link6_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.001 5 | Ns 15.686281 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link7_assemble.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl robot.002 5 | Ns 15.686281 6 | Ka 0.150000 0.150000 0.150000 7 | Kd 0.487364 0.487364 0.527718 8 | Ks 0.600013 0.600013 0.600013 9 | Ke 0.0 0.0 0.0 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link7_assemble.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/flexiv/graphics/link7_assemble.stl -------------------------------------------------------------------------------- /models/flexiv/graphics/link7_assemble_coll.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 500 6 | Ka 0.8 0.8 0.8 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | -------------------------------------------------------------------------------- /models/flexiv/graphics/link7_assemble_coll.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.83.0 OBJ File: '' 2 | # www.blender.org 3 | mtllib link7_assemble_coll.mtl 4 | o Cylinder_Cylinder.002 5 | v 0.000000 0.051500 0.000000 6 | v 0.000000 0.051500 0.080000 7 | v 0.009950 0.050520 0.000000 8 | v 0.009950 0.050520 0.080000 9 | v 0.019517 0.047618 0.000000 10 | v 0.019517 0.047618 0.080000 11 | v 0.028334 0.042905 0.000000 12 | v 0.028334 0.042905 0.080000 13 | v 0.036062 0.036562 0.000000 14 | v 0.036062 0.036562 0.080000 15 | v 0.042405 0.028834 0.000000 16 | v 0.042405 0.028834 0.080000 17 | v 0.047118 0.020017 0.000000 18 | v 0.047118 0.020017 0.080000 19 | v 0.050020 0.010450 0.000000 20 | v 0.050020 0.010450 0.080000 21 | v 0.051000 0.000500 0.000000 22 | v 0.051000 0.000500 0.080000 23 | v 0.050020 -0.009450 0.000000 24 | v 0.050020 -0.009450 0.080000 25 | v 0.047118 -0.019017 0.000000 26 | v 0.047118 -0.019017 0.080000 27 | v 0.042405 -0.027834 0.000000 28 | v 0.042405 -0.027834 0.080000 29 | v 0.036062 -0.035562 0.000000 30 | v 0.036062 -0.035562 0.080000 31 | v 0.028334 -0.041905 0.000000 32 | v 0.028334 -0.041905 0.080000 33 | v 0.019517 -0.046618 -0.000000 34 | v 0.019517 -0.046618 0.080000 35 | v 0.009950 -0.049520 -0.000000 36 | v 0.009950 -0.049520 0.080000 37 | v -0.000000 -0.050500 -0.000000 38 | v -0.000000 -0.050500 0.080000 39 | v -0.009950 -0.049520 -0.000000 40 | v -0.009950 -0.049520 0.080000 41 | v -0.019517 -0.046618 -0.000000 42 | v -0.019517 -0.046618 0.080000 43 | v -0.028334 -0.041905 0.000000 44 | v -0.028334 -0.041905 0.080000 45 | v -0.036062 -0.035562 0.000000 46 | v -0.036062 -0.035562 0.080000 47 | v -0.042405 -0.027834 0.000000 48 | v -0.042405 -0.027834 0.080000 49 | v -0.047118 -0.019017 0.000000 50 | v -0.047118 -0.019017 0.080000 51 | v -0.050020 -0.009450 0.000000 52 | v -0.050020 -0.009450 0.080000 53 | v -0.051000 0.000500 0.000000 54 | v -0.051000 0.000500 0.080000 55 | v -0.050020 0.010450 0.000000 56 | v -0.050020 0.010450 0.080000 57 | v -0.047118 0.020017 0.000000 58 | v -0.047118 0.020017 0.080000 59 | v -0.042405 0.028834 0.000000 60 | v -0.042405 0.028834 0.080000 61 | v -0.036062 0.036562 0.000000 62 | v -0.036062 0.036562 0.080000 63 | v -0.028334 0.042905 0.000000 64 | v -0.028334 0.042905 0.080000 65 | v -0.019517 0.047618 0.000000 66 | v -0.019517 0.047618 0.080000 67 | v -0.009950 0.050520 0.000000 68 | v -0.009950 0.050520 0.080000 69 | vt 1.000000 0.500000 70 | vt 1.000000 1.000000 71 | vt 0.968750 1.000000 72 | vt 0.968750 0.500000 73 | vt 0.937500 1.000000 74 | vt 0.937500 0.500000 75 | vt 0.906250 1.000000 76 | vt 0.906250 0.500000 77 | vt 0.875000 1.000000 78 | vt 0.875000 0.500000 79 | vt 0.843750 1.000000 80 | vt 0.843750 0.500000 81 | vt 0.812500 1.000000 82 | vt 0.812500 0.500000 83 | vt 0.781250 1.000000 84 | vt 0.781250 0.500000 85 | vt 0.750000 1.000000 86 | vt 0.750000 0.500000 87 | vt 0.718750 1.000000 88 | vt 0.718750 0.500000 89 | vt 0.687500 1.000000 90 | vt 0.687500 0.500000 91 | vt 0.656250 1.000000 92 | vt 0.656250 0.500000 93 | vt 0.625000 1.000000 94 | vt 0.625000 0.500000 95 | vt 0.593750 1.000000 96 | vt 0.593750 0.500000 97 | vt 0.562500 1.000000 98 | vt 0.562500 0.500000 99 | vt 0.531250 1.000000 100 | vt 0.531250 0.500000 101 | vt 0.500000 1.000000 102 | vt 0.500000 0.500000 103 | vt 0.468750 1.000000 104 | vt 0.468750 0.500000 105 | vt 0.437500 1.000000 106 | vt 0.437500 0.500000 107 | vt 0.406250 1.000000 108 | vt 0.406250 0.500000 109 | vt 0.375000 1.000000 110 | vt 0.375000 0.500000 111 | vt 0.343750 1.000000 112 | vt 0.343750 0.500000 113 | vt 0.312500 1.000000 114 | vt 0.312500 0.500000 115 | vt 0.281250 1.000000 116 | vt 0.281250 0.500000 117 | vt 0.250000 1.000000 118 | vt 0.250000 0.500000 119 | vt 0.218750 1.000000 120 | vt 0.218750 0.500000 121 | vt 0.187500 1.000000 122 | vt 0.187500 0.500000 123 | vt 0.156250 1.000000 124 | vt 0.156250 0.500000 125 | vt 0.125000 1.000000 126 | vt 0.125000 0.500000 127 | vt 0.093750 1.000000 128 | vt 0.093750 0.500000 129 | vt 0.062500 1.000000 130 | vt 0.062500 0.500000 131 | vt 0.296822 0.485388 132 | vt 0.250000 0.490000 133 | vt 0.203179 0.485389 134 | vt 0.158156 0.471731 135 | vt 0.116663 0.449553 136 | vt 0.080295 0.419706 137 | vt 0.050447 0.383337 138 | vt 0.028269 0.341844 139 | vt 0.014612 0.296822 140 | vt 0.010000 0.250000 141 | vt 0.014611 0.203179 142 | vt 0.028269 0.158156 143 | vt 0.050447 0.116663 144 | vt 0.080294 0.080294 145 | vt 0.116663 0.050447 146 | vt 0.158156 0.028269 147 | vt 0.203178 0.014612 148 | vt 0.250000 0.010000 149 | vt 0.296822 0.014612 150 | vt 0.341844 0.028269 151 | vt 0.383337 0.050447 152 | vt 0.419706 0.080294 153 | vt 0.449553 0.116663 154 | vt 0.471731 0.158156 155 | vt 0.485388 0.203178 156 | vt 0.490000 0.250000 157 | vt 0.485388 0.296822 158 | vt 0.471731 0.341844 159 | vt 0.449553 0.383337 160 | vt 0.419706 0.419706 161 | vt 0.383337 0.449553 162 | vt 0.341844 0.471731 163 | vt 0.031250 1.000000 164 | vt 0.031250 0.500000 165 | vt 0.000000 1.000000 166 | vt 0.000000 0.500000 167 | vt 0.750000 0.490000 168 | vt 0.796822 0.485388 169 | vt 0.841844 0.471731 170 | vt 0.883337 0.449553 171 | vt 0.919706 0.419706 172 | vt 0.949553 0.383337 173 | vt 0.971731 0.341844 174 | vt 0.985388 0.296822 175 | vt 0.990000 0.250000 176 | vt 0.985388 0.203178 177 | vt 0.971731 0.158156 178 | vt 0.949553 0.116663 179 | vt 0.919706 0.080294 180 | vt 0.883337 0.050447 181 | vt 0.841844 0.028269 182 | vt 0.796822 0.014612 183 | vt 0.750000 0.010000 184 | vt 0.703178 0.014612 185 | vt 0.658156 0.028269 186 | vt 0.616663 0.050447 187 | vt 0.580294 0.080294 188 | vt 0.550447 0.116663 189 | vt 0.528269 0.158156 190 | vt 0.514611 0.203179 191 | vt 0.510000 0.250000 192 | vt 0.514612 0.296822 193 | vt 0.528269 0.341844 194 | vt 0.550447 0.383337 195 | vt 0.580295 0.419706 196 | vt 0.616663 0.449553 197 | vt 0.658156 0.471731 198 | vt 0.703179 0.485389 199 | vn 0.0980 0.9952 0.0000 200 | vn 0.2903 0.9569 0.0000 201 | vn 0.4714 0.8819 0.0000 202 | vn 0.6344 0.7730 0.0000 203 | vn 0.7730 0.6344 0.0000 204 | vn 0.8819 0.4714 0.0000 205 | vn 0.9569 0.2903 0.0000 206 | vn 0.9952 0.0980 0.0000 207 | vn 0.9952 -0.0980 -0.0000 208 | vn 0.9569 -0.2903 -0.0000 209 | vn 0.8819 -0.4714 -0.0000 210 | vn 0.7730 -0.6344 -0.0000 211 | vn 0.6344 -0.7730 0.0000 212 | vn 0.4714 -0.8819 0.0000 213 | vn 0.2903 -0.9569 0.0000 214 | vn 0.0980 -0.9952 0.0000 215 | vn -0.0980 -0.9952 0.0000 216 | vn -0.2903 -0.9569 0.0000 217 | vn -0.4714 -0.8819 0.0000 218 | vn -0.6344 -0.7730 0.0000 219 | vn -0.7730 -0.6344 -0.0000 220 | vn -0.8819 -0.4714 -0.0000 221 | vn -0.9569 -0.2903 -0.0000 222 | vn -0.9952 -0.0980 -0.0000 223 | vn -0.9952 0.0980 0.0000 224 | vn -0.9569 0.2903 0.0000 225 | vn -0.8819 0.4714 0.0000 226 | vn -0.7730 0.6344 0.0000 227 | vn -0.6344 0.7730 0.0000 228 | vn -0.4714 0.8819 0.0000 229 | vn 0.0000 0.0000 1.0000 230 | vn -0.2903 0.9569 0.0000 231 | vn -0.0980 0.9952 0.0000 232 | vn 0.0000 0.0000 -1.0000 233 | usemtl None 234 | s off 235 | f 1/1/1 2/2/1 4/3/1 3/4/1 236 | f 3/4/2 4/3/2 6/5/2 5/6/2 237 | f 5/6/3 6/5/3 8/7/3 7/8/3 238 | f 7/8/4 8/7/4 10/9/4 9/10/4 239 | f 9/10/5 10/9/5 12/11/5 11/12/5 240 | f 11/12/6 12/11/6 14/13/6 13/14/6 241 | f 13/14/7 14/13/7 16/15/7 15/16/7 242 | f 15/16/8 16/15/8 18/17/8 17/18/8 243 | f 17/18/9 18/17/9 20/19/9 19/20/9 244 | f 19/20/10 20/19/10 22/21/10 21/22/10 245 | f 21/22/11 22/21/11 24/23/11 23/24/11 246 | f 23/24/12 24/23/12 26/25/12 25/26/12 247 | f 25/26/13 26/25/13 28/27/13 27/28/13 248 | f 27/28/14 28/27/14 30/29/14 29/30/14 249 | f 29/30/15 30/29/15 32/31/15 31/32/15 250 | f 31/32/16 32/31/16 34/33/16 33/34/16 251 | f 33/34/17 34/33/17 36/35/17 35/36/17 252 | f 35/36/18 36/35/18 38/37/18 37/38/18 253 | f 37/38/19 38/37/19 40/39/19 39/40/19 254 | f 39/40/20 40/39/20 42/41/20 41/42/20 255 | f 41/42/21 42/41/21 44/43/21 43/44/21 256 | f 43/44/22 44/43/22 46/45/22 45/46/22 257 | f 45/46/23 46/45/23 48/47/23 47/48/23 258 | f 47/48/24 48/47/24 50/49/24 49/50/24 259 | f 49/50/25 50/49/25 52/51/25 51/52/25 260 | f 51/52/26 52/51/26 54/53/26 53/54/26 261 | f 53/54/27 54/53/27 56/55/27 55/56/27 262 | f 55/56/28 56/55/28 58/57/28 57/58/28 263 | f 57/58/29 58/57/29 60/59/29 59/60/29 264 | f 59/60/30 60/59/30 62/61/30 61/62/30 265 | f 4/63/31 2/64/31 64/65/31 62/66/31 60/67/31 58/68/31 56/69/31 54/70/31 52/71/31 50/72/31 48/73/31 46/74/31 44/75/31 42/76/31 40/77/31 38/78/31 36/79/31 34/80/31 32/81/31 30/82/31 28/83/31 26/84/31 24/85/31 22/86/31 20/87/31 18/88/31 16/89/31 14/90/31 12/91/31 10/92/31 8/93/31 6/94/31 266 | f 61/62/32 62/61/32 64/95/32 63/96/32 267 | f 63/96/33 64/95/33 2/97/33 1/98/33 268 | f 1/99/34 3/100/34 5/101/34 7/102/34 9/103/34 11/104/34 13/105/34 15/106/34 17/107/34 19/108/34 21/109/34 23/110/34 25/111/34 27/112/34 29/113/34 31/114/34 33/115/34 35/116/34 37/117/34 39/118/34 41/119/34 43/120/34 45/121/34 47/122/34 49/123/34 51/124/34 53/125/34 55/126/34 57/127/34 59/128/34 61/129/34 63/130/34 269 | -------------------------------------------------------------------------------- /models/flexiv/robot_pvt3.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | -------------------------------------------------------------------------------- /models/flexiv/robot_pvt4.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | -------------------------------------------------------------------------------- /models/franka/LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Stanford Autonomous Systems Lab 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 | -------------------------------------------------------------------------------- /models/franka/franka modified from panda.txt: -------------------------------------------------------------------------------- 1 | https://github.com/StanfordASL/PandaRobot.jl/tree/master/deps/Panda -------------------------------------------------------------------------------- /models/franka/franka.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | -------------------------------------------------------------------------------- /models/franka/meshes/collision/finger.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.0.1262) 3 | 4 | mtllib finger.stl.obj.mtl 5 | 6 | # 96 vertex positions 7 | v 0.01036 0.0264034 0.000154629 8 | v 0.0104486 0.0025833 0.000146801 9 | v -0.0103872 0.00253418 0.000131696 10 | v -0.0103872 0.00253418 0.000131696 11 | v -0.0104795 0.0161016 0.0189882 12 | v -0.0104013 0.0263094 0.00016651 13 | v -0.0104013 0.0263094 0.00016651 14 | v -0.0104795 0.0161016 0.0189882 15 | v -0.0103889 0.0252203 0.0191876 16 | v 0.0104486 0.0025833 0.000146801 17 | v -0.0087304 -2.35252e-05 0.0361648 18 | v -0.0103872 0.00253418 0.000131696 19 | v 0.0104005 0.0252534 0.0190366 20 | v -0.0103889 0.0252203 0.0191876 21 | v 0.00583983 0.0142743 0.0538035 22 | v -0.0103872 0.00253418 0.000131696 23 | v -0.0104013 0.0263094 0.00016651 24 | v 0.01036 0.0264034 0.000154629 25 | v 0.00861608 0.0139887 0.0513279 26 | v 0.0104948 0.0151026 0.0184356 27 | v 0.0104005 0.0252534 0.0190366 28 | v 0.0104948 0.0151026 0.0184356 29 | v 0.00869277 -0.000132643 0.0501662 30 | v 0.0104486 0.0025833 0.000146801 31 | v 0.00861608 0.0139887 0.0513279 32 | v 0.00869277 -0.000132643 0.0501662 33 | v 0.0104948 0.0151026 0.0184356 34 | v -0.00862294 -5.68019e-05 0.0509528 35 | v -0.0103872 0.00253418 0.000131696 36 | v -0.0087304 -2.35252e-05 0.0361648 37 | v 0.0104486 0.0025833 0.000146801 38 | v 0.00869277 -0.000132643 0.0501662 39 | v -0.0087304 -2.35252e-05 0.0361648 40 | v 0.00869277 -0.000132643 0.0501662 41 | v -0.00548142 -9.11208e-05 0.0537247 42 | v -0.0087304 -2.35252e-05 0.0361648 43 | v 0.0104005 0.0252534 0.0190366 44 | v 0.0104948 0.0151026 0.0184356 45 | v 0.0104486 0.0025833 0.000146801 46 | v 0.0104005 0.0252534 0.0190366 47 | v 0.0104486 0.0025833 0.000146801 48 | v 0.01036 0.0264034 0.000154629 49 | v 0.0104005 0.0252534 0.0190366 50 | v 0.01036 0.0264034 0.000154629 51 | v -0.0103889 0.0252203 0.0191876 52 | v -0.0103889 0.0252203 0.0191876 53 | v -0.00527792 0.0142931 0.053849 54 | v 0.00583983 0.0142743 0.0538035 55 | v -0.00777838 0.0142182 0.0523659 56 | v -0.00527792 0.0142931 0.053849 57 | v -0.0103889 0.0252203 0.0191876 58 | v -0.00862294 -5.68019e-05 0.0509528 59 | v -0.00548142 -9.11208e-05 0.0537247 60 | v -0.00777838 0.0142182 0.0523659 61 | v -0.0103872 0.00253418 0.000131696 62 | v -0.00862294 -5.68019e-05 0.0509528 63 | v -0.0104795 0.0161016 0.0189882 64 | v 0.0104005 0.0252534 0.0190366 65 | v 0.00583983 0.0142743 0.0538035 66 | v 0.00861608 0.0139887 0.0513279 67 | v 0.01036 0.0264034 0.000154629 68 | v -0.0104013 0.0263094 0.00016651 69 | v -0.0103889 0.0252203 0.0191876 70 | v -0.0104795 0.0161016 0.0189882 71 | v -0.00884117 0.0139176 0.0505894 72 | v -0.0103889 0.0252203 0.0191876 73 | v -0.00884117 0.0139176 0.0505894 74 | v -0.00777838 0.0142182 0.0523659 75 | v -0.0103889 0.0252203 0.0191876 76 | v 0.00583983 0.0142743 0.0538035 77 | v -0.00548142 -9.11208e-05 0.0537247 78 | v 0.00613802 -2.06026e-05 0.0535776 79 | v -0.00527792 0.0142931 0.053849 80 | v -0.00548142 -9.11208e-05 0.0537247 81 | v 0.00583983 0.0142743 0.0538035 82 | v 0.00869277 -0.000132643 0.0501662 83 | v 0.00613802 -2.06026e-05 0.0535776 84 | v -0.00548142 -9.11208e-05 0.0537247 85 | v 0.00613802 -2.06026e-05 0.0535776 86 | v 0.00869277 -0.000132643 0.0501662 87 | v 0.00861608 0.0139887 0.0513279 88 | v -0.00548142 -9.11208e-05 0.0537247 89 | v -0.00862294 -5.68019e-05 0.0509528 90 | v -0.0087304 -2.35252e-05 0.0361648 91 | v -0.00777838 0.0142182 0.0523659 92 | v -0.00548142 -9.11208e-05 0.0537247 93 | v -0.00527792 0.0142931 0.053849 94 | v -0.00862294 -5.68019e-05 0.0509528 95 | v -0.00884117 0.0139176 0.0505894 96 | v -0.0104795 0.0161016 0.0189882 97 | v 0.00613802 -2.06026e-05 0.0535776 98 | v 0.00861608 0.0139887 0.0513279 99 | v 0.00583983 0.0142743 0.0538035 100 | v -0.00777838 0.0142182 0.0523659 101 | v -0.00884117 0.0139176 0.0505894 102 | v -0.00862294 -5.68019e-05 0.0509528 103 | 104 | # 0 UV coordinates 105 | 106 | # 96 vertex normals 107 | vn 0.000724164 0.000331308 -1 108 | vn 0.000724164 0.000331308 -1 109 | vn 0.000724164 0.000331308 -1 110 | vn -0.99999 -0.000585782 -0.00447174 111 | vn -0.99999 -0.000585782 -0.00447174 112 | vn -0.99999 -0.000585782 -0.00447174 113 | vn -0.99995 0.00990875 0.00122006 114 | vn -0.99995 0.00990875 0.00122006 115 | vn -0.99995 0.00990875 0.00122006 116 | vn 0.00240304 -0.997479 -0.0709137 117 | vn 0.00240304 -0.997479 -0.0709137 118 | vn 0.00240304 -0.997479 -0.0709137 119 | vn 0.000668274 0.953556 0.301214 120 | vn 0.000668274 0.953556 0.301214 121 | vn 0.000668274 0.953556 0.301214 122 | vn -0.0005789 0.00146393 -0.999999 123 | vn -0.0005789 0.00146393 -0.999999 124 | vn -0.0005789 0.00146393 -0.999999 125 | vn 0.998344 0.00589157 0.0572229 126 | vn 0.998344 0.00589157 0.0572229 127 | vn 0.998344 0.00589157 0.0572229 128 | vn 0.998185 -0.050838 0.0322792 129 | vn 0.998185 -0.050838 0.0322792 130 | vn 0.998185 -0.050838 0.0322792 131 | vn 0.998371 0.000729252 0.0570496 132 | vn 0.998371 0.000729252 0.0570496 133 | vn 0.998371 0.000729252 0.0570496 134 | vn -0.871287 -0.490747 0.00522707 135 | vn -0.871287 -0.490747 0.00522707 136 | vn -0.871287 -0.490747 0.00522707 137 | vn 0.0362712 -0.99794 -0.0529128 138 | vn 0.0362712 -0.99794 -0.0529128 139 | vn 0.0362712 -0.99794 -0.0529128 140 | vn -0.00372285 -0.999988 -0.00316058 141 | vn -0.00372285 -0.999988 -0.00316058 142 | vn -0.00372285 -0.999988 -0.00316058 143 | vn 0.999909 0.00984192 -0.00926313 144 | vn 0.999909 0.00984192 -0.00926313 145 | vn 0.999909 0.00984192 -0.00926313 146 | vn 0.999991 0.00372285 -0.00191858 147 | vn 0.999991 0.00372285 -0.00191858 148 | vn 0.999991 0.00372285 -0.00191858 149 | vn -0.0011495 0.99815 0.0607926 150 | vn -0.0011495 0.99815 0.0607926 151 | vn -0.0011495 0.99815 0.0607926 152 | vn 0.00284562 0.953846 0.300284 153 | vn 0.00284562 0.953846 0.300284 154 | vn 0.00284562 0.953846 0.300284 155 | vn -0.218924 0.920873 0.32259 156 | vn -0.218924 0.920873 0.32259 157 | vn -0.218924 0.920873 0.32259 158 | vn -0.661425 -0.0350314 0.749193 159 | vn -0.661425 -0.0350314 0.749193 160 | vn -0.661425 -0.0350314 0.749193 161 | vn -0.998169 -0.0513123 0.0320353 162 | vn -0.998169 -0.0513123 0.0320353 163 | vn -0.998169 -0.0513123 0.0320353 164 | vn 0.377717 0.867563 0.323518 165 | vn 0.377717 0.867563 0.323518 166 | vn 0.377717 0.867563 0.323518 167 | vn -0.00448618 0.998355 0.0571685 168 | vn -0.00448618 0.998355 0.0571685 169 | vn -0.00448618 0.998355 0.0571685 170 | vn -0.998589 0.0087761 0.0523757 171 | vn -0.998589 0.0087761 0.0523757 172 | vn -0.998589 0.0087761 0.0523757 173 | vn -0.665951 0.690851 0.281485 174 | vn -0.665951 0.690851 0.281485 175 | vn -0.665951 0.690851 0.281485 176 | vn 0.0127505 -0.0155288 0.999798 177 | vn 0.0127505 -0.0155288 0.999798 178 | vn 0.0127505 -0.0155288 0.999798 179 | vn 0.00408502 -0.00870048 0.999954 180 | vn 0.00408502 -0.00870048 0.999954 181 | vn 0.00408502 -0.00870048 0.999954 182 | vn 0.006542 -0.999267 0.0377181 183 | vn 0.006542 -0.999267 0.0377181 184 | vn 0.006542 -0.999267 0.0377181 185 | vn 0.798906 -0.0450007 0.59977 186 | vn 0.798906 -0.0450007 0.59977 187 | vn 0.798906 -0.0450007 0.59977 188 | vn -0.00899609 -0.999957 -0.00218479 189 | vn -0.00899609 -0.999957 -0.00218479 190 | vn -0.00899609 -0.999957 -0.00218479 191 | vn -0.510144 -0.000216662 0.860089 192 | vn -0.510144 -0.000216662 0.860089 193 | vn -0.510144 -0.000216662 0.860089 194 | vn -0.998608 -0.0142745 0.0507836 195 | vn -0.998608 -0.0142745 0.0507836 196 | vn -0.998608 -0.0142745 0.0507836 197 | vn 0.665648 0.00209596 0.746263 198 | vn 0.665648 0.00209596 0.746263 199 | vn 0.665648 0.00209596 0.746263 200 | vn -0.858159 -4.90786e-05 0.513384 201 | vn -0.858159 -4.90786e-05 0.513384 202 | vn -0.858159 -4.90786e-05 0.513384 203 | 204 | # Mesh '' with 32 faces 205 | g 206 | usemtl DefaultMaterial 207 | f 1//1 2//2 3//3 208 | f 4//4 5//5 6//6 209 | f 7//7 8//8 9//9 210 | f 10//10 11//11 12//12 211 | f 13//13 14//14 15//15 212 | f 16//16 17//17 18//18 213 | f 19//19 20//20 21//21 214 | f 22//22 23//23 24//24 215 | f 25//25 26//26 27//27 216 | f 28//28 29//29 30//30 217 | f 31//31 32//32 33//33 218 | f 34//34 35//35 36//36 219 | f 37//37 38//38 39//39 220 | f 40//40 41//41 42//42 221 | f 43//43 44//44 45//45 222 | f 46//46 47//47 48//48 223 | f 49//49 50//50 51//51 224 | f 52//52 53//53 54//54 225 | f 55//55 56//56 57//57 226 | f 58//58 59//59 60//60 227 | f 61//61 62//62 63//63 228 | f 64//64 65//65 66//66 229 | f 67//67 68//68 69//69 230 | f 70//70 71//71 72//72 231 | f 73//73 74//74 75//75 232 | f 76//76 77//77 78//78 233 | f 79//79 80//80 81//81 234 | f 82//82 83//83 84//84 235 | f 85//85 86//86 87//87 236 | f 88//88 89//89 90//90 237 | f 91//91 92//92 93//93 238 | f 94//94 95//95 96//96 239 | 240 | -------------------------------------------------------------------------------- /models/franka/panda.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | -------------------------------------------------------------------------------- /models/kuka/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | -------------------------------------------------------------------------------- /models/kuka/source.txt: -------------------------------------------------------------------------------- 1 | https://github.com/neka-nat/kinpy/tree/master/examples/kuka_iiwa -------------------------------------------------------------------------------- /models/ur5/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/base.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/forearm.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/shoulder.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/upperarm.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/wrist1.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/wrist2.stl -------------------------------------------------------------------------------- /models/ur5/meshes/ur5/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/meshes/ur5/collision/wrist3.stl -------------------------------------------------------------------------------- /models/ur5/source.txt: -------------------------------------------------------------------------------- 1 | https://github.com/Kami-code/UR5_Robotiq85_description/tree/main/src/ur5_description -------------------------------------------------------------------------------- /models/ur5/urdf/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | -------------------------------------------------------------------------------- /models/ur5/urdf/ur5 modified from ur5_robotiq_85.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/models/ur5/urdf/ur5 modified from ur5_robotiq_85.txt -------------------------------------------------------------------------------- /models/ur5/urdf/ur5.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | alive_progress 2 | ipython 3 | kinpy 4 | librosa 5 | matplotlib 6 | numpy 7 | open3d 8 | opencv_python 9 | pynput 10 | PyYAML 11 | tqdm 12 | transforms3d 13 | urdf_parser_py 14 | -------------------------------------------------------------------------------- /requirements_api.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | opencv_python 3 | transforms3d 4 | -------------------------------------------------------------------------------- /rh20t_api/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/rh20t_api/__init__.py -------------------------------------------------------------------------------- /rh20t_api/configurations.py: -------------------------------------------------------------------------------- 1 | import json 2 | import numpy as np 3 | from typing import List 4 | from .transforms import pose_array_euler_2_pose_array_quat, pose_array_quat_2_matrix, \ 5 | pose_array_rotvec_2_matrix, matrix_2_pose_array_quat, pose_array_quat_2_pose_array_euler 6 | from transforms3d.quaternions import axangle2quat 7 | 8 | def bound_to_0_2pi(num): return (num + np.pi * 2 if num < 0 else num) 9 | 10 | def identical_tcp_preprocessor(tcp:np.ndarray): return tcp 11 | def kuka_tcp_preprocessor(tcp:np.ndarray): return np.array([tcp[0] / 1000., tcp[1] / 1000., tcp[2] / 1000., bound_to_0_2pi(tcp[3]), bound_to_0_2pi(tcp[4]), bound_to_0_2pi(tcp[5])], dtype=np.float64) 12 | def franka_tcp_preprocessor(tcp:np.ndarray): return pose_array_quat_2_pose_array_euler(matrix_2_pose_array_quat(pose_array_rotvec_2_matrix(tcp) @ np.array([[1., 0., 0., 0.], [0., -1., 0., 0.], [0., 0., -1., 0], [0., 0., 0., 1.]], dtype=np.float64))) 13 | def ur5_tcp_preprocessor(tcp:np.ndarray): return np.array([tcp[0], tcp[1], tcp[2]] + axangle2quat(tcp[3:6], np.linalg.norm(tcp[3:6])).tolist(), dtype=np.float64) 14 | 15 | tcp_preprocessors = { 16 | "flexiv": identical_tcp_preprocessor, 17 | "ur5": ur5_tcp_preprocessor, 18 | "franka": franka_tcp_preprocessor, 19 | "kuka": kuka_tcp_preprocessor 20 | } 21 | 22 | def tcp_as_q(tcp:np.ndarray): 23 | assert tcp.shape == (6,), tcp 24 | tcp_q = pose_array_euler_2_pose_array_quat(tcp[0:6]) 25 | tcp_q = np.concatenate((tcp_q, tcp[6:]), axis=0) 26 | return tcp_q 27 | 28 | calib_tcp_preprocessors = { 29 | "flexiv": identical_tcp_preprocessor, 30 | "ur5": identical_tcp_preprocessor, 31 | "franka": franka_tcp_preprocessor, 32 | "kuka": kuka_tcp_preprocessor 33 | } 34 | 35 | def robotiq_gripper_preprocessor(gripper_cmd:np.ndarray): return [(255 - gripper_cmd[0]) / 255 * 85, gripper_cmd[1], gripper_cmd[2]] 36 | def dahuan_gripper_preprocessor(gripper_cmd:np.ndarray): return [gripper_cmd[0] / 1000 * 95, gripper_cmd[1], gripper_cmd[2]] 37 | def wsg_gripper_preprocessor(gripper_cmd:np.ndarray): return [gripper_cmd[0] / 1.1, gripper_cmd[1], gripper_cmd[2]] 38 | def franka_gripper_preprocessor(gripper_cmd:np.ndarray): return [gripper_cmd[0] / 100, gripper_cmd[1], gripper_cmd[2]] 39 | 40 | gripper_preprocessors = { 41 | "Dahuan AG-95": dahuan_gripper_preprocessor, 42 | "WSG-50": wsg_gripper_preprocessor, 43 | "Robotiq 2F-85": robotiq_gripper_preprocessor, 44 | "franka": franka_gripper_preprocessor 45 | } 46 | 47 | valid_sensors = ["dahuan", "ati", "none"] 48 | valid_grippers = ["Dahuan AG-95", "WSG-50", "Robotiq 2F-85", "franka"] 49 | 50 | class Configuration: 51 | def __init__(self, conf_dict): 52 | self.conf_num:int = conf_dict["conf_num"] 53 | self.robot:str = conf_dict["robot"] 54 | self.robot_urdf:str = conf_dict["robot_urdf"] 55 | self.robot_mesh:str = conf_dict["robot_mesh"] 56 | self.robot_tcp_field:List[int] = conf_dict["robot_tcp_field"] 57 | self.robot_ft_field:List[int] = conf_dict["robot_ft_field"] 58 | self.robot_joint_field:List[int] = conf_dict["robot_joint_field"] 59 | self.robot_joint_sequence:List[str] = conf_dict["robot_joint_sequence"] 60 | self.sensor:str = conf_dict["sensor"] 61 | self.sensor_ft_field:List[int] = conf_dict["sensor_ft_field"] 62 | self.offset:np.ndarray = np.array(conf_dict["offset"], dtype=np.float64) 63 | self.centroid:np.ndarray = np.array(conf_dict["centroid"], dtype=np.float64) 64 | self.gravity:np.ndarray = np.array(conf_dict["gravity"], dtype=np.float64).reshape(4, 1) 65 | self.tcp2sensor_rot:np.ndarray = np.array(conf_dict["ts_rot"], dtype=np.float64) 66 | self.in_hand_serial:List[str] = conf_dict["in_hand"] 67 | self.tcp_camera_mat:np.ndarray = np.array(conf_dict["tc_mat"], dtype=np.float64) 68 | self.align_mat_tcp:np.ndarray = np.array(conf_dict["align_mat_tcp"], dtype=np.float64) 69 | self.align_mat_base:np.ndarray = np.array(conf_dict["align_mat_base"], dtype=np.float64) 70 | self.tactile:bool = conf_dict["tactile"] 71 | self.gripper:str = conf_dict["gripper"] 72 | 73 | self.tcp_preprocessor = tcp_preprocessors[self.robot] 74 | self.calib_tcp_preprocessor = calib_tcp_preprocessors[self.robot] 75 | self.gripper_preprocessor = gripper_preprocessors[self.gripper] 76 | 77 | assert len(self.robot_tcp_field) == 2 78 | assert len(self.robot_ft_field) == 2 79 | assert len(self.sensor_ft_field) == (0 if self.sensor == "none" else 2) 80 | assert self.offset.shape == (6,) 81 | assert self.centroid.shape == (3,) 82 | assert self.tcp2sensor_rot.shape == (4, 4) 83 | assert self.tcp_camera_mat.shape == (4, 4) 84 | assert self.sensor in valid_sensors 85 | assert self.gripper in valid_grippers 86 | 87 | def update_offset(self, raw_fts:np.ndarray, raw_tcps:np.ndarray): 88 | """ 89 | Considering temperature drift, the force and torque sensor offset 90 | should be updated according to the values sampled at the beginning 91 | of a scene (when the the robot arm is considered to be still) 92 | 93 | Params: 94 | ---------- 95 | raw_fts: the sampled still force and torque values 96 | raw_tcps: the sampled still tcps 97 | 98 | Returns: 99 | ---------- 100 | None 101 | """ 102 | # print("updating offset...") 103 | np.set_printoptions(precision=3, suppress=True) 104 | # print("original offset:", self.offset) 105 | _Fg_homos, _offsets = [], [] 106 | for (raw_ft, raw_tcp) in zip(raw_fts, raw_tcps): 107 | # print("ft:", raw_ft, "tcp:", raw_tcp) 108 | raw_tcp = self.tcp_preprocessor(raw_tcp) 109 | _tcp = tcp_as_q(raw_tcp) if len(raw_tcp) == 6 else raw_tcp 110 | _tcp = pose_array_quat_2_matrix(_tcp) 111 | _centroid, _gravity = self.centroid, self.gravity[:3].reshape(3,) 112 | _gravity_torque = np.cross(_centroid, _gravity) 113 | _force, _torque = raw_ft[0:3], raw_ft[3:6] 114 | _T_offset = _torque - _gravity_torque 115 | _Fg_homo = np.array([_gravity[0], _gravity[1], _gravity[2], 0]).reshape(4, 1) 116 | _Fg_homos.append(_Fg_homo) 117 | _F_offset = _force - (self.tcp2sensor_rot @ np.linalg.inv(_tcp) @ _Fg_homo).reshape(4,)[:3] 118 | _offsets.append(np.concatenate([_F_offset, _T_offset])) 119 | self.offset = np.mean(_offsets, axis=0) 120 | # print("computed new offsets") 121 | # for _offset in _offsets: print(_offset) 122 | # print("new offset:", self.offset) 123 | 124 | def load_conf(path:str): 125 | '''load configurations 126 | Params: 127 | ---------- 128 | path: configuration file path 129 | Returns: 130 | ---------- 131 | confs: configurations 132 | ''' 133 | _f = open(path, "r") 134 | _confs = json.load(_f) 135 | confs = [Configuration(_conf) for _conf in _confs] 136 | _f.close() 137 | return confs 138 | 139 | def get_conf_from_dir_name(scene_dir_name:str, confs:list): 140 | ''' 141 | get the Configuration entity given the scene directory name 142 | 143 | Params: 144 | ---------- 145 | scene_dir_name: the given scene directory name 146 | confs: the loaded Configuration entities 147 | parent_folder: the parent folder name of the scene directory 148 | is_action: if the folder is a scene or action 149 | ''' 150 | if scene_dir_name[-1] == "/": scene_dir_name = scene_dir_name[:-1] 151 | scene_dir_name = scene_dir_name.split("/")[-1] 152 | conf_num = 0 153 | if 'cfg_0001' in scene_dir_name: conf_num = 1 154 | elif 'cfg_0002' in scene_dir_name: conf_num = 2 155 | elif 'cfg_0003' in scene_dir_name: conf_num = 3 156 | elif 'cfg_0004' in scene_dir_name: conf_num = 4 157 | elif 'cfg_0005' in scene_dir_name: conf_num = 5 158 | elif 'cfg_0006' in scene_dir_name: conf_num = 6 159 | elif 'cfg_0007' in scene_dir_name: conf_num = 7 160 | else: raise NotImplementedError 161 | for _c in confs: 162 | if _c.conf_num == conf_num: return _c 163 | return None 164 | 165 | def get_conf_by_conf_num(confs:list, conf_num:int): 166 | if conf_num > len(confs): raise ValueError("conf_num is greater than provided confs") 167 | return confs[conf_num - 1] -------------------------------------------------------------------------------- /rh20t_api/convert.py: -------------------------------------------------------------------------------- 1 | """ 2 | String conversion for file names, time, camera serial etc. 3 | """ 4 | 5 | from datetime import datetime 6 | 7 | def img_name_to_timestamp(file_name:str): return int(file_name[:13]) 8 | def timestamp_to_img_name(timestamp:int): return str(timestamp) + ".png" 9 | 10 | def npy_to_timestamp(file_name:str): return int(file_name[:13]) 11 | def timestamp_to_npy(timestamp:int): return str(timestamp) + ".npy" 12 | 13 | def serial_to_cam_dir(serial:str): return "cam_" + serial 14 | def cam_dir_to_serial(cam_dir:str): return cam_dir.replace("cam_", "") 15 | 16 | def timestamp_to_datetime_str(timestamp:int): return datetime.fromtimestamp(timestamp / 1000.0).strftime("%Y-%m-%d %H:%M:%S") -------------------------------------------------------------------------------- /rh20t_api/extract.py: -------------------------------------------------------------------------------- 1 | """ 2 | Scripts and sample usages to convert RH20T to image version. 3 | This script should be executed after unzipped the file if you want to use RH20T_api functions. 4 | """ 5 | import os 6 | import cv2 7 | import time 8 | import numpy as np 9 | from PIL import Image 10 | from multiprocessing import Pool 11 | 12 | 13 | ################################## Convert RH20T to image version ################################## 14 | 15 | def convert_color(color_file, color_timestamps, dest_color_dir): 16 | """ 17 | Args: 18 | - color_file: the color video file; 19 | - color_timestamps: the color timestamps; 20 | - dest_color_dir: the destination color directory. 21 | """ 22 | cap = cv2.VideoCapture(color_file) 23 | cnt = 0 24 | while True: 25 | ret, frame = cap.read() 26 | if ret: 27 | cv2.imwrite(os.path.join(dest_color_dir, '{}.jpg'.format(color_timestamps[cnt])), frame) 28 | cnt += 1 29 | else: 30 | break 31 | cap.release() 32 | 33 | def convert_depth(depth_file, depth_timestamps, dest_depth_dir, size = (1280, 720)): 34 | """ 35 | Args: 36 | - depth_file: the depth video file (special encoded); 37 | - depth_timestamps: the depth timestamps; 38 | - dest_depth_dir: the destination depth directory; 39 | - size: the size of the depth map ( (640, 360) for resized version ). 40 | """ 41 | width, height = size 42 | cap = cv2.VideoCapture(depth_file) 43 | cnt = 0 44 | is_l515 = ("cam_f" in depth_file) 45 | while True: 46 | ret, frame = cap.read() 47 | if ret: 48 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 49 | gray1 = np.array(gray[:height, :]).astype(np.int32) 50 | gray2 = np.array(gray[height:, :]).astype(np.int32) 51 | gray = np.array(gray2 * 256 + gray1).astype(np.uint16) 52 | if is_l515: 53 | gray = gray * 4 54 | cv2.imwrite(os.path.join(dest_depth_dir, '{}.png'.format(depth_timestamps[cnt])), gray) 55 | cnt += 1 56 | else: 57 | break 58 | cap.release() 59 | 60 | def convert_dir(color_file, timestamps_file, dest_dir, depth_file = None, size = (1280, 720)): 61 | """ 62 | Args: 63 | - color_file: the color video file; 64 | - timestamps_file: the timestamps file; 65 | - dest_dir: the destination directory; 66 | - depth_file: the depth video file (special encoded), set to None if no depth usage; 67 | - size: the size of the depth map ( (640, 360) for resized version ). 68 | """ 69 | with_depth = (depth_file is not None) 70 | assert os.path.exists(color_file) 71 | assert os.path.exists(timestamps_file) 72 | if with_depth: 73 | # with depth, but no depth map is generated for the scene due to technical issues. 74 | if not os.path.exists(depth_file): 75 | with_depth = False 76 | meta = np.load(timestamps_file, allow_pickle = True).item() 77 | dest_color_dir = os.path.join(dest_dir, 'color') 78 | if not os.path.exists(dest_color_dir): 79 | os.makedirs(dest_color_dir) 80 | convert_color(color_file = color_file, color_timestamps = meta['color'], dest_color_dir = dest_color_dir) 81 | if with_depth: 82 | dest_depth_dir = os.path.join(dest_dir, 'depth') 83 | if not os.path.exists(dest_depth_dir): 84 | os.makedirs(dest_depth_dir) 85 | convert_depth(depth_file = depth_file, depth_timestamps = meta['depth'], dest_depth_dir = dest_depth_dir, size = size) 86 | 87 | def convert_scene(scene_root_dir, dest_scene_dir, scene_depth_dir = None, size = (1280, 720)): 88 | """ 89 | Args: 90 | - scene_root_dir: the root directory for the current scene; 91 | - dest_scene_dir: the destination root directory for the current scene (set to the same as scene_root_dir to extract into the original directory); 92 | - dest_dir: the destination scene directory; 93 | - depth_file: the depth video file (special encoded), set to None if no depth usage; 94 | - size: the size of the depth map ( (640, 360) for resized version ). 95 | """ 96 | assert os.path.exists(scene_root_dir) 97 | for cam_folder in os.listdir(scene_root_dir): 98 | if "cam_" not in cam_folder: 99 | continue 100 | convert_dir( 101 | color_file = os.path.join(scene_root_dir, cam_folder, 'color.mp4'), 102 | timestamps_file = os.path.join(scene_root_dir, cam_folder, 'timestamps.npy'), 103 | dest_dir = os.path.join(dest_scene_dir, cam_folder), 104 | depth_file = None if scene_depth_dir is None else os.path.join(scene_depth_dir, cam_folder, 'depth.mp4'), 105 | size = size 106 | ) 107 | 108 | def convert_rh20t(root_dir, dest_dir, depth_dir = None, num_workers = 20): 109 | """ 110 | Args: 111 | - root_dir: the root directory for RH20T; 112 | - dest_dir: the destination root directory for RH20T in image version (it can be the same as root_dir); 113 | - depth_dir: the root directory for RH20T depth file; 114 | - num_workers: the number of workers in multiprocessing. 115 | """ 116 | assert os.path.exists(root_dir) 117 | if 'resized' in root_dir: 118 | size = (640, 360) 119 | else: 120 | size = (1280, 720) 121 | with Pool(processes = num_workers) as pool: 122 | for cfg_folder in os.listdir(root_dir): 123 | cfg_root_dir = os.path.join(root_dir, cfg_folder) 124 | dest_cfg_dir = os.path.join(dest_dir, cfg_folder) 125 | cfg_depth_dir = (None if depth_dir is None else os.path.join(depth_dir, cfg_folder)) 126 | for scene_folder in os.listdir(cfg_root_dir): 127 | pool.apply_async(convert_scene, args = ( 128 | os.path.join(cfg_root_dir, scene_folder), 129 | os.path.join(dest_cfg_dir, scene_folder), 130 | None if cfg_depth_dir is None else os.path.join(cfg_depth_dir, scene_folder), 131 | size 132 | )) 133 | pool.close() 134 | pool.join() 135 | 136 | 137 | 138 | ################################## Sample Usage ################################## 139 | 140 | if __name__ == '__main__': 141 | # 1. For full version (or ignore depth_dir if no depth usage) 142 | convert_rh20t( 143 | root_dir = "/path/to/RH20T/", 144 | dest_dir = "/path/to/destination/RH20T/", 145 | depth_dir = "/path/to/RH20T_depth/", 146 | num_workers = 20 147 | ) 148 | """ 149 | # 2. For resized version. 150 | convert_rh20t( 151 | root_dir = "/path/to/RH20T_resized/", 152 | dest_dir = "/path/to/destination/RH20T_resized/", 153 | depth_dir = "/path/to/RH20T_resized_depth/", 154 | num_workers = 20 155 | ) 156 | """ -------------------------------------------------------------------------------- /rh20t_api/online.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | from .configurations import Configuration, tcp_as_q 4 | from .scene import load_dict_npy 5 | from .transforms import calc_base_world_mat, calc_tcp_camera_mat, calc_tcp_world_mat, matrix_2_pose_array_quat, pose_array_euler_2_pose_array_quat, pose_array_quat_2_matrix 6 | 7 | def aligned_tcp_in_base(tcp_base_pose:np.ndarray, conf:Configuration): return matrix_2_pose_array_quat(np.linalg.inv(conf.align_mat_base) @ pose_array_quat_2_matrix(tcp_base_pose) @ conf.align_mat_tcp) 8 | def aligned_tcp_glob_mat(tcp_glob_mat:np.ndarray, conf:Configuration): return tcp_glob_mat @ conf.align_mat_tcp 9 | def aligned_ft_base(ft:np.ndarray, conf:Configuration): 10 | f_cam = (conf.align_mat_base @ np.array([ft[0], ft[1], ft[2], 0.], dtype=np.float64).reshape(4, 1)).reshape(4,) 11 | t_cam = (conf.align_mat_base @ np.array([ft[3], ft[4], ft[5], 0.], dtype=np.float64).reshape(4, 1)).reshape(4,) 12 | return np.array([f_cam[0], f_cam[1], f_cam[2], t_cam[0], t_cam[1], t_cam[2]], dtype=np.float64) 13 | 14 | def ft_base_to_cam(base_cam_mat:np.ndarray, ft_base:np.ndarray): 15 | '''convert force and torque value from base coord to cam coord 16 | Params: 17 | ---------- 18 | base_cam_mat: 4x4 base to cam transformation matrix 19 | ft_base: force (N) and torque (N m) value concatenated, 6d 20 | 21 | Returns: 22 | ---------- 23 | ft_cam: converted force and torque value in cam coord 24 | ''' 25 | assert base_cam_mat.shape == (4, 4) and ft_base.shape == (6,), f"ft_base_to_cam_err, {base_cam_mat.shape} and {ft_base.shape}" 26 | f_cam = (base_cam_mat @ np.array([ft_base[0], ft_base[1], ft_base[2], 0.], dtype=np.float64).reshape(4, 1)).reshape(4,) 27 | t_cam = (base_cam_mat @ np.array([ft_base[3], ft_base[4], ft_base[5], 0.], dtype=np.float64).reshape(4, 1)).reshape(4,) 28 | return np.array([f_cam[0], f_cam[1], f_cam[2], t_cam[0], t_cam[1], t_cam[2]], dtype=np.float64) 29 | 30 | def check_tcp_valid(tcp:np.ndarray): 31 | if True in np.isnan(tcp): return False 32 | for _ in tcp: 33 | if _ != 0: return True 34 | return False 35 | 36 | def raw_force_torque_base(tcp:np.ndarray, force_torque:np.ndarray, conf_info:Configuration): 37 | '''convert raw force and torque value to base coord 38 | Params: 39 | ---------- 40 | tcp: tool cartesian pose in base coord, 7d 41 | force_torque: raw force (N) and torque (N m) value concatenated, 6d 42 | conf_info: configuration information, including zeroing params and tcp dimensions etc. 43 | 44 | Returns: 45 | ---------- 46 | raw_force_torque: the zeroed force and torque value in base coord 47 | ''' 48 | assert tcp.shape == (7,) and force_torque.shape == (6,) 49 | tcp2sensor_rot = conf_info.tcp2sensor_rot 50 | 51 | sensor_base_mat = pose_array_quat_2_matrix(tcp) @ np.linalg.inv(tcp2sensor_rot) 52 | 53 | force = np.squeeze(sensor_base_mat @ np.asarray([force_torque[0], force_torque[1], force_torque[2], 0.]).reshape(4, 1), axis=(1)) 54 | torque = np.squeeze(sensor_base_mat @ np.asarray([force_torque[3], force_torque[4], force_torque[5], 0.]).reshape(4, 1), axis=(1)) 55 | 56 | raw_force_torque = np.concatenate((force[0:3], torque[0:3]), axis=0) 57 | return raw_force_torque 58 | 59 | def zeroed_force_torque_base(tcp:np.ndarray, force_torque:np.ndarray, conf_info:Configuration): 60 | '''zero the force and torque value in base coord 61 | Params: 62 | ---------- 63 | tcp: tool cartesian pose in base coord, 7d 64 | force_torque: raw force (N) and torque (N m) value concatenated, 6d 65 | conf_info: configuration information, including zeroing params and tcp dimensions etc. 66 | 67 | Returns: 68 | ---------- 69 | zeroed_force_torque: the zeroed force and torque value in base coord 70 | ''' 71 | assert tcp.shape == (7,) and force_torque.shape == (6,) 72 | _offset, _centroid_pos, _gravity, tcp2sensor_rot = conf_info.offset, conf_info.centroid, conf_info.gravity, conf_info.tcp2sensor_rot 73 | 74 | zeroed_force_torque = force_torque - _offset 75 | 76 | sensor_base_mat = pose_array_quat_2_matrix(tcp) @ np.linalg.inv(tcp2sensor_rot) 77 | gravity = np.linalg.inv(sensor_base_mat) @ _gravity 78 | gravity = gravity[0:3, :].reshape(3,) 79 | zeroed_force_torque[0:3] -= gravity 80 | gravity_torque = np.cross(_centroid_pos, gravity[0:3]) 81 | zeroed_force_torque[3:6] -= gravity_torque 82 | 83 | force = np.squeeze(sensor_base_mat @ np.asarray([zeroed_force_torque[0], zeroed_force_torque[1], zeroed_force_torque[2], 0.]).reshape(4, 1), axis=(1)) 84 | torque = np.squeeze(sensor_base_mat @ np.asarray([zeroed_force_torque[3], zeroed_force_torque[4], zeroed_force_torque[5], 0.]).reshape(4, 1), axis=(1)) 85 | 86 | zeroed_force_torque = np.concatenate((force[0:3], torque[0:3]), axis=0) 87 | return zeroed_force_torque 88 | 89 | class RH20TOnline: 90 | def __init__(self, calib_path:str, conf:Configuration, cam_to_project:str=""): 91 | """ 92 | Online tcp and ft processor for RH20T 93 | 94 | Params: 95 | ---------- 96 | calib_path: path to the calibration data 97 | conf: the configuration for the online scene 98 | cam_to_project: the cam coord to be projected to 99 | """ 100 | self._conf = conf 101 | 102 | self._intrinsics = load_dict_npy(os.path.join(calib_path, "intrinsics.npy")) 103 | self._extrinsics = load_dict_npy(os.path.join(calib_path, "extrinsics.npy")) 104 | for _k in self._extrinsics: self._extrinsics[_k] = self._extrinsics[_k][0] 105 | self._tcp_calib = np.load(os.path.join(calib_path, "tcp.npy")) 106 | if len(self._tcp_calib) == 6: self._tcp_calib = tcp_as_q(self._tcp_calib) 107 | 108 | self._calc_base_world_mat() 109 | 110 | if cam_to_project not in self._extrinsics or cam_to_project not in self._intrinsics: 111 | raise ValueError("Cannot find given serial number in the calib data") 112 | self._cam_to_project = cam_to_project 113 | 114 | def _calc_base_world_mat(self): self._base_world_mat = calc_base_world_mat(self._extrinsics[self._conf.in_hand_serial[0]], self._tcp_calib, self._conf.tcp_camera_mat) 115 | 116 | @property 117 | def cam_to_project(self): 118 | if self._cam_to_project == "": raise ValueError("The cam serial is not determined yet") 119 | return self._cam_to_project 120 | 121 | @cam_to_project.setter 122 | def cam_to_project(self, serial_number:str): 123 | if serial_number not in self._extrinsics or serial_number not in self._intrinsics: 124 | raise ValueError("Cannot find given serial number in the calib data") 125 | self._cam_to_project = serial_number 126 | 127 | def project_raw_from_robot_sensor(self, raw_data:np.ndarray, is_align:bool=True, is_zero:bool=False): 128 | """ 129 | Project raw data from the robot sensor saved in [cam_serial]/tcp/[timestamp].npy 130 | 131 | Params: 132 | ---------- 133 | raw_data: raw data obtained from the robot sensor, [tcp force torque] 134 | is_align: whether to align tcp and coord to unified tcp and coord, defaults to True 135 | is_zero: whether to zero force and torque, defaults to False 136 | TODO: ONLY IF USING CONFIG 5 YOU SHOULD ENABLE ZERO 137 | 138 | Returns: 139 | ---------- 140 | projected_tcp: 7d np.array 141 | projected_ft: 6d np.array 142 | """ 143 | tcp_robot = self._conf.tcp_preprocessor(raw_data[self._conf.robot_tcp_field[0]:self._conf.robot_tcp_field[1]]) 144 | if not check_tcp_valid(tcp_robot): 145 | raise ValueError("tcp split from the raw data is invalid, containing np.nan or all being zeros") 146 | 147 | ft_robot = raw_data[self._conf.robot_ft_field[0]:self._conf.robot_ft_field[1]] 148 | if is_zero: ft_robot = zeroed_force_torque_base(tcp_robot, ft_robot, self._conf) 149 | tcp_base_q = tcp_as_q(tcp_robot) if len(tcp_robot) == 6 else tcp_robot # all tcp records convert to quaternion 150 | tcp_global_mat = calc_tcp_world_mat(tcp_base_q, base_world_mat=self._base_world_mat) 151 | if is_align: tcp_global_mat = aligned_tcp_glob_mat(tcp_global_mat, self._conf) 152 | tcp_camera_mat = calc_tcp_camera_mat(tcp_global_mat, self._extrinsics[self.cam_to_project]) 153 | base_cam_mat = self._extrinsics[self.cam_to_project] @ self._base_world_mat 154 | if is_align: base_cam_mat = base_cam_mat @ self._conf.align_mat_base 155 | return matrix_2_pose_array_quat(tcp_camera_mat), ft_base_to_cam(base_cam_mat, ft_robot) 156 | 157 | def project_robottcp_from_cameratcp(self, camera_tcp:np.array): 158 | """ 159 | Project camera tcp pose to robot base 160 | 161 | Params: 162 | ---------- 163 | camera_tcp: camera based tcp predicted by the network, 6d np.array, [xyzrpy] 164 | 165 | Returns: 166 | ---------- 167 | robot_tcp: 7d np.array, [xyz quat] 168 | """ 169 | camera_tcp = tcp_as_q(camera_tcp)# all tcp records convert to quaternion 170 | marker_tcp = np.linalg.inv(self._extrinsics[self.cam_to_project]) @ pose_array_quat_2_matrix(camera_tcp) 171 | robot_tcp = np.linalg.inv(self._base_world_mat) @ marker_tcp 172 | 173 | return matrix_2_pose_array_quat(robot_tcp) 174 | 175 | def project_raw_from_external_sensor(self, raw_data:np.array, tcp:np.array, is_align=True, is_zero:bool=True): 176 | """ 177 | Project raw data read from external sensor saved in [cam_serial]/force_torque/[timestamp].npy to a given cam coord 178 | TODO: PLZ DO NOT USE THIS FUNC IN CONFIG 5 179 | 180 | Params: 181 | ---------- 182 | raw_data: raw data obtained from the external sensor, [force torque] 183 | tcp: tcp at the same time, simply raw data with same timestamp from the tcp folder is ok 184 | is_align: whether align to unified tcp and coord 185 | is_zero: whether zero the force and torque 186 | 187 | Returns: 188 | ---------- 189 | projected_ft: 6d np.array 190 | """ 191 | tcp_robot = self._conf.tcp_preprocessor(tcp[self._conf.robot_tcp_field[0]:self._conf.robot_tcp_field[1]]) 192 | if not check_tcp_valid(tcp_robot): 193 | raise ValueError("tcp split from the raw data is invalid, containing np.nan or all being zeros") 194 | 195 | tcp_base_q = tcp_as_q(tcp_robot) if len(tcp_robot) == 6 else tcp_robot # all tcp records convert to quaternion 196 | 197 | base_cam_mat = self._extrinsics[self.cam_to_project] @ self._base_world_mat 198 | if is_align: base_cam_mat = base_cam_mat @ self._conf.align_mat_base 199 | ft_base = zeroed_force_torque_base(tcp_base_q, raw_data, self._conf) if is_zero else raw_force_torque_base(tcp_base_q, raw_data, self._conf) 200 | base_cam_mat = self._extrinsics[self.cam_to_project] @ self._base_world_mat 201 | if is_align: base_cam_mat = base_cam_mat @ self._conf.align_mat_base 202 | return ft_base_to_cam(base_cam_mat, ft_base) 203 | 204 | -------------------------------------------------------------------------------- /rh20t_api/search.py: -------------------------------------------------------------------------------- 1 | """ 2 | Searching functions for interpolations 3 | """ 4 | 5 | import numpy as np 6 | from math import * 7 | 8 | def binary_search_latest_range(arr, l:int, r:int, x): 9 | if arr[r] <= x or r == l: return arr[r] 10 | mid = ((l + r) >> 1) + 1 11 | return binary_search_latest_range(arr, mid, r, x) if arr[mid] <= x else binary_search_latest_range(arr, l, mid - 1, x) 12 | 13 | def binary_search_latest(arr:list, x): 14 | ''' 15 | search for the nearest item in arr just smaller than x, 16 | if no one smaller than x is found, return the smallest 17 | 18 | Params: 19 | ---------- 20 | arr: the array to search on 21 | x: the target value 22 | 23 | Returns: 24 | ---------- 25 | x_t: the closest previous value in arr of x 26 | ''' 27 | if len(arr) <= 0: raise ValueError("input array should contain at least one element") 28 | return binary_search_latest_range(arr, 0, len(arr) - 1, x) 29 | 30 | def interpolate_linear(target_t:int, t1:int, t2:int, x1:np.ndarray, x2:np.ndarray): return (x1 + (target_t - t1) / (t2 - t1) * (x2 - x1) if t1 != t2 else x1) 31 | 32 | def binary_search_closest_two_idx(t:list, target_t:int): 33 | # linearly searches indices of two closest element of target_t in t 34 | # for continuous values (i.e. non-image data) searching 35 | prev_t_idx = t.index(binary_search_latest(t, target_t)) 36 | return (prev_t_idx, (prev_t_idx + 1 if prev_t_idx < len(t) - 1 else prev_t_idx - 1)) 37 | 38 | def binary_search_closest(t:list, target_t:int): 39 | # for image path searching 40 | if target_t in t: return target_t 41 | prev_t_idx = t.index(binary_search_latest(t, target_t)) 42 | if prev_t_idx == len(t) - 1: return t[prev_t_idx] 43 | return t[prev_t_idx] if abs(t[prev_t_idx] - target_t) < abs(t[prev_t_idx + 1] - target_t) else t[prev_t_idx + 1] 44 | 45 | def sort_by_timestamp(_dict_of_list_of_dict): 46 | for _k in _dict_of_list_of_dict: _dict_of_list_of_dict[_k] = sorted(_dict_of_list_of_dict[_k], key=lambda item:item["timestamp"]) -------------------------------------------------------------------------------- /rh20t_api/transforms.py: -------------------------------------------------------------------------------- 1 | """ 2 | Functions related to 3D geometric transformations. 3 | Mainly implements: 4 | 1. transformations between different 3D pose/transformation representations; 5 | 2. relative transformations for robot arm base, world(marker), tcp and fixed camera 6 | 7 | Implemented representations: 8 | - quaternion, 7d 9 | - matrix, 4x4 10 | - axis rotation vector, 6d 11 | - euler angles, 6d 12 | 13 | | pose representation | quaternion | matrix | rotation vector | euler | 14 | |---------------------|------------|--------|-----------------|-------| 15 | | quaternion | - | √ | √ | √ | 16 | | matrix | √ | - | | | 17 | | rotation vector | √ | √ | - | | 18 | | euler | √ | | | - | 19 | """ 20 | 21 | import numpy as np 22 | from transforms3d.euler import euler2quat, quat2euler 23 | from transforms3d.quaternions import quat2mat, mat2quat, axangle2quat, quat2axangle 24 | import cv2 25 | 26 | def pose_array_quat_2_matrix(pose:np.ndarray): 27 | '''transform pose array of quaternion to transformation matrix 28 | 29 | Param: 30 | pose: 7d vector, with t(3d) + q(4d) 31 | ---------- 32 | Return: 33 | mat: 4x4 matrix, with R,T,0,1 form 34 | ''' 35 | if pose.shape != (7,): raise ValueError 36 | mat = quat2mat([pose[3], pose[4], pose[5], pose[6]]) 37 | return np.array([[mat[0][0], mat[0][1], mat[0][2], pose[0]], 38 | [mat[1][0], mat[1][1], mat[1][2], pose[1]], 39 | [mat[2][0], mat[2][1], mat[2][2], pose[2]], 40 | [0,0,0,1]]) 41 | 42 | def matrix_2_pose_array_quat(mat:np.ndarray): 43 | '''transform transformation matrix to pose array of quaternion 44 | 45 | Param: 46 | mat: 4x4 matrix, with R,T,0,1 form 47 | ---------- 48 | Return: 49 | pose: 7d vector, with t(3d) + q(4d) 50 | ''' 51 | if mat.shape != (4, 4): raise ValueError 52 | rotation_mat = np.array([[mat[0][0], mat[0][1], mat[0][2]], 53 | [mat[1][0], mat[1][1], mat[1][2]], 54 | [mat[2][0], mat[2][1], mat[2][2]]]) 55 | q = mat2quat(rotation_mat) 56 | return np.array([mat[0][3], mat[1][3], mat[2][3], q[0], q[1], q[2], q[3]]) 57 | 58 | def pose_array_rot_vec_2_pose_array_quat(rot_vec:np.ndarray): 59 | '''transform pose array of euler to pose array of quaternion 60 | 61 | Param: 62 | rot_vec: 6d vector, with t(3d) + r(3d) 63 | ---------- 64 | Return: 65 | pose: 7d vector, with t(3d) + q(4d) 66 | ''' 67 | if rot_vec.shape != (6,): raise ValueError 68 | q = axangle2quat(rot_vec[3:6], np.linalg.norm(rot_vec[3:6])) 69 | return np.array([rot_vec[0], rot_vec[1], rot_vec[2], q[0], q[1], q[2], q[3]]) 70 | 71 | def pose_array_euler_2_pose_array_quat(euler:np.ndarray): 72 | '''transform pose array of euler to pose array of quaternion 73 | 74 | Param: 75 | euler: 6d vector, with t(3d) + r(3d) 76 | ---------- 77 | Return: 78 | pose: 7d vector, with t(3d) + q(4d) 79 | ''' 80 | if euler.shape != (6,): raise ValueError 81 | q = euler2quat(euler[3], euler[4], euler[5]) 82 | return np.array([euler[0], euler[1], euler[2], q[0], q[1], q[2], q[3]]) 83 | 84 | def pose_array_quat_2_pose_array_rot_vec(pos:np.ndarray): 85 | '''transform pose array of quaternion to pose array of euler 86 | 87 | Param: 88 | pose: 7d vector, with t(3d) + q(4d) 89 | ---------- 90 | Return: 91 | rot_vec: 6d vector, with t(3d) + r(3d) 92 | ''' 93 | if pos.shape != (7,): raise ValueError 94 | theta, rot_vec = quat2axangle(pos[3:7]) 95 | k = theta / np.linalg.norm(rot_vec) 96 | for i, item in enumerate(rot_vec): 97 | rot_vec[i] = item * k 98 | return np.array([pos[0], pos[1], pos[2], rot_vec[0], rot_vec[1], rot_vec[2]]) 99 | 100 | def pose_array_quat_2_pose_array_euler(pos:np.ndarray): 101 | '''transform pose array of quaternion to pose array of euler 102 | 103 | Param: 104 | pose: 7d vector, with t(3d) + q(4d) 105 | ---------- 106 | Return: 107 | euler: 6d vector, with t(3d) + r(3d) 108 | ''' 109 | if pos.shape != (7,): raise ValueError 110 | ai, aj, ak = quat2euler(pos[3:7]) 111 | return np.array([pos[0], pos[1], pos[2], ai, aj, ak]) 112 | 113 | def pose_array_rotvec_2_matrix(pose:np.ndarray): 114 | if pose.shape != (6,): raise ValueError 115 | mat = np.array(cv2.Rodrigues(pose[3:6])[0]).astype(np.float32) 116 | return np.array([ 117 | [mat[0][0], mat[0][1], mat[0][2], pose[0]], 118 | [mat[1][0], mat[1][1], mat[1][2], pose[1]], 119 | [mat[2][0], mat[2][1], mat[2][2], pose[2]], 120 | [0, 0, 0, 1 ] 121 | ]) 122 | 123 | def calc_base_world_mat(world_camera_mat:np.ndarray, tcp_base_pose_quat:np.ndarray, tcp_camera_mat:np.ndarray): 124 | '''calculate the base's pose relative to the marker 125 | 126 | Params: 127 | ---------- 128 | world_camera_mat: a 4x4 matrix, the extrinsic matrix of the in-hand camera 129 | tcp_base_pose_quat: a 7d vector, with t(3d) + q(4d), tcp pose in base coord 130 | tcp_camera_mat: a 4x4 matrix, calibrated before 131 | 132 | Return: 133 | ---------- 134 | base_world_mat: a 4x4 matrix, the base's pose relative to the world 135 | ''' 136 | if world_camera_mat.shape != (4, 4) or tcp_base_pose_quat.shape != (7,) or tcp_camera_mat.shape != (4, 4): raise ValueError 137 | return np.linalg.inv(world_camera_mat) @ tcp_camera_mat @ np.linalg.inv(pose_array_quat_2_matrix(tcp_base_pose_quat)) 138 | 139 | def calc_tcp_world_mat(tcp_base_pose_quat:np.ndarray, base_world_mat:np.ndarray): 140 | """calculate the tcp's pose relative to the marker 141 | Params: 142 | --------- 143 | tcp_base_mat: a 7d vector, with t(3d) + q(4d) 144 | base_world_mat: the base-world matrix obtained from calc_base_world_mat() \ 145 | at calibratin time 146 | 147 | Returns: 148 | ---------- 149 | tcp_world_mat: a 1x4x4 matrix, the tcp's pose relative to the world 150 | """ 151 | if tcp_base_pose_quat.shape != (7,) or base_world_mat.shape != (4, 4): raise ValueError 152 | return base_world_mat @ pose_array_quat_2_matrix(tcp_base_pose_quat) 153 | 154 | def calc_tcp_camera_mat(tcp_world_mat:np.ndarray, camera_extrinsics:np.ndarray): 155 | """calculate the tcp's pose relative to the camera 156 | Params: 157 | ---------- 158 | tcp_world_mat: a 4x4 matrix, tcp's pose relative to world 159 | camera_extrinsics: a 4x4 matrix, camera's extrinsics, in the form of R,T,0,1 160 | 161 | Returns: 162 | ---------- 163 | tcp_camera_mat: a 4x4 matrix, tcp's pose relative to the camera 164 | """ 165 | if tcp_world_mat.shape != (4, 4) or camera_extrinsics.shape != (4, 4): raise ValueError 166 | return camera_extrinsics @ tcp_world_mat -------------------------------------------------------------------------------- /rh20t_api/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities. 3 | """ 4 | 5 | import json 6 | import numpy as np 7 | 8 | def load_json(path:str): 9 | with open(path, "r") as _json_file: _json_content = json.load(_json_file) 10 | return _json_content 11 | 12 | def write_json(path:str, _json_content): 13 | with open(path, "w") as _json_file: json.dump(_json_content, _json_file) 14 | 15 | def load_dict_npy(file_name:str): 16 | """ 17 | Load the dictionary data stored in .npy file. 18 | """ 19 | return np.load(file_name, allow_pickle=True).item() -------------------------------------------------------------------------------- /scripts/test_joint_angle_getter.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append(".") 3 | from rh20t_api.configurations import load_conf 4 | from rh20t_api.scene import RH20TScene 5 | import numpy as np 6 | 7 | def test_joint_angle_getter(): 8 | robot_configs = load_conf("configs/configs.json") 9 | for scene_path in [ 10 | "/aidata/RH20T_resized/RH20T_cfg4/task_0001_user_0010_scene_0001_cfg_0004", 11 | "/aidata/RH20T_resized/RH20T_cfg1/task_0001_user_0001_scene_0001_cfg_0001" 12 | ]: 13 | scene = RH20TScene(scene_path, robot_configs) 14 | start_t = scene.start_timestamp_low_freq 15 | end_t = scene.end_timestamp_low_freq 16 | rnd_t = np.random.randint(start_t + 1, end_t) 17 | for test_t in [start_t, end_t, rnd_t]: 18 | # `RH20TScene.get_joints_angles` has been tested in visualization 19 | # assert np.sum(np.abs(scene.get_joints_angles(test_t) - scene.get_joint_angles_aligned(test_t, serial="base"))) < 1e-9 20 | print(scene.get_joint_angles_aligned(test_t, serial="base")) 21 | 22 | if __name__ == "__main__": 23 | test_joint_angle_getter() 24 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rh20t/rh20t_api/aa3124434729ed622109a29b2cbb9f3bbb1c5eeb/utils/__init__.py -------------------------------------------------------------------------------- /utils/keyboard_listener.py: -------------------------------------------------------------------------------- 1 | from pynput import keyboard 2 | import threading 3 | 4 | class KeyboardListener(threading.Thread): 5 | """ 6 | Customized keyboard listener 7 | """ 8 | def __init__(self): 9 | threading.Thread.__init__(self) 10 | self._pause = False 11 | self._left = 0 12 | self._esc = False 13 | self._terminated = False 14 | self._save = False 15 | self._pcd = True 16 | self._model = True 17 | self._ft = False 18 | 19 | def press_action(self, key): 20 | if self._terminated or key == keyboard.Key.esc: 21 | self._esc = True 22 | return False 23 | if key == keyboard.Key.alt_l or key == keyboard.Key.alt_r: self._pause ^= True 24 | elif key == keyboard.Key.left: self._left += 1 25 | elif key == keyboard.Key.right: self._left -= 1 26 | elif hasattr(key, "char"): 27 | if key.char == 'c' or key.char == 'C': self._save = True 28 | elif key.char == 'p' or key.char == 'P': self._pcd = not self._pcd 29 | elif key.char == 'm' or key.char == 'M': self._model = not self._model 30 | elif key.char == 'f' or key.char == 'F': self._ft = not self._ft 31 | 32 | def release_action(self, key): pass 33 | 34 | def terminate(self): self._terminated = True 35 | 36 | def run(self): 37 | with keyboard.Listener(on_press=self.press_action, on_release=self.release_action) as listener: listener.join() 38 | 39 | @property 40 | def pause(self): return self._pause 41 | 42 | @property 43 | def left(self): return self._left 44 | @left.setter 45 | def left(self, val:int): self._left = val 46 | 47 | @property 48 | def save(self): return self._save 49 | @save.setter 50 | def save(self, val:bool): self._save = val 51 | 52 | @property 53 | def esc(self): return self._esc 54 | 55 | 56 | -------------------------------------------------------------------------------- /utils/logger.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | class CustomFormatter(logging.Formatter): 4 | """Logging Formatter to add colors and count warning / errors""" 5 | 6 | grey = "\x1b[38;21m" 7 | yellow = "\x1b[33;21m" 8 | green = "\x1b[32;21m" 9 | red = "\x1b[31;21m" 10 | bold_red = "\x1b[31;1m" 11 | reset = "\x1b[0m" 12 | format = "[%(name)s] - [%(levelname)s] - %(message)s (%(filename)s:%(lineno)d)" 13 | 14 | FORMATS = { 15 | logging.DEBUG: grey + format + reset, 16 | logging.INFO: green + format + reset, 17 | logging.WARNING: yellow + format + reset, 18 | logging.ERROR: red + format + reset, 19 | logging.CRITICAL: bold_red + format + reset 20 | } 21 | 22 | def format(self, record): 23 | log_fmt = self.FORMATS.get(record.levelno) 24 | formatter = logging.Formatter(log_fmt) 25 | return formatter.format(record) 26 | 27 | def logger_begin(name:str, color:bool=True, level:str=None): 28 | """create a logger 29 | 30 | Args: 31 | name (str): the name for this logger 32 | color (bool, optional): whether to color the log. Defaults to True. 33 | level (str, optional): the logging's level. Defaults to None. 34 | 35 | Returns: 36 | logger (logging.Logger): the created logger 37 | [usage]: 38 | logger.debug("debug message")\n 39 | logger.info("info message")\n 40 | logger.warning("warning message")\n 41 | logger.error("error message")\n 42 | logger.critical("critical message") 43 | """ 44 | # create logger with 'spam_application' 45 | logger:logging.Logger = logging.getLogger(name) 46 | logger.propagate = False 47 | 48 | # create console handler with a higher log level 49 | ch = logging.StreamHandler() 50 | ch.setLevel(logging.DEBUG) 51 | 52 | # set the output format 53 | ch.setFormatter(CustomFormatter() if color else logging.Formatter('%(asctime)s : %(levelname)s : %(name)s : %(message)s')) 54 | 55 | # set the logging level 56 | logger.setLevel(logging.DEBUG if level is None else logging.__getattribute__(level)) 57 | 58 | logger.addHandler(ch) 59 | 60 | return logger 61 | 62 | if __name__ == '__main__': 63 | logger = logger_begin('debug', color=True, level='DEBUG') 64 | logger.debug("debug message") 65 | logger = logger_begin('info', color=True, level='INFO') 66 | logger.info("info message") 67 | logger = logger_begin('warning', color=True, level='WARNING') 68 | logger.warning("warning message") 69 | logger = logger_begin('error', color=True, level='ERROR') 70 | logger.error("error message") 71 | logger = logger_begin('critical', color=True, level='CRITICAL') 72 | logger.critical("critical message") -------------------------------------------------------------------------------- /utils/point_cloud.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | from typing import Any, Dict 4 | 5 | import cv2 6 | import numpy as np 7 | import open3d as o3d 8 | from tqdm import tqdm 9 | 10 | 11 | class PointCloud: 12 | # TODO: L515 13 | def __init__( 14 | self, 15 | logger:logging.Logger, 16 | downsample_voxel_size_m:float=0.0001, 17 | filter_num_neighbor:int=10, 18 | filter_radius_m:float=0.01, 19 | filter_std_ratio:float=2.0, 20 | normal_radius:float=0.01, 21 | normal_num_neighbor:int=30, 22 | min_depth_m:float=0.3, 23 | max_depth_m:float=0.8, 24 | width:int=640, 25 | height:int=360, 26 | pcd_file_format:str=".ply", 27 | debug:bool=True 28 | ): 29 | self.downsample_voxel_size_m = downsample_voxel_size_m 30 | self.filter_num_neighbor = filter_num_neighbor 31 | self.filter_std_ratio = filter_std_ratio 32 | self.normal_radius = normal_radius 33 | self.normal_num_neighbor = normal_num_neighbor 34 | self.min_depth_m = min_depth_m 35 | self.max_depth_m = max_depth_m 36 | self.width = width 37 | self.height = height 38 | self.filter_radius_m = filter_radius_m 39 | self.debug = debug 40 | self.logger = logger 41 | self.pcd_file_format = pcd_file_format 42 | 43 | self.img_names = {} 44 | 45 | def merge_pointclouds(self, pcd1:o3d.geometry.PointCloud, pcd2:o3d.geometry.PointCloud): 46 | merged_points = np.vstack((np.asarray(pcd1.points), np.asarray(pcd2.points))) 47 | merged_colors = np.vstack((np.asarray(pcd1.colors), np.asarray(pcd2.colors))) 48 | 49 | merged_pcd = o3d.geometry.PointCloud() 50 | merged_pcd.points = o3d.utility.Vector3dVector(merged_points) 51 | merged_pcd.colors = o3d.utility.Vector3dVector(merged_colors) 52 | 53 | # merged_pcd = merged_pcd.voxel_down_sample(self.downsample_voxel_size_m) 54 | 55 | _, ind = merged_pcd.remove_statistical_outlier(nb_neighbors=self.filter_num_neighbor, 56 | std_ratio=self.filter_std_ratio) 57 | merged_pcd = merged_pcd.select_by_index(ind) 58 | 59 | merged_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( 60 | radius=self.normal_radius, max_nn=self.normal_num_neighbor)) 61 | 62 | return merged_pcd 63 | 64 | def rgbd_to_pointcloud( 65 | self, 66 | is_l515:bool, 67 | color_image_path:str, 68 | depth_image_path:str, 69 | width:int, 70 | height:int, 71 | intrinsic:np.ndarray, 72 | extrinsic:np.ndarray=np.eye(4), 73 | downsample_factor:float=1 74 | ): 75 | color = cv2.cvtColor(cv2.imread(color_image_path), cv2.COLOR_BGR2RGB) 76 | depth = cv2.imread(depth_image_path, cv2.IMREAD_UNCHANGED).astype(np.float32) 77 | 78 | # downsample image 79 | color = cv2.resize(color, (int(width / downsample_factor), int(height / downsample_factor))).astype(np.int8) 80 | depth = cv2.resize(depth, (int(width / downsample_factor), int(height / downsample_factor))) 81 | 82 | depth /= 4000.0 if is_l515 else 1000.0 # from millimeters to meters 83 | depth[depth < self.min_depth_m] = 0 84 | depth[depth > self.max_depth_m] = 0 85 | 86 | 87 | rgbd_image = o3d.geometry.RGBDImage() 88 | rgbd_image = rgbd_image.create_from_color_and_depth(o3d.geometry.Image(color), 89 | o3d.geometry.Image(depth), depth_scale=1.0, convert_rgb_to_intensity=False) 90 | 91 | intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic() 92 | intrinsic_o3d.set_intrinsics(int(width / downsample_factor), int(height / downsample_factor), 93 | 0.5 * intrinsic[0, 0] / downsample_factor, 0.5 * intrinsic[1, 1] / downsample_factor, 94 | 0.5 * intrinsic[0, 2] / downsample_factor, 0.5 * intrinsic[1, 2] / downsample_factor) 95 | pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic_o3d, extrinsic=extrinsic) 96 | return pcd 97 | 98 | def filter_pointclouds(self, pcds, num_neighbors:int, std_ratio:float, radius): 99 | for i in range(len(pcds)): 100 | _, ind = pcds[i].remove_statistical_outlier(nb_neighbors=num_neighbors, std_ratio=std_ratio) 101 | pcds[i] = pcds[i].select_by_index(ind) 102 | if radius > 0: 103 | _, ind = pcds[i].remove_radius_outlier(nb_points=num_neighbors, radius=radius) 104 | pcds[i] = pcds[i].select_by_index(ind) 105 | 106 | return pcds 107 | 108 | def point_cloud_path(self, write_folder:str, timestamp:int): 109 | return os.path.join(write_folder, str(timestamp) + self.pcd_file_format) 110 | 111 | def point_cloud_single_frame( 112 | self, 113 | image_pairs:dict, 114 | timestamp:int, 115 | in_hand_serials:list, 116 | intrinsics:dict, 117 | extrinsics:dict, 118 | write_folder:str="" 119 | ): 120 | """generate a point cloud according to multiview RGBD image pairs for single frame 121 | 122 | Args: 123 | image_pairs (dict): color and depth image path pairs 124 | timestamp (int): _description_ 125 | in_hand_serials (list): the serial numbers of the in-hand cameras 126 | intrinsics (np.array): dict of 3x4 numpy matrices of intrinsics 127 | extrinsics (np.array): dict of 4x4 numpy matrices of extrinsics 128 | write_folder (str, optional): the path to write the generated point cloud, "" for not to write. Defaults to "". 129 | 130 | Returns: 131 | o3d.geometry.PointCloud: the generated point cloud 132 | """ 133 | # generate point clouds 134 | pcds = [] 135 | for serial in image_pairs: 136 | if serial in in_hand_serials or len(image_pairs[serial]) == 0: 137 | if self.debug: self.logger.debug(f"[PointCloud] Met in-hand camera {serial}") 138 | continue 139 | pcds.append( 140 | self.rgbd_to_pointcloud( 141 | serial[0].isalpha(), 142 | image_pairs[serial][0], 143 | image_pairs[serial][1], 144 | self.width, 145 | self.height, 146 | intrinsics[serial], 147 | extrinsics[serial] 148 | ) 149 | ) 150 | 151 | # merge the clouds and visualize 152 | pcds = [pcd.voxel_down_sample(self.downsample_voxel_size_m) for pcd in pcds] 153 | pcds = self.filter_pointclouds(pcds, self.filter_num_neighbor, self.filter_std_ratio, self.filter_radius_m) 154 | full_pcd = pcds[0] 155 | for i in range(1, len(pcds)): 156 | if i not in [2, 3]: continue 157 | full_pcd = self.merge_pointclouds(pcds[i], full_pcd) 158 | 159 | if self.debug: self.logger.info(f"pcd size: {np.asarray(full_pcd.points).shape}") 160 | 161 | if write_folder != "": 162 | os.makedirs(write_folder, exist_ok=True) 163 | o3d.io.write_point_cloud(self.point_cloud_path(write_folder, timestamp), full_pcd, write_ascii=False) 164 | else: 165 | if self.debug: self.logger.info("write folder path is empty, not writing!") 166 | 167 | _, ind = full_pcd.remove_statistical_outlier(nb_neighbors=self.filter_num_neighbor, 168 | std_ratio=self.filter_std_ratio) 169 | full_pcd.select_by_index(ind) 170 | 171 | return full_pcd 172 | 173 | def point_cloud_multi_frames(self, image_pairs:list, in_hand_serials:list, intrinsics:np.ndarray, extrinsics:np.ndarray, write_folder:str=""): 174 | """generate point clouds according to multiview RGBD image pairs for multiple frames 175 | 176 | Args: 177 | image_pairs (list): color and depth image path pairs 178 | in_hand_serials (list): the serial numbers of the in-hand cameras 179 | intrinsics (np.array): dict of 3x4 numpy matrices of intrinsics 180 | extrinsics (np.array): dict of 4x4 numpy matrices of extrinsics 181 | write_folder (str, optional): the path to write the generated point cloud, "" for not to write. Defaults to "". 182 | 183 | Returns: 184 | o3d.geometry.PointCloud: the generated point clouds if write_folder is "", else None 185 | """ 186 | pcds = [] if write_folder == "" else None 187 | for img_idx, img_pair in enumerate(tqdm(image_pairs)): 188 | _t = img_pair["timestamp"] 189 | del img_pair["timestamp"] 190 | if pcds: 191 | pcds.append( 192 | self.point_cloud_single_frame( 193 | image_pairs=img_pair, 194 | timestamp=_t, 195 | in_hand_serials=in_hand_serials, 196 | intrinsics=intrinsics, 197 | extrinsics=extrinsics, 198 | write_folder=write_folder 199 | ) 200 | ) 201 | else: 202 | self.point_cloud_single_frame( 203 | image_pairs=img_pair, 204 | timestamp=_t, 205 | in_hand_serials=in_hand_serials, 206 | intrinsics=intrinsics, 207 | extrinsics=extrinsics, 208 | write_folder=write_folder 209 | ) 210 | return pcds 211 | 212 | 213 | def create_point_cloud_manager(logger:logging.Logger, vis_cfg:Dict[str, Any]): 214 | """ 215 | Create a point cloud manager given a configuration. 216 | 217 | Params: 218 | ---------- 219 | logger: the point cloud logger 220 | vis_cfg: configuration 221 | 222 | Returns: 223 | ---------- 224 | pointcloud: created point cloud manager 225 | """ 226 | return PointCloud( 227 | logger=logger, 228 | downsample_voxel_size_m=vis_cfg["downsample_voxel_size_m"], 229 | filter_num_neighbor=vis_cfg["filter_num_neighbor"], 230 | filter_radius_m=vis_cfg["filter_radius_m"], 231 | filter_std_ratio=vis_cfg["filter_std_ratio"], 232 | normal_radius=vis_cfg["normal_radius"], 233 | normal_num_neighbor=vis_cfg["normal_num_neighbor"], 234 | min_depth_m=vis_cfg["min_depth_m"], 235 | max_depth_m=vis_cfg["max_depth_m"], 236 | width=vis_cfg["resolution"][0], 237 | height=vis_cfg["resolution"][1], 238 | debug=vis_cfg["debug"] 239 | ) 240 | -------------------------------------------------------------------------------- /utils/robot.py: -------------------------------------------------------------------------------- 1 | import os 2 | from typing import List 3 | 4 | import kinpy as kp 5 | import numpy as np 6 | import open3d as o3d 7 | 8 | 9 | class RobotModel: 10 | def __init__(self, robot_joint_sequence:List[str], robot_urdf:str, robot_mesh:str) -> None: 11 | """Robot model manager. 12 | 13 | Params: 14 | ---------- 15 | robot_joint_sequence: the robot joint name sequence 16 | robot_urdf: the urdf file path for the robot model 17 | robot_mesh: the mesh folder for the robot model 18 | """ 19 | self._robot_joint_sequence = robot_joint_sequence 20 | self._robot_urdf = robot_urdf 21 | self._robot_mesh = robot_mesh 22 | 23 | self._model_chain = kp.build_chain_from_urdf(open(robot_urdf).read().encode('utf-8')) 24 | self._visuals_map = self._model_chain.visuals_map() 25 | 26 | self._model_meshes = {} 27 | self._prev_model_transform = {} 28 | 29 | self._init_buffer() 30 | 31 | def _init_buffer(self): 32 | self._geometries_to_add = [] 33 | self._geometries_to_update = [] 34 | 35 | def update(self, rotates:np.ndarray, first_time:bool): 36 | """ 37 | Update robot model transformations. 38 | 39 | Params: 40 | ---------- 41 | rotates: rotations for each joint 42 | first_time: if it is the first time when geometries 43 | should be added to the visualizer 44 | 45 | Returns: 46 | ---------- 47 | None 48 | """ 49 | self._init_buffer() 50 | transformations = {joint: rotates[i] for i, joint in enumerate(self._robot_joint_sequence)} 51 | cur_transforms = self._model_chain.forward_kinematics(transformations) 52 | for link, transform in cur_transforms.items(): 53 | if first_time: self._model_meshes[link], self._prev_model_transform[link] = {}, {} 54 | for v in self._visuals_map[link]: 55 | if v.geom_param is None: continue 56 | tf = np.dot(transform.matrix(), v.offset.matrix()) 57 | if first_time: 58 | self._model_meshes[link][v.geom_param] = o3d.io.read_triangle_mesh(os.path.join(self._robot_mesh, v.geom_param)) 59 | self._geometries_to_add.append(self._model_meshes[link][v.geom_param]) 60 | else: 61 | self._model_meshes[link][v.geom_param].transform(np.linalg.inv(self._prev_model_transform[link][v.geom_param])) 62 | self._model_meshes[link][v.geom_param].transform(tf) 63 | self._prev_model_transform[link][v.geom_param] = tf 64 | self._model_meshes[link][v.geom_param].compute_vertex_normals() 65 | self._geometries_to_update.append(self._model_meshes[link][v.geom_param]) 66 | 67 | @property 68 | def geometries_to_add(self): return self._geometries_to_add 69 | 70 | @property 71 | def geometries_to_update(self): return self._geometries_to_update -------------------------------------------------------------------------------- /utils/stopwatch.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | 3 | 4 | class Stopwatch: 5 | def __init__(self): self.reset() 6 | 7 | def reset(self): self.start_time = time() 8 | 9 | @property 10 | def split(self): return time() - self.start_time --------------------------------------------------------------------------------