├── .gitignore ├── LICENSE ├── README.md ├── collector ├── collector.py └── collector.sh ├── configs ├── flexiv_exoskeleton_gather_balls.json ├── flexiv_exoskeleton_grasp_from_the_curtained_shelf.json ├── flexiv_teleop_gather_balls.json └── flexiv_teleop_grasp_from_the_curtained_shelf.json ├── exoskeleton ├── README.md ├── configs │ ├── flexiv_left_gather_balls.json │ ├── flexiv_left_grasp_from_the_curtained_shelf.json │ ├── flexiv_right_gather_balls.json │ └── flexiv_right_grasp_from_the_curtained_shelf.json ├── robot │ ├── __init__.py │ ├── api.py │ └── flexiv.py └── teleop.py ├── main.py ├── test_encoder.py ├── test_exoskeleton.py ├── test_exoskeleton_left.py └── test_exoskeleton_right.py /.gitignore: -------------------------------------------------------------------------------- 1 | # User-defined files 2 | .history/ 3 | .DS_Store 4 | */.DS_Store 5 | eval.py 6 | .vscode/ 7 | .wholebody.py 8 | .chkpts/ 9 | 10 | # Byte-compiled / optimized / DLL files 11 | __pycache__/ 12 | *.py[cod] 13 | *$py.class 14 | 15 | # C extensions 16 | *.so 17 | 18 | # Distribution / packaging 19 | .Python 20 | build/ 21 | develop-eggs/ 22 | dist/ 23 | downloads/ 24 | eggs/ 25 | .eggs/ 26 | lib/ 27 | lib64/ 28 | parts/ 29 | sdist/ 30 | var/ 31 | wheels/ 32 | pip-wheel-metadata/ 33 | share/python-wheels/ 34 | *.egg-info/ 35 | .installed.cfg 36 | *.egg 37 | MANIFEST 38 | 39 | # PyInstaller 40 | # Usually these files are written by a python script from a template 41 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 42 | *.manifest 43 | *.spec 44 | 45 | # Installer logs 46 | pip-log.txt 47 | pip-delete-this-directory.txt 48 | 49 | # Unit test / coverage reports 50 | htmlcov/ 51 | .tox/ 52 | .nox/ 53 | .coverage 54 | .coverage.* 55 | .cache 56 | nosetests.xml 57 | coverage.xml 58 | *.cover 59 | *.py,cover 60 | .hypothesis/ 61 | .pytest_cache/ 62 | 63 | # Translations 64 | *.mo 65 | *.pot 66 | 67 | # Django stuff: 68 | *.log 69 | local_settings.py 70 | db.sqlite3 71 | db.sqlite3-journal 72 | 73 | # Flask stuff: 74 | instance/ 75 | .webassets-cache 76 | 77 | # Scrapy stuff: 78 | .scrapy 79 | 80 | # Sphinx documentation 81 | docs/_build/ 82 | 83 | # PyBuilder 84 | target/ 85 | 86 | # Jupyter Notebook 87 | .ipynb_checkpoints 88 | 89 | # IPython 90 | profile_default/ 91 | ipython_config.py 92 | 93 | # pyenv 94 | .python-version 95 | 96 | # pipenv 97 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 98 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 99 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 100 | # install all needed dependencies. 101 | #Pipfile.lock 102 | 103 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 104 | __pypackages__/ 105 | 106 | # Celery stuff 107 | celerybeat-schedule 108 | celerybeat.pid 109 | 110 | # SageMath parsed files 111 | *.sage.py 112 | 113 | # Environments 114 | .env 115 | .venv 116 | env/ 117 | venv/ 118 | ENV/ 119 | env.bak/ 120 | venv.bak/ 121 | 122 | # Spyder project settings 123 | .spyderproject 124 | .spyproject 125 | 126 | # Rope project settings 127 | .ropeproject 128 | 129 | # mkdocs documentation 130 | /site 131 | 132 | # mypy 133 | .mypy_cache/ 134 | .dmypy.json 135 | dmypy.json 136 | 137 | # Pyre type checker 138 | .pyre/ 139 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 SuperExo 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 | # AirExo 2 | 3 | [[Paper]](https://arxiv.org/pdf/2309.14975.pdf) [[Project Page]](https://airexo.github.io/) [[Sample Demonstration Data]](https://drive.google.com/drive/folders/1f_bmrFPep90aUSBj28TdXRiNvHo7PpxR?usp=drive_link) [[ACT with in-the-Wild Learning]](https://github.com/AirExo/act-in-the-wild) 4 | 5 | This repository contains code for teleoperating Flexiv robotic arms using ***AirExo***, as well as code for demonstration data collection, including teleoperated demonstration data and in-the-wild demonstration data. 6 | 7 | ## Requirements 8 | 9 | **Hardware**. ***AirExo***, Intel RealSense D415/D435 camera(s), and two Flexiv Rizon robotic arms. For other types of robots, you can slightly modify the code to adapt them. 10 | 11 | **Python Dependencies**. 12 | 13 | - numpy 14 | - pynput 15 | - argparse 16 | - [easyrobot](https://github.com/galaxies99/easyrobot) 17 | 18 | ## Run 19 | 20 | ### Configurations 21 | 22 | There are 3 types of configuration files in this repository, namely 23 | 24 | - `exoskeleton/configs/flexiv_[left/right]_[task].json`: the configuration files for teleoperation of a single arm with ***AirExo***, including the robot and gripper specifications, as well as joint mapping parameters obtained from calibration. 25 | - `configs/flexiv_teleop_[task].json`: the configuration files for teleoperated demonstration data collection with ***AirExo***, including the encoder specifications, the data to be collected, and the camera configurations. 26 | - `configs/flexiv_exoskeleton_[task].json`: the configuration files for in-the-wild demonstration data collection with ***AirExo***, including the encoder specifications, the data to be collected, and the camera configurations. 27 | 28 | We provide the configuration files for Flexiv robots in our experimental environments. You might need to modify the configurations based on your own settings and calibration results. 29 | 30 | ### Encoders 31 | 32 | Before testing teleoperation, please make sure that your encoders in ***AirExo*** works correctly. You can modify Line 5 in `test_encoder.py` according to your settings and execute the script. The encoder readings should be displayed in the terminal in real time. 33 | 34 | ### Teleoperation 35 | 36 | Use the following command to test the teleoperation function to see if the configurations are set correctly. Before teleoperation begins, the robot will slowly move to the position corresponding to ***AirExo***. After a few seconds, the robot can be controlled in real-time. Please make sure that the operator does not move during the waiting time to prevent unexpected movements of the robot. 37 | 38 | ```bash 39 | python test_exoskeleton.py 40 | ``` 41 | 42 | You can also test the teleoperation function of a single arm using the following commands. 43 | 44 | ```bash 45 | python test_exoskeleton_left.py 46 | python test_exoskeleton_right.py 47 | ``` 48 | 49 | ### Teleoperated Demonstration Data Collection 50 | 51 | Please complete the `collector/collector.sh` file based on your environment first, and we recommend to test the encoder and the teleoperation functions using instructions above before teleoperated demonstration data collection. For teleoperated demonstrations, two Flexiv robotic arms should be connected to the workstation during data collection. 52 | 53 | Then, use the following command for data collection. 54 | 55 | ```bash 56 | python main.py --type teleop --task [Task Name] 57 | ``` 58 | 59 | Here, for `[Task Name]` we support 2 tasks in our paper: `gather_balls` and `grasp_from_the_curtained_shelf`. Before teleoperation, you will be asked to provide task ID, scene ID and user ID (operator ID) respectively. The collected data will be stored under `[Data Path]/task[Task ID]/scene[Scene ID]/` according to the configuration settings, with the format of: 60 | 61 | ```text 62 | meta.json 63 | [Timestamp 1].npy 64 | [Timestamp 2].npy 65 | ... 66 | [Timestamp T].npy 67 | ``` 68 | 69 | where `[Timestamp i]` denote the timestamp for this data record, and `meta.json` stores all meta information, including all valid timestamps, configurations, *etc.* 70 | 71 | ### In-the-Wild Demonstration Data Collection 72 | 73 | Please complete the `collector/collector.sh` file based on your environment first, and we recommend to test the encoder function using instructions above before in-the-wild demonstration data collection. For in-the-wild demonstrations, no robotic arm is needed during data collection. 74 | 75 | Then, use the following command for data collection. 76 | 77 | ```bash 78 | python main.py --type exoskeleton --task [Task Name] 79 | ``` 80 | 81 | Here, for `[Task Name]` we support 2 tasks in our paper: `gather_balls` and `grasp_from_the_curtained_shelf`. Before data collection, you will be asked to provide task ID, scene ID and user ID (operator ID) respectively. The collected data will be stored under `[Data Path]/task[Task ID]_itw/scene[Scene ID]/` according to the configuration settings, with the same format as the teleoperated demonstrations. 82 | 83 | ## Citation 84 | 85 | If you find ***AirExo*** useful in your research, please consider citing the following paper: 86 | 87 | ```bibtex 88 | @article{ 89 | fang2023low, 90 | title = {Low-Cost Exoskeletons for Learning Whole-Arm Manipulation in the Wild}, 91 | author = {Fang, Hongjie and Fang, Hao-Shu and Wang, Yiming and Ren, Jieji and Chen, Jingjing and Zhang, Ruo and Wang, Weiming and Lu, Cewu}, 92 | journal = {arXiv preprint arXiv:2309.14975}, 93 | year = {2023} 94 | } 95 | ``` -------------------------------------------------------------------------------- /collector/collector.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import json 4 | import logging 5 | import argparse 6 | import threading 7 | import numpy as np 8 | from pynput import keyboard 9 | from easydict import EasyDict as edict 10 | from easyrobot.camera.api import get_rgbd_camera 11 | from easyrobot.utils.shared_memory import SharedMemoryManager 12 | 13 | 14 | def to_type(s): 15 | if s == "float": 16 | return np.float64 17 | elif s == "int": 18 | return np.int64 19 | elif s == "uint8": 20 | return np.uint8 21 | elif s == "uint16": 22 | return np.uint16 23 | else: 24 | raise AttributeError("Unsupported type name.") 25 | 26 | 27 | class Collector(object): 28 | """ 29 | Collector. 30 | """ 31 | def __init__(self, cfgs, subpath = None, user = 0, **kwargs): 32 | super(Collector, self).__init__() 33 | # Data path 34 | self.path = cfgs.data_path 35 | if subpath is not None: 36 | self.path = os.path.join(self.path, subpath) 37 | if os.path.exists(self.path) == False: 38 | os.makedirs(self.path) 39 | # Initialize logger 40 | self.cfgs = cfgs 41 | self.logger = logging.getLogger(self.cfgs.get('logger_name', 'Collector')) 42 | # Initialize cameras 43 | self.cameras = [] 44 | for cam in cfgs.cameras: 45 | cam = get_rgbd_camera(**cam) 46 | cam.streaming() 47 | self.cameras.append(cam) 48 | # Initialize data collectors 49 | self.shm_list = cfgs.shm_data.keys() 50 | self.shm_managers = [] 51 | for shm_name in self.shm_list: 52 | self.shm_managers.append( 53 | SharedMemoryManager( 54 | shm_name, 55 | 1, 56 | cfgs.shm_data[shm_name]["shape"], 57 | to_type(cfgs.shm_data[shm_name]["type"]) 58 | ) 59 | ) 60 | self.meta = edict() 61 | self.meta.cfgs = cfgs 62 | self.meta.user = user 63 | # Initialize collection 64 | self.is_collecting = False 65 | 66 | def receive(self): 67 | ''' 68 | Receive the data. 69 | ''' 70 | res = {'time': int(time.time() * 1000)} 71 | for i, shm_name in enumerate(self.shm_list): 72 | res[shm_name] = self.shm_managers[i].execute() 73 | return res 74 | 75 | def save(self): 76 | ''' 77 | Save the collected data to the data path, return the timestamp at collection. 78 | ''' 79 | res = self.receive() 80 | timestamp = res['time'] 81 | 82 | np.save(os.path.join(self.path, '{}.npy'.format(timestamp)), res, allow_pickle = True) 83 | return timestamp 84 | 85 | def start(self, delay_time = 0.0): 86 | ''' 87 | Start collecting data. 88 | 89 | Parameters: 90 | - delay_time: float, optional, default: 0.0, the delay time before collecting data. 91 | ''' 92 | self.thread = threading.Thread(target = self.collecting_thread, kwargs = {'delay_time': delay_time}) 93 | self.thread.setDaemon(True) 94 | self.thread.start() 95 | self.meta.start_time = int(time.time() * 1000) 96 | self.meta.timestamps = [] 97 | 98 | def collecting_thread(self, delay_time = 0.0): 99 | time.sleep(delay_time) 100 | self.is_collecting = True 101 | self.logger.info('[{}] Start collecting data ...'.format(self.cfgs.name)) 102 | while self.is_collecting: 103 | timestamp = self.save() 104 | self.meta.timestamps.append(timestamp) 105 | 106 | def stop(self): 107 | ''' 108 | Stop collecting data. 109 | ''' 110 | self.meta.stop_time = int(time.time() * 1000) 111 | self.is_collecting = False 112 | self.thread.join() 113 | self.logger.info('[{}] Stop collecting data.'.format(self.cfgs.name)) 114 | for cam in self.cameras: 115 | cam.stop_streaming() 116 | 117 | def save_meta(self): 118 | ''' 119 | Save metadata. 120 | ''' 121 | with open(os.path.join(self.path, 'meta.json'), 'w') as f: 122 | json.dump(self.meta, f) 123 | 124 | 125 | if __name__ == '__main__': 126 | parser = argparse.ArgumentParser() 127 | parser.add_argument( 128 | '--cfg', '-c', 129 | default = os.path.join('configs', 'flexiv_teleop.yaml'), 130 | help = 'path to the configuration file', 131 | type = str 132 | ) 133 | parser.add_argument('--task', '-t', default = 0, help = 'task id', type = int) 134 | parser.add_argument('--scene', '-s', default = 0, help = 'scene id', type = int) 135 | parser.add_argument('--user', '-u', default = 0, help = 'user id', type = int) 136 | args = parser.parse_args() 137 | if not os.path.exists(args.cfg): 138 | raise AttributeError('Please provide the configuration file {}.'.format(args.cfg)) 139 | with open(args.cfg, 'r') as f: 140 | cfgs = edict(json.load(f)) 141 | if cfgs.mode == 'exoskeleton': 142 | subpath = os.path.join('task{}_itw'.format(args.task), 'scene{}'.format(args.scene)) 143 | else: 144 | subpath = os.path.join('task{}'.format(args.task), 'scene{}'.format(args.scene)) 145 | collector = Collector(cfgs, subpath, user = args.user) 146 | 147 | has_stop = False 148 | 149 | def _on_press(key): 150 | global has_stop 151 | try: 152 | if key.char == 'q': 153 | if not has_stop: 154 | collector.stop() 155 | has_stop = True 156 | except AttributeError: 157 | pass 158 | 159 | def _on_release(key): 160 | pass 161 | 162 | listener = keyboard.Listener(on_press = _on_press, on_release = _on_release) 163 | collector.start() 164 | listener.start() 165 | while not has_stop: 166 | pass 167 | listener.stop() 168 | collector.save_meta() 169 | -------------------------------------------------------------------------------- /collector/collector.sh: -------------------------------------------------------------------------------- 1 | source [CONDA ACTIVATE SCRIPT] 2 | conda activate [CONDA ENV] 3 | export DISPLAY=:1 4 | cd [PATH TO THE COLLECTOR] 5 | cfg=$1 6 | tid=$2 7 | sid=$3 8 | uid=$4 9 | python collector/collector.py --cfg $cfg --task $tid --scene $sid --user $uid 10 | -------------------------------------------------------------------------------- /configs/flexiv_exoskeleton_gather_balls.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Collector", 3 | "mode": "exoskeleton", 4 | "encoder_left": { 5 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 6 | "port": "/dev/ttyUSB0", 7 | "baudrate": 115200, 8 | "logger_name": "AngleEncoder-left", 9 | "shm_name": "encoder_left", 10 | "streaming_freq": 30 11 | }, 12 | "encoder_right": { 13 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 14 | "port": "/dev/ttyUSB2", 15 | "baudrate": 115200, 16 | "logger_name": "AngleEncoder-right", 17 | "shm_name": "encoder_right", 18 | "streaming_freq": 30 19 | }, 20 | "data_path": "/home/ubuntu/data", 21 | "shm_data": { 22 | "encoder_left": { 23 | "shape": [8], 24 | "type": "float" 25 | }, 26 | "encoder_right": { 27 | "shape": [8], 28 | "type": "float" 29 | }, 30 | "image": { 31 | "shape": [720, 1280, 3], 32 | "type": "uint8" 33 | }, 34 | "depth": { 35 | "shape": [720, 1280], 36 | "type": "float" 37 | } 38 | }, 39 | "cameras": [ 40 | { 41 | "name": "realsense", 42 | "serial": "104122061602", 43 | "frame_rate": 30, 44 | "resolution": [1280, 720], 45 | "align": 1, 46 | "logger_name": "RealSense Camera", 47 | "shm_name_rgb": "image", 48 | "shm_name_depth": "depth", 49 | "streaming_freq": 30 50 | } 51 | ] 52 | } -------------------------------------------------------------------------------- /configs/flexiv_exoskeleton_grasp_from_the_curtained_shelf.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Collector", 3 | "mode": "exoskeleton", 4 | "encoder_left": { 5 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 6 | "port": "/dev/ttyUSB0", 7 | "baudrate": 115200, 8 | "logger_name": "AngleEncoder-left", 9 | "shm_name": "encoder_left", 10 | "streaming_freq": 30 11 | }, 12 | "encoder_right": { 13 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 14 | "port": "/dev/ttyUSB2", 15 | "baudrate": 115200, 16 | "logger_name": "AngleEncoder-right", 17 | "shm_name": "encoder_right", 18 | "streaming_freq": 30 19 | }, 20 | "data_path": "/home/ubuntu/data", 21 | "shm_data": { 22 | "encoder_left": { 23 | "shape": [8], 24 | "type": "float" 25 | }, 26 | "encoder_right": { 27 | "shape": [8], 28 | "type": "float" 29 | }, 30 | "image": { 31 | "shape": [720, 1280, 3], 32 | "type": "uint8" 33 | }, 34 | "depth": { 35 | "shape": [720, 1280], 36 | "type": "float" 37 | }, 38 | "image_up": { 39 | "shape": [720, 1280, 3], 40 | "type": "uint8" 41 | }, 42 | "depth_up": { 43 | "shape": [720, 1280], 44 | "type": "float" 45 | } 46 | }, 47 | "cameras": [ 48 | { 49 | "name": "realsense", 50 | "serial": "104122061602", 51 | "frame_rate": 30, 52 | "resolution": [1280, 720], 53 | "align": 1, 54 | "logger_name": "RealSense Camera", 55 | "shm_name_rgb": "image", 56 | "shm_name_depth": "depth", 57 | "streaming_freq": 30 58 | }, 59 | { 60 | "name": "realsense", 61 | "serial": "104122061850", 62 | "frame_rate": 30, 63 | "resolution": [1280, 720], 64 | "align": 1, 65 | "logger_name": "RealSense Camera", 66 | "shm_name_rgb": "image_up", 67 | "shm_name_depth": "depth_up", 68 | "streaming_freq": 30 69 | } 70 | ] 71 | } -------------------------------------------------------------------------------- /configs/flexiv_teleop_gather_balls.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Collector", 3 | "mode": "teleop", 4 | "config": { 5 | "left": "exoskeleton/configs/flexiv_left_gather_balls.json", 6 | "right": "exoskeleton/configs/flexiv_right_gather_balls.json" 7 | }, 8 | "data_path": "/home/ubuntu/data", 9 | "shm_data": { 10 | "robot_left": { 11 | "shape": [39], 12 | "type": "float" 13 | }, 14 | "gripper_left": { 15 | "shape": [7], 16 | "type": "int" 17 | }, 18 | "robot_right": { 19 | "shape": [39], 20 | "type": "float" 21 | }, 22 | "encoder_left": { 23 | "shape": [8], 24 | "type": "float" 25 | }, 26 | "encoder_right": { 27 | "shape": [8], 28 | "type": "float" 29 | }, 30 | "image": { 31 | "shape": [720, 1280, 3], 32 | "type": "uint8" 33 | }, 34 | "depth": { 35 | "shape": [720, 1280], 36 | "type": "float" 37 | } 38 | }, 39 | "cameras": [ 40 | { 41 | "name": "realsense", 42 | "serial": "104122060811", 43 | "frame_rate": 30, 44 | "resolution": [1280, 720], 45 | "align": 1, 46 | "logger_name": "RealSense Camera", 47 | "shm_name_rgb": "image", 48 | "shm_name_depth": "depth", 49 | "streaming_freq": 30 50 | } 51 | ] 52 | } -------------------------------------------------------------------------------- /configs/flexiv_teleop_grasp_from_the_curtained_shelf.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Collector", 3 | "mode": "teleop", 4 | "config": { 5 | "left": "exoskeleton/configs/flexiv_left_grasp_from_the_curtained_shelf.json", 6 | "right": "exoskeleton/configs/flexiv_right_grasp_from_the_curtained_shelf.json" 7 | }, 8 | "data_path": "/home/ubuntu/data", 9 | "shm_data": { 10 | "robot_left": { 11 | "shape": [39], 12 | "type": "float" 13 | }, 14 | "gripper_left": { 15 | "shape": [7], 16 | "type": "int" 17 | }, 18 | "robot_right": { 19 | "shape": [39], 20 | "type": "float" 21 | }, 22 | "encoder_left": { 23 | "shape": [8], 24 | "type": "float" 25 | }, 26 | "encoder_right": { 27 | "shape": [8], 28 | "type": "float" 29 | }, 30 | "image": { 31 | "shape": [720, 1280, 3], 32 | "type": "uint8" 33 | }, 34 | "depth": { 35 | "shape": [720, 1280], 36 | "type": "float" 37 | }, 38 | "image_up": { 39 | "shape": [720, 1280, 3], 40 | "type": "uint8" 41 | }, 42 | "depth_up": { 43 | "shape": [720, 1280], 44 | "type": "float" 45 | } 46 | }, 47 | "cameras": [ 48 | { 49 | "name": "realsense", 50 | "serial": "104122060811", 51 | "frame_rate": 30, 52 | "resolution": [1280, 720], 53 | "align": 1, 54 | "logger_name": "RealSense Camera", 55 | "shm_name_rgb": "image", 56 | "shm_name_depth": "depth", 57 | "streaming_freq": 30 58 | }, 59 | { 60 | "name": "realsense", 61 | "serial": "104122061018", 62 | "frame_rate": 30, 63 | "resolution": [1280, 720], 64 | "align": 1, 65 | "logger_name": "RealSense Camera", 66 | "shm_name_rgb": "image_up", 67 | "shm_name_depth": "depth_up", 68 | "streaming_freq": 30 69 | } 70 | ] 71 | } -------------------------------------------------------------------------------- /exoskeleton/README.md: -------------------------------------------------------------------------------- 1 | # Exoskeleton Tele-Operation 2 | -------------------------------------------------------------------------------- /exoskeleton/configs/flexiv_left_gather_balls.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "TeleOP-left", 3 | "logger_name": "TeleOP-left", 4 | "freq": 50, 5 | "encoder": { 6 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 7 | "port": "/dev/ttyUSB0", 8 | "baudrate": 115200, 9 | "logger_name": "AngleEncoder-left", 10 | "shm_name": "encoder_left", 11 | "streaming_freq": 30 12 | }, 13 | "robot": { 14 | "name": "flexiv", 15 | "robot_ip_address": "192.168.2.100", 16 | "pc_ip_address": "192.168.2.35", 17 | "gripper": { 18 | "name": "robotiq-2f-85", 19 | "port": "/dev/ttyUSB1", 20 | "logger_name": "gripper-left", 21 | "shm_name": "gripper_left", 22 | "streaming_freq": 30 23 | }, 24 | "logger_name": "flexiv-left", 25 | "shm_name": "robot_left", 26 | "streaming_freq": 30, 27 | "joint_num": 7 28 | }, 29 | "mapping": { 30 | "enc1": { 31 | "scaling": 0, 32 | "encoder_min": 265.5, 33 | "encoder_max": 265, 34 | "encoder_direction": 1, 35 | "encoder_rad": 0, 36 | "robot_min": 159.5, 37 | "robot_max": 200.5, 38 | "robot_direction": -1, 39 | "robot_rad": 1, 40 | "robot_zero_centered": 1, 41 | "encoder_mapping": 20, 42 | "robot_mapping": 0, 43 | "fixed": 0, 44 | "fixed_value": 0, 45 | "initial_value": 330, 46 | "special":0 47 | }, 48 | "enc2": { 49 | "scaling": 0, 50 | "encoder_min": 131.5, 51 | "encoder_max": 131, 52 | "encoder_direction": 1, 53 | "encoder_rad": 0, 54 | "robot_min": 129.5, 55 | "robot_max": 230.5, 56 | "robot_direction": -1, 57 | "robot_rad": 1, 58 | "robot_zero_centered": 1, 59 | "encoder_mapping": 330.8, 60 | "robot_mapping": 0, 61 | "fixed": 0, 62 | "fixed_value": 0, 63 | "initial_value": 269, 64 | "special":0 65 | }, 66 | "enc3": { 67 | "scaling": 0, 68 | "encoder_min": 240.5, 69 | "encoder_max": 240, 70 | "encoder_direction": 1, 71 | "encoder_rad": 0, 72 | "robot_min": 169.5, 73 | "robot_max": 190.5, 74 | "robot_direction": -1, 75 | "robot_rad": 1, 76 | "robot_zero_centered": 1, 77 | "encoder_mapping": 74.7, 78 | "robot_mapping": 0, 79 | "fixed": 0, 80 | "fixed_value": 0, 81 | "initial_value": 250, 82 | "special":0 83 | }, 84 | "enc4": { 85 | "scaling": 0, 86 | "encoder_min": 165, 87 | "encoder_max": 140, 88 | "encoder_direction": 1, 89 | "encoder_rad": 0, 90 | "robot_min": 153.5, 91 | "robot_max": 253.5, 92 | "robot_direction": -1, 93 | "robot_rad": 1, 94 | "robot_zero_centered": 1, 95 | "encoder_mapping": 332.2, 96 | "robot_mapping": 0, 97 | "fixed": 0, 98 | "fixed_value": 0, 99 | "initial_value": 44, 100 | "special":0 101 | }, 102 | "enc5": { 103 | "scaling": 0, 104 | "encoder_min": 200.5, 105 | "encoder_max": 200, 106 | "encoder_direction": 1, 107 | "encoder_rad": 0, 108 | "robot_min": 169.5, 109 | "robot_max": 190.5, 110 | "robot_direction": -1, 111 | "robot_rad": 1, 112 | "robot_zero_centered": 1, 113 | "encoder_mapping": 13.5, 114 | "robot_mapping": 0, 115 | "fixed": 1, 116 | "fixed_value": 32, 117 | "initial_value": 32, 118 | "special":0 119 | }, 120 | "enc6": { 121 | "scaling": 0, 122 | "encoder_min": 15.5, 123 | "encoder_max": 15, 124 | "encoder_direction": 1, 125 | "encoder_rad": 0, 126 | "robot_min": 259.5, 127 | "robot_max": 280.5, 128 | "robot_direction": -1, 129 | "robot_rad": 1, 130 | "robot_zero_centered": 1, 131 | "encoder_mapping": 186.3, 132 | "robot_mapping": 0, 133 | "fixed": 1, 134 | "fixed_value": 90, 135 | "initial_value": 90, 136 | "special":0 137 | }, 138 | "enc7": { 139 | "scaling": 0, 140 | "encoder_min": 293.5, 141 | "encoder_max": 293, 142 | "encoder_direction": 1, 143 | "encoder_rad": 0, 144 | "robot_min": 169.5, 145 | "robot_max": 190.5, 146 | "robot_direction": -1, 147 | "robot_rad": 1, 148 | "robot_zero_centered": 1, 149 | "encoder_mapping": 37.7, 150 | "robot_mapping": 0, 151 | "fixed": 1, 152 | "fixed_value": 80, 153 | "initial_value": 80, 154 | "special":0 155 | }, 156 | "enc8": { 157 | "scaling": 1, 158 | "encoder_min": 50, 159 | "encoder_max": 190, 160 | "encoder_direction": 1, 161 | "encoder_rad": 0, 162 | "robot_min": 255, 163 | "robot_max": 0, 164 | "fixed": 0, 165 | "fixed_value": 0, 166 | "initial_value": 50, 167 | "special":0 168 | } 169 | } 170 | } -------------------------------------------------------------------------------- /exoskeleton/configs/flexiv_left_grasp_from_the_curtained_shelf.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "TeleOP-left", 3 | "logger_name": "TeleOP-left", 4 | "freq": 50, 5 | "encoder": { 6 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 7 | "port": "/dev/ttyUSB0", 8 | "baudrate": 115200, 9 | "logger_name": "AngleEncoder-left", 10 | "shm_name": "encoder_left", 11 | "streaming_freq": 30 12 | }, 13 | "robot": { 14 | "name": "flexiv", 15 | "robot_ip_address": "192.168.2.100", 16 | "pc_ip_address": "192.168.2.35", 17 | "gripper": { 18 | "name": "robotiq-2f-85", 19 | "port": "/dev/ttyUSB1", 20 | "logger_name": "gripper-left", 21 | "shm_name": "gripper_left", 22 | "streaming_freq": 30 23 | }, 24 | "logger_name": "flexiv-left", 25 | "shm_name": "robot_left", 26 | "streaming_freq": 30, 27 | "joint_num": 7 28 | }, 29 | "mapping": { 30 | "enc1": { 31 | "scaling": 0, 32 | "encoder_min": 265.5, 33 | "encoder_max": 265, 34 | "encoder_direction": 1, 35 | "encoder_rad": 0, 36 | "robot_min": 159.5, 37 | "robot_max": 200.5, 38 | "robot_direction": -1, 39 | "robot_rad": 1, 40 | "robot_zero_centered": 1, 41 | "encoder_mapping": 20, 42 | "robot_mapping": 0, 43 | "fixed": 0, 44 | "fixed_value": 0, 45 | "initial_value": 350, 46 | "special":0 47 | }, 48 | "enc2": { 49 | "scaling": 0, 50 | "encoder_min": 131.5, 51 | "encoder_max": 131, 52 | "encoder_direction": 1, 53 | "encoder_rad": 0, 54 | "robot_min": 129.5, 55 | "robot_max": 230.5, 56 | "robot_direction": -1, 57 | "robot_rad": 1, 58 | "robot_zero_centered": 1, 59 | "encoder_mapping": 330.8, 60 | "robot_mapping": 0, 61 | "fixed": 0, 62 | "fixed_value": 0, 63 | "initial_value": 231, 64 | "special":0 65 | }, 66 | "enc3": { 67 | "scaling": 0, 68 | "encoder_min": 240.5, 69 | "encoder_max": 240, 70 | "encoder_direction": 1, 71 | "encoder_rad": 0, 72 | "robot_min": 169.5, 73 | "robot_max": 190.5, 74 | "robot_direction": -1, 75 | "robot_rad": 1, 76 | "robot_zero_centered": 1, 77 | "encoder_mapping": 74.7, 78 | "robot_mapping": 0, 79 | "fixed": 0, 80 | "fixed_value": 0, 81 | "initial_value": 89, 82 | "special":0 83 | }, 84 | "enc4": { 85 | "scaling": 0, 86 | "encoder_min": 165, 87 | "encoder_max": 140, 88 | "encoder_direction": 1, 89 | "encoder_rad": 0, 90 | "robot_min": 50.5, 91 | "robot_max": 253.5, 92 | "robot_direction": -1, 93 | "robot_rad": 1, 94 | "robot_zero_centered": 1, 95 | "encoder_mapping": 332.2, 96 | "robot_mapping": 0, 97 | "fixed": 0, 98 | "fixed_value": 0, 99 | "initial_value": 256, 100 | "special":0 101 | }, 102 | "enc5": { 103 | "scaling": 0, 104 | "encoder_min": 200.5, 105 | "encoder_max": 200, 106 | "encoder_direction": 1, 107 | "encoder_rad": 0, 108 | "robot_min": 169.5, 109 | "robot_max": 190.5, 110 | "robot_direction": -1, 111 | "robot_rad": 1, 112 | "robot_zero_centered": 1, 113 | "encoder_mapping": 13.5, 114 | "robot_mapping": 0, 115 | "fixed": 0, 116 | "fixed_value": 32, 117 | "initial_value": 356, 118 | "special":0 119 | }, 120 | "enc6": { 121 | "scaling": 0, 122 | "encoder_min": 15.5, 123 | "encoder_max": 15, 124 | "encoder_direction": 1, 125 | "encoder_rad": 0, 126 | "robot_min": 259.5, 127 | "robot_max": 280.5, 128 | "robot_direction": -1, 129 | "robot_rad": 1, 130 | "robot_zero_centered": 1, 131 | "encoder_mapping": 186.3, 132 | "robot_mapping": 0, 133 | "fixed": 0, 134 | "fixed_value": 90, 135 | "initial_value": 125, 136 | "special":0 137 | }, 138 | "enc7": { 139 | "scaling": 0, 140 | "encoder_min": 293.5, 141 | "encoder_max": 293, 142 | "encoder_direction": 1, 143 | "encoder_rad": 0, 144 | "robot_min": 169.5, 145 | "robot_max": 190.5, 146 | "robot_direction": -1, 147 | "robot_rad": 1, 148 | "robot_zero_centered": 1, 149 | "encoder_mapping": 2.7, 150 | "robot_mapping": 0, 151 | "fixed": 0, 152 | "fixed_value": 80, 153 | "initial_value": 80, 154 | "special":0 155 | }, 156 | "enc8": { 157 | "scaling": 1, 158 | "encoder_min": 50, 159 | "encoder_max": 190, 160 | "encoder_direction": 1, 161 | "encoder_rad": 0, 162 | "robot_min": 255, 163 | "robot_max": 0, 164 | "fixed": 0, 165 | "fixed_value": 0, 166 | "initial_value": 50, 167 | "special":0 168 | } 169 | } 170 | } -------------------------------------------------------------------------------- /exoskeleton/configs/flexiv_right_gather_balls.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "TeleOP-right", 3 | "logger_name": "TeleOP-right", 4 | "freq": 50, 5 | "encoder": { 6 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 7 | "port": "/dev/ttyUSB2", 8 | "baudrate": 115200, 9 | "logger_name": "AngleEncoder-right", 10 | "shm_name": "encoder_right", 11 | "streaming_freq": 30 12 | }, 13 | "robot": { 14 | "name": "flexiv", 15 | "robot_ip_address": "192.168.3.100", 16 | "pc_ip_address": "192.168.3.35", 17 | "gripper": { 18 | }, 19 | "logger_name": "flexiv-right", 20 | "shm_name": "robot_right", 21 | "streaming_freq": 30, 22 | "joint_num": 7 23 | }, 24 | "mapping": { 25 | "enc1": { 26 | "scaling": 0, 27 | "encoder_min": 339.5, 28 | "encoder_max": 339, 29 | "encoder_direction": 1, 30 | "encoder_rad": 0, 31 | "robot_min": 200.5, 32 | "robot_max": 159.5, 33 | "robot_direction": 1, 34 | "robot_rad": 1, 35 | "robot_zero_centered": 1, 36 | "encoder_mapping": 51, 37 | "robot_mapping": 0, 38 | "fixed": 0, 39 | "fixed_value": 0, 40 | "initial_value": 322, 41 | "special":0 42 | }, 43 | "enc2": { 44 | "scaling": 0, 45 | "encoder_min": 148.5, 46 | "encoder_max": 148, 47 | "encoder_direction": 1, 48 | "encoder_rad": 0, 49 | "robot_min": 230.5, 50 | "robot_max": 129.5, 51 | "robot_direction": 1, 52 | "robot_rad": 1, 53 | "robot_zero_centered": 1, 54 | "encoder_mapping": 296.5, 55 | "robot_mapping": 0, 56 | "fixed": 0, 57 | "fixed_value": 0, 58 | "initial_value": 263, 59 | "special":0 60 | }, 61 | "enc3": { 62 | "scaling": 0, 63 | "encoder_min": 85.5, 64 | "encoder_max": 85, 65 | "encoder_direction": 1, 66 | "encoder_rad": 0, 67 | "robot_min": 190.5, 68 | "robot_max": 169.5, 69 | "robot_direction": 1, 70 | "robot_rad": 1, 71 | "robot_zero_centered": 1, 72 | "encoder_mapping": 270, 73 | "robot_mapping": 0, 74 | "fixed": 0, 75 | "fixed_value": 0, 76 | "initial_value": 251, 77 | "special":0 78 | }, 79 | "enc4": { 80 | "scaling": 0, 81 | "encoder_min": 165, 82 | "encoder_max": 140, 83 | "encoder_direction": 1, 84 | "encoder_rad": 0, 85 | "robot_min": 253.5, 86 | "robot_max": 153.5, 87 | "robot_direction": 1, 88 | "robot_rad": 1, 89 | "robot_zero_centered": 1, 90 | "encoder_mapping": 336.7, 91 | "robot_mapping": 0, 92 | "fixed": 0, 93 | "fixed_value": 0, 94 | "initial_value": 27, 95 | "special":0 96 | }, 97 | "enc5": { 98 | "scaling": 0, 99 | "encoder_min": 75.5, 100 | "encoder_max": 75, 101 | "encoder_direction": 1, 102 | "encoder_rad": 0, 103 | "robot_min": 190.5, 104 | "robot_max": 169.5, 105 | "robot_direction": 1, 106 | "robot_rad": 1, 107 | "robot_zero_centered": 1, 108 | "encoder_mapping": 197, 109 | "robot_mapping": 0, 110 | "fixed": 1, 111 | "fixed_value": 36, 112 | "initial_value": 36, 113 | "special":0 114 | }, 115 | "enc6": { 116 | "scaling": 0, 117 | "encoder_min": 317.5, 118 | "encoder_max": 317, 119 | "encoder_direction": 1, 120 | "encoder_rad": 0, 121 | "robot_min": 280.5, 122 | "robot_max": 259.5, 123 | "robot_direction": 1, 124 | "robot_rad": 1, 125 | "robot_zero_centered": 1, 126 | "encoder_mapping": 17.3, 127 | "robot_mapping": 0, 128 | "fixed": 1, 129 | "fixed_value": 259, 130 | "initial_value": 259, 131 | "special":1 132 | }, 133 | "enc7": { 134 | "scaling": 0, 135 | "encoder_min": 159.5, 136 | "encoder_max": 159, 137 | "encoder_direction": 1, 138 | "encoder_rad": 0, 139 | "robot_min": 190.5, 140 | "robot_max": 169.5, 141 | "robot_direction": 1, 142 | "robot_rad": 1, 143 | "robot_zero_centered": 1, 144 | "encoder_mapping": 308, 145 | "robot_mapping": 45, 146 | "fixed": 1, 147 | "fixed_value": 0, 148 | "initial_value": 0, 149 | "special":0 150 | }, 151 | "enc8": { 152 | "scaling": 1, 153 | "encoder_min": 220, 154 | "encoder_max": 30, 155 | "encoder_direction": 1, 156 | "encoder_rad": 0, 157 | "robot_min": 255, 158 | "robot_max": 0, 159 | "fixed": 0, 160 | "fixed_value": 0, 161 | "initial_value": 30, 162 | "special":0 163 | } 164 | } 165 | } -------------------------------------------------------------------------------- /exoskeleton/configs/flexiv_right_grasp_from_the_curtained_shelf.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "TeleOP-right", 3 | "logger_name": "TeleOP-right", 4 | "freq": 50, 5 | "encoder": { 6 | "ids": [1, 2, 3, 4, 5, 6, 7, 8], 7 | "port": "/dev/ttyUSB2", 8 | "baudrate": 115200, 9 | "logger_name": "AngleEncoder-right", 10 | "shm_name": "encoder_right", 11 | "streaming_freq": 30 12 | }, 13 | "robot": { 14 | "name": "flexiv", 15 | "robot_ip_address": "192.168.3.100", 16 | "pc_ip_address": "192.168.3.35", 17 | "gripper": { 18 | }, 19 | "logger_name": "flexiv-right", 20 | "shm_name": "robot_right", 21 | "streaming_freq": 30, 22 | "joint_num": 7 23 | }, 24 | "mapping": { 25 | "enc1": { 26 | "scaling": 0, 27 | "encoder_min": 339.5, 28 | "encoder_max": 339, 29 | "encoder_direction": 1, 30 | "encoder_rad": 0, 31 | "robot_min": 200.5, 32 | "robot_max": 159.5, 33 | "robot_direction": 1, 34 | "robot_rad": 1, 35 | "robot_zero_centered": 1, 36 | "encoder_mapping": 51, 37 | "robot_mapping": 0, 38 | "fixed": 0, 39 | "fixed_value": 0, 40 | "initial_value": 350, 41 | "special":0 42 | }, 43 | "enc2": { 44 | "scaling": 0, 45 | "encoder_min": 148.5, 46 | "encoder_max": 148, 47 | "encoder_direction": 1, 48 | "encoder_rad": 0, 49 | "robot_min": 230.5, 50 | "robot_max": 129.5, 51 | "robot_direction": 1, 52 | "robot_rad": 1, 53 | "robot_zero_centered": 1, 54 | "encoder_mapping": 296.5, 55 | "robot_mapping": 0, 56 | "fixed": 0, 57 | "fixed_value": 0, 58 | "initial_value": 231, 59 | "special":0 60 | }, 61 | "enc3": { 62 | "scaling": 0, 63 | "encoder_min": 85.5, 64 | "encoder_max": 85, 65 | "encoder_direction": 1, 66 | "encoder_rad": 0, 67 | "robot_min": 190.5, 68 | "robot_max": 169.5, 69 | "robot_direction": 1, 70 | "robot_rad": 1, 71 | "robot_zero_centered": 1, 72 | "encoder_mapping": 270, 73 | "robot_mapping": 0, 74 | "fixed": 0, 75 | "fixed_value": 0, 76 | "initial_value": 89, 77 | "special":0 78 | }, 79 | "enc4": { 80 | "scaling": 0, 81 | "encoder_min": 165, 82 | "encoder_max": 140, 83 | "encoder_direction": 1, 84 | "encoder_rad": 0, 85 | "robot_min": 253.5, 86 | "robot_max": 153.5, 87 | "robot_direction": 1, 88 | "robot_rad": 1, 89 | "robot_zero_centered": 1, 90 | "encoder_mapping": 336.7, 91 | "robot_mapping": 0, 92 | "fixed": 0, 93 | "fixed_value": 0, 94 | "initial_value": 256, 95 | "special":0 96 | }, 97 | "enc5": { 98 | "scaling": 0, 99 | "encoder_min": 75.5, 100 | "encoder_max": 75, 101 | "encoder_direction": 1, 102 | "encoder_rad": 0, 103 | "robot_min": 190.5, 104 | "robot_max": 169.5, 105 | "robot_direction": 1, 106 | "robot_rad": 1, 107 | "robot_zero_centered": 1, 108 | "encoder_mapping": 287, 109 | "robot_mapping": 0, 110 | "fixed": 0, 111 | "fixed_value": 356, 112 | "initial_value": 356, 113 | "special":0 114 | }, 115 | "enc6": { 116 | "scaling": 0, 117 | "encoder_min": 317.5, 118 | "encoder_max": 317, 119 | "encoder_direction": 1, 120 | "encoder_rad": 0, 121 | "robot_min": 280.5, 122 | "robot_max": 259.5, 123 | "robot_direction": 1, 124 | "robot_rad": 1, 125 | "robot_zero_centered": 1, 126 | "encoder_mapping": 284.3, 127 | "robot_mapping": 0, 128 | "fixed": 0, 129 | "fixed_value": 253, 130 | "initial_value": 253, 131 | "special":1 132 | }, 133 | "enc7": { 134 | "scaling": 0, 135 | "encoder_min": 159.5, 136 | "encoder_max": 159, 137 | "encoder_direction": 1, 138 | "encoder_rad": 0, 139 | "robot_min": 190.5, 140 | "robot_max": 169.5, 141 | "robot_direction": 1, 142 | "robot_rad": 1, 143 | "robot_zero_centered": 1, 144 | "encoder_mapping": 308, 145 | "robot_mapping": 45, 146 | "fixed": 0, 147 | "fixed_value": 0, 148 | "initial_value": 0, 149 | "special":0 150 | }, 151 | "enc8": { 152 | "scaling": 1, 153 | "encoder_min": 220, 154 | "encoder_max": 30, 155 | "encoder_direction": 1, 156 | "encoder_rad": 0, 157 | "robot_min": 255, 158 | "robot_max": 0, 159 | "fixed": 0, 160 | "fixed_value": 0, 161 | "initial_value": 30, 162 | "special":0 163 | } 164 | } 165 | } -------------------------------------------------------------------------------- /exoskeleton/robot/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AirExo/collector/21e06b142eecbe2f0b3f6a3ddd9a056dc5ce6071/exoskeleton/robot/__init__.py -------------------------------------------------------------------------------- /exoskeleton/robot/api.py: -------------------------------------------------------------------------------- 1 | import re 2 | from easyrobot.robot.base import RobotBase 3 | from exoskeleton.robot.flexiv import FlexivInterface 4 | 5 | 6 | def get_robot(**params): 7 | name = params.get('name', None) 8 | try: 9 | if re.fullmatch('[ -_]*flexiv[ -_]*', str.lower(name)): 10 | return FlexivInterface(**params) 11 | else: 12 | return RobotBase(**params) 13 | except Exception: 14 | return RobotBase(**params) 15 | -------------------------------------------------------------------------------- /exoskeleton/robot/flexiv.py: -------------------------------------------------------------------------------- 1 | """ 2 | Modified flexiv robot interface for exoskeleton tele-operation. 3 | """ 4 | from easyrobot.robot.flexiv import FlexivRobot 5 | 6 | 7 | class FlexivInterface(FlexivRobot): 8 | """ 9 | Modified Flexiv Robot Interface. 10 | """ 11 | def __init__( 12 | self, 13 | robot_ip_address, 14 | pc_ip_address, 15 | gripper = {}, 16 | with_streaming = False, 17 | streaming_freq = 30, 18 | shm_name = "robot", 19 | joint_num = 7, 20 | min_joint_diff = 0.01, 21 | **kwargs 22 | ): 23 | super(FlexivInterface, self).__init__( 24 | robot_ip_address = robot_ip_address, 25 | pc_ip_address = pc_ip_address, 26 | gripper = gripper, 27 | with_streaming = with_streaming, 28 | streaming_freq = streaming_freq, 29 | shm_name = shm_name, 30 | **kwargs 31 | ) 32 | self.joint_num = joint_num 33 | self.prev_pos = [0.0] * self.DOF 34 | self.min_joint_diff = min_joint_diff 35 | 36 | def action(self, action_dict, wait = False): 37 | ''' 38 | Perform action in the given action dict. 39 | 40 | Parameters: 41 | - wait: whether to wait until the robot reach the target position. 42 | ''' 43 | joint_pos = [] 44 | target_vel = action_dict.get('target_joint_vel', [0.0] * self.DOF) 45 | target_acc = action_dict.get('target_joint_acc', [0.0] * self.DOF) 46 | max_vel = action_dict.get('max_joint_vel', [2.0] * self.DOF) 47 | max_acc = action_dict.get('max_joint_acc', [3.0] * self.DOF) 48 | joint_pos = action_dict["enc"][:self.joint_num] 49 | 50 | if any(abs(self.prev_pos[i] - joint_pos[i]) > self.min_joint_diff for i in range(self.joint_num)): 51 | joint_pos = joint_pos 52 | else: 53 | joint_pos = self.prev_pos 54 | self.prev_pos = joint_pos 55 | 56 | self.send_joint_pos(joint_pos, wait, target_vel, target_acc, max_vel, max_acc) 57 | gripper_action = action_dict["enc"][self.joint_num:] 58 | self.gripper_action(*gripper_action) -------------------------------------------------------------------------------- /exoskeleton/teleop.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import json 4 | import logging 5 | import threading 6 | from easydict import EasyDict as edict 7 | from easyrobot.utils.transforms.degree import * 8 | from easyrobot.utils.logger import ColoredLogger 9 | from easyrobot.encoder.angle import AngleEncoder 10 | from exoskeleton.robot.api import get_robot 11 | 12 | 13 | class SingleArmTeleOperator(object): 14 | ''' 15 | Tele-operator for single robot arm. 16 | ''' 17 | def __init__(self, cfgs): 18 | ''' 19 | Initialization. 20 | 21 | Parameters: 22 | - cfgs: the path to the configuration file. 23 | ''' 24 | super(SingleArmTeleOperator, self).__init__() 25 | if not os.path.exists(cfgs): 26 | raise AttributeError('Please provide the configuration file {}.'.format(cfgs)) 27 | with open(cfgs, 'r') as f: 28 | self.cfgs = edict(json.load(f)) 29 | logging.setLoggerClass(ColoredLogger) 30 | self.logger = logging.getLogger(self.cfgs.get('logger_name', 'TeleOP')) 31 | self.action_freq = self.cfgs.freq 32 | self.is_teleop = False 33 | if not self.cfgs.encoder: 34 | self.ids = [1, 2, 3, 4, 5, 6, 7, 8] 35 | self.encoder = None 36 | else: 37 | self.encoder = AngleEncoder(**self.cfgs.encoder) 38 | self.ids = self.encoder.ids 39 | self.encoder.streaming() 40 | self.num_ids = len(self.ids) 41 | self.robot = get_robot(**self.cfgs.robot) 42 | self.robot.streaming() 43 | 44 | def mapping(self, enc, x, num): 45 | scaling = self.cfgs.mapping[enc].scaling 46 | emin = self.cfgs.mapping[enc].encoder_min 47 | emax = self.cfgs.mapping[enc].encoder_max 48 | edir = self.cfgs.mapping[enc].encoder_direction 49 | erad = self.cfgs.mapping[enc].encoder_rad 50 | rmin = self.cfgs.mapping[enc].robot_min 51 | rmax = self.cfgs.mapping[enc].robot_max 52 | if erad: 53 | x = rad_2_deg(x) 54 | x = deg_clip_in_range(x, emin, emax, edir) 55 | if scaling: 56 | return deg_percentile(x, emin, emax, edir) * (rmax - rmin) + rmin 57 | rdir = self.cfgs.mapping[enc].robot_direction 58 | me, mr = self.cfgs.mapping[enc].encoder_mapping, self.cfgs.mapping[enc].robot_mapping 59 | rrad = self.cfgs.mapping[enc].robot_rad 60 | rzc = self.cfgs.mapping[enc].robot_zero_centered 61 | fixed = self.cfgs.mapping[enc].fixed 62 | x = deg_clip_in_range(mr + deg_distance(me, x, edir) * rdir, rmin, rmax, rdir) 63 | if fixed: 64 | x = self.cfgs.mapping[enc].fixed_value 65 | if rzc: 66 | x = deg_zero_centered(x, rmin, rmax, rdir) 67 | if rrad: 68 | x = deg_2_rad(x) 69 | return x 70 | 71 | def transform_action(self, enc_res): 72 | ''' 73 | Transform the action in encoder field into robot field. 74 | ''' 75 | action_dict = {} 76 | action = [] 77 | for i in range(self.num_ids): 78 | rad = self.mapping("enc{}".format(self.ids[i]), enc_res[i], i) 79 | action.append(rad) 80 | action_dict["enc"] = action 81 | return action_dict 82 | 83 | def initialization(self): 84 | self.logger.info('[TeleOP-{}] Initialization ... Please remain still.'.format(self.cfgs.name)) 85 | action_dict = {} 86 | action = [] 87 | for i in range(self.num_ids): 88 | enc = "enc{}".format(self.ids[i]) 89 | x = self.cfgs.mapping[enc].initial_value 90 | scaling = self.cfgs.mapping[enc].scaling 91 | if scaling: 92 | action.append(x) 93 | continue 94 | rmin = self.cfgs.mapping[enc].robot_min 95 | rmax = self.cfgs.mapping[enc].robot_max 96 | rdir = self.cfgs.mapping[enc].robot_direction 97 | rrad = self.cfgs.mapping[enc].robot_rad 98 | rzc = self.cfgs.mapping[enc].robot_zero_centered 99 | if rzc: 100 | x = deg_zero_centered(x, rmin, rmax, rdir) 101 | if rrad: 102 | x = deg_2_rad(x) 103 | action.append(x) 104 | action_dict["enc"] = action 105 | action_dict["max_joint_vel"] = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5] 106 | action_dict["max_joint_acc"] = [0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3] 107 | self.robot.action(action_dict, wait = True) 108 | self.logger.info('[TeleOP-{}] Finish initialization.'.format(self.cfgs.name)) 109 | 110 | 111 | def calibration(self): 112 | ''' 113 | Calibrate the robot with the exoskeleton when starting tele-operation. 114 | ''' 115 | self.logger.info('[TeleOP-{}] Initially calibrate ... Please remain still.'.format(self.cfgs.name)) 116 | enc_res = self.encoder.fetch_info() 117 | action_dict = self.transform_action(enc_res) 118 | action_dict["max_joint_vel"] = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] 119 | action_dict["max_joint_acc"] = [0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3] 120 | self.robot.action(action_dict, wait = True) 121 | self.logger.info('[TeleOP-{}] Finish initial calibration.'.format(self.cfgs.name)) 122 | 123 | def start(self, delay_time = 0.0): 124 | ''' 125 | Start tele-operation. 126 | 127 | Parameters: 128 | - delay_time: float, optional, default: 0.0, the delay time before collecting data. 129 | ''' 130 | self.thread = threading.Thread(target = self.teleop_thread, kwargs = {'delay_time': delay_time}) 131 | self.thread.setDaemon(True) 132 | self.thread.start() 133 | 134 | def teleop_thread(self, delay_time = 0.0): 135 | time.sleep(delay_time) 136 | self.is_teleop = True 137 | self.logger.info('[{}] Start tele-operation ...'.format(self.cfgs.name)) 138 | while self.is_teleop: 139 | enc_res = self.encoder.fetch_info() 140 | action_dict = self.transform_action(enc_res) 141 | self.robot.action(action_dict) 142 | time.sleep(1.0 / self.action_freq) 143 | 144 | def stop(self): 145 | ''' 146 | Stop tele-operation process. 147 | ''' 148 | self.is_teleop = False 149 | if self.thread: 150 | self.thread.join() 151 | self.logger.info('[{}] Stop tele-operation.'.format(self.cfgs.name)) 152 | if self.encoder: 153 | self.encoder.stop_streaming() 154 | self.robot.stop_streaming() 155 | self.robot.stop() 156 | 157 | 158 | class DualArmTeleOperator(object): 159 | ''' 160 | Tele-operator for dual robot arm. 161 | ''' 162 | def __init__(self, cfgs_left, cfgs_right): 163 | ''' 164 | Initialization. 165 | 166 | Parameters: 167 | - cfgs_left: the path to the configuration file of the left arm; 168 | - cfgs_right: the path to the configuration file of the right arm. 169 | ''' 170 | super(DualArmTeleOperator, self).__init__() 171 | self.op_left = SingleArmTeleOperator(cfgs_left) 172 | self.op_right = SingleArmTeleOperator(cfgs_right) 173 | 174 | def initialization(self): 175 | self.op_left.initialization() 176 | self.op_right.initialization() 177 | return 178 | 179 | def calibration(self): 180 | ''' 181 | Calibrate the robot with the exoskeleton when starting tele-operation. 182 | ''' 183 | self.op_left.calibration() 184 | self.op_right.calibration() 185 | return 186 | 187 | def start(self, delay_time = 0.0): 188 | ''' 189 | Start tele-operation. 190 | 191 | Parameters: 192 | - delay_time: float, optional, default: 0.0, the delay time before collecting data. 193 | ''' 194 | self.op_left.start(delay_time) 195 | self.op_right.start(delay_time) 196 | 197 | def stop(self): 198 | ''' 199 | Stop the tele-operation process. 200 | ''' 201 | self.op_left.stop() 202 | self.op_right.stop() 203 | 204 | 205 | if __name__ == '__main__': 206 | op = SingleArmTeleOperator('configs/flexiv_left_gather_balls.json') 207 | op.calibration() 208 | op.start() 209 | time.sleep(10) 210 | op.stop() 211 | 212 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import json 4 | import argparse 5 | from pynput import keyboard 6 | from easydict import EasyDict as edict 7 | from easyrobot.encoder.angle import AngleEncoder 8 | from exoskeleton.teleop import DualArmTeleOperator 9 | 10 | 11 | if __name__ == '__main__': 12 | os.system("kill -9 `ps -ef | grep collector | grep -v grep | awk '{print $2}'`") 13 | os.system('rm -f /dev/shm/*') 14 | os.system('udevadm trigger') 15 | 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument( 18 | '--task', '-t', 19 | default = 'gather_balls', 20 | help = 'task name', 21 | type = str, 22 | choices = ['gather_balls', 'grasp_from_the_curtained_shelf'] 23 | ) 24 | parser.add_argument( 25 | '--type', 26 | default = 'teleop', 27 | help = 'type of demonstration collection', 28 | type = str, 29 | choices = ['teleop', 'exoskeleton'] 30 | ) 31 | args = parser.parse_args() 32 | 33 | run_path = 'configs/flexiv_' + str(args.type) + '_' + str(args.task) + '.json' 34 | 35 | if not os.path.exists(run_path): 36 | raise AttributeError('Please provide the configuration file {}.'.format(run_path)) 37 | with open(run_path, 'r') as f: 38 | cfgs = edict(json.load(f)) 39 | 40 | tid = int(input('Task ID: ')) 41 | sid = int(input('Scene ID: ')) 42 | uid = int(input('User ID: ')) 43 | 44 | if cfgs.mode == 'teleop': 45 | op = DualArmTeleOperator(cfgs.config.left, cfgs.config.right) 46 | op.initialization() 47 | time.sleep(2) 48 | op.calibration() 49 | 50 | has_start = False 51 | has_stop = False 52 | has_record = False 53 | 54 | def _on_press(key): 55 | global has_start 56 | global has_stop 57 | global has_record 58 | try: 59 | if key.char == 's': 60 | if not has_start: 61 | op.start() 62 | has_start = True 63 | else: 64 | pass 65 | if key.char == 'q': 66 | if not has_start: 67 | pass 68 | else: 69 | op.stop() 70 | op.initialization() 71 | time.sleep(2) 72 | has_stop = True 73 | if key.char == 'r': 74 | if not has_record: 75 | os.system('bash collector/collector.sh {} {} {} {} &'.format(run_path, tid, sid, uid)) 76 | time.sleep(1) 77 | has_record = True 78 | else: 79 | pass 80 | except Exception as e: 81 | print(e) 82 | pass 83 | 84 | def _on_release(key): 85 | pass 86 | 87 | listener = keyboard.Listener(on_press = _on_press, on_release = _on_release) 88 | listener.start() 89 | while not has_start: 90 | time.sleep(0.5) 91 | while not has_stop: 92 | time.sleep(0.5) 93 | listener.stop() 94 | elif cfgs.mode == 'exoskeleton': 95 | encoder_left = AngleEncoder(**cfgs.encoder_left) 96 | encoder_left.streaming() 97 | encoder_right = AngleEncoder(**cfgs.encoder_right) 98 | encoder_right.streaming() 99 | has_start = False 100 | has_stop = False 101 | 102 | def _on_press(key): 103 | global has_start 104 | global has_stop 105 | try: 106 | if key.char == 'q': 107 | if not has_start: 108 | pass 109 | else: 110 | encoder_right.stop_streaming() 111 | encoder_left.stop_streaming() 112 | has_stop = True 113 | if key.char == 's': 114 | if not has_start: 115 | os.system('bash collector/collector.sh {} {} {} {} &'.format(run_path, tid, sid, uid)) 116 | time.sleep(3) 117 | has_start = True 118 | else: 119 | pass 120 | except AttributeError: 121 | pass 122 | 123 | def _on_release(key): 124 | pass 125 | 126 | listener = keyboard.Listener(on_press = _on_press, on_release = _on_release) 127 | listener.start() 128 | while not has_stop: 129 | pass 130 | listener.stop() 131 | 132 | else: 133 | raise AttributeError('Invalid type.') -------------------------------------------------------------------------------- /test_encoder.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from easyrobot.encoder.angle import AngleEncoder 4 | 5 | encoder = AngleEncoder(ids = [1, 2, 3, 4, 5, 6, 7, 8], port = '/dev/ttyUSB0', baudrate = 115200) 6 | 7 | while True: 8 | e = encoder.fetch_info() 9 | print(e) 10 | -------------------------------------------------------------------------------- /test_exoskeleton.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | import argparse 4 | from exoskeleton.teleop import DualArmTeleOperator 5 | 6 | 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument( 9 | '--task', '-t', 10 | default = 'gather_balls', 11 | help = 'task name', 12 | type = str, 13 | choices = ['gather_balls', 'grasp_from_the_curtained_shelf'] 14 | ) 15 | args = parser.parse_args() 16 | 17 | op = DualArmTeleOperator( 18 | 'exoskeleton/configs/flexiv_left_' + str(args.task) + '.json', 'exoskeleton/configs/flexiv_right_' + str(args.task) + '.json' 19 | ) 20 | 21 | logger = logging.getLogger("TeleOP-left") 22 | logger.setLevel(logging.INFO) 23 | 24 | logger = logging.getLogger("TeleOP-right") 25 | logger.setLevel(logging.INFO) 26 | 27 | op.initialization() 28 | time.sleep(10) 29 | 30 | op.calibration() 31 | 32 | time.sleep(5) 33 | 34 | try: 35 | op.start() 36 | while True: 37 | time.sleep(1) 38 | except Exception: 39 | op.stop() 40 | -------------------------------------------------------------------------------- /test_exoskeleton_left.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | import argparse 4 | from exoskeleton.teleop import SingleArmTeleOperator 5 | 6 | 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument( 9 | '--task', '-t', 10 | default = 'gather_balls', 11 | help = 'task name', 12 | type = str, 13 | choices = ['gather_balls', 'grasp_from_the_curtained_shelf'] 14 | ) 15 | args = parser.parse_args() 16 | 17 | op = SingleArmTeleOperator('exoskeleton/configs/flexiv_left_' + str(args.task) + '.json') 18 | 19 | logger = logging.getLogger("TeleOP-left") 20 | logger.setLevel(logging.INFO) 21 | 22 | op.initialization() 23 | time.sleep(5) 24 | 25 | op.calibration() 26 | 27 | time.sleep(5) 28 | 29 | try: 30 | op.start() 31 | while True: 32 | time.sleep(1) 33 | except Exception: 34 | op.stop() 35 | -------------------------------------------------------------------------------- /test_exoskeleton_right.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | import argparse 4 | from exoskeleton.teleop import SingleArmTeleOperator 5 | 6 | 7 | parser = argparse.ArgumentParser() 8 | parser.add_argument( 9 | '--task', '-t', 10 | default = 'gather_balls', 11 | help = 'task name', 12 | type = str, 13 | choices = ['gather_balls', 'grasp_from_the_curtained_shelf'] 14 | ) 15 | args = parser.parse_args() 16 | 17 | op = SingleArmTeleOperator('exoskeleton/configs/flexiv_right_' + str(args.task) + '.json') 18 | 19 | logger = logging.getLogger("TeleOP-right") 20 | logger.setLevel(logging.INFO) 21 | 22 | op.initialization() 23 | time.sleep(5) 24 | 25 | op.calibration() 26 | 27 | time.sleep(5) 28 | 29 | try: 30 | op.start() 31 | while True: 32 | time.sleep(1) 33 | except Exception: 34 | op.stop() 35 | --------------------------------------------------------------------------------