├── .gitignore ├── LICENSE ├── assets ├── demo_gx11.gif ├── ex12_urdf.png └── gx11_urdf.png ├── demo_ex12.py ├── demo_gx11.py ├── libgex ├── config.py ├── dynamixel_sdk │ ├── __init__.py │ ├── group_bulk_read.py │ ├── group_bulk_write.py │ ├── group_sync_read.py │ ├── group_sync_write.py │ ├── packet_handler.py │ ├── port_handler.py │ ├── protocol1_packet_handler.py │ ├── protocol2_packet_handler.py │ └── robotis_def.py ├── ex12 │ ├── kinematics.py │ ├── meshes │ │ ├── Link1.STL │ │ ├── Link10.STL │ │ ├── Link11.STL │ │ ├── Link12.STL │ │ ├── Link13.STL │ │ ├── Link14.STL │ │ ├── Link15.STL │ │ ├── Link2.STL │ │ ├── Link3.STL │ │ ├── Link4.STL │ │ ├── Link5.STL │ │ ├── Link6.STL │ │ ├── Link7.STL │ │ ├── Link8.STL │ │ ├── Link9.STL │ │ └── base_link.STL │ ├── test_ik.py │ └── urdf │ │ └── ex12.urdf ├── gx11 │ ├── kinematics.py │ ├── meshes │ │ ├── Link1.STL │ │ ├── Link10.STL │ │ ├── Link11.STL │ │ ├── Link12.STL │ │ ├── Link13.STL │ │ ├── Link14.STL │ │ ├── Link2.STL │ │ ├── Link3.STL │ │ ├── Link4.STL │ │ ├── Link5.STL │ │ ├── Link6.STL │ │ ├── Link7.STL │ │ ├── Link8.STL │ │ ├── Link9.STL │ │ └── base_link.STL │ ├── test_ik.py │ └── urdf │ │ ├── finger1.urdf │ │ └── gx11.urdf ├── libex12.py ├── libgx11.py ├── motor.py └── utils.py ├── readme.md └── zero_hand.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # UV 98 | # Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | #uv.lock 102 | 103 | # poetry 104 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 105 | # This is especially recommended for binary packages to ensure reproducibility, and is more 106 | # commonly ignored for libraries. 107 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 108 | #poetry.lock 109 | 110 | # pdm 111 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 112 | #pdm.lock 113 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 114 | # in version control. 115 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 116 | .pdm.toml 117 | .pdm-python 118 | .pdm-build/ 119 | 120 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 121 | __pypackages__/ 122 | 123 | # Celery stuff 124 | celerybeat-schedule 125 | celerybeat.pid 126 | 127 | # SageMath parsed files 128 | *.sage.py 129 | 130 | # Environments 131 | .env 132 | .venv 133 | env/ 134 | venv/ 135 | ENV/ 136 | env.bak/ 137 | venv.bak/ 138 | 139 | # Spyder project settings 140 | .spyderproject 141 | .spyproject 142 | 143 | # Rope project settings 144 | .ropeproject 145 | 146 | # mkdocs documentation 147 | /site 148 | 149 | # mypy 150 | .mypy_cache/ 151 | .dmypy.json 152 | dmypy.json 153 | 154 | # Pyre type checker 155 | .pyre/ 156 | 157 | # pytype static type analyzer 158 | .pytype/ 159 | 160 | # Cython debug symbols 161 | cython_debug/ 162 | 163 | # PyCharm 164 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 165 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 166 | # and can be added to the global gitignore or merged into this file. For a more nuclear 167 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 168 | #.idea/ 169 | 170 | # Ruff stuff: 171 | .ruff_cache/ 172 | 173 | # PyPI configuration file 174 | .pypirc 175 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Democratizing Dexterous 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 | -------------------------------------------------------------------------------- /assets/demo_gx11.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/assets/demo_gx11.gif -------------------------------------------------------------------------------- /assets/ex12_urdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/assets/ex12_urdf.png -------------------------------------------------------------------------------- /assets/gx11_urdf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/assets/gx11_urdf.png -------------------------------------------------------------------------------- /demo_ex12.py: -------------------------------------------------------------------------------- 1 | from libgex.libex12 import Glove 2 | from libgex.utils import search_ports 3 | import time 4 | 5 | 6 | port = search_ports()[0] 7 | 8 | glove = Glove('/dev/ttyACM1') # COM* for Windows, ttyACM* or ttyUSB* for Linux 9 | glove.connect(init=False) # do not torque on glove yet. 10 | 11 | print(glove.fk_finger1()) # get the thumb tip xyz position in base_link frame (bottom of the palm), unit m 12 | -------------------------------------------------------------------------------- /demo_gx11.py: -------------------------------------------------------------------------------- 1 | from libgex.libgx11 import Hand 2 | import time 3 | 4 | hand = Hand(port='/dev/ttyACM0') # COM* for Windows, ttyACM* or ttyUSB* for Linux 5 | hand.connect(goal_pwm=600) # goal_pwm changes the speed, max 855 6 | 7 | hand.home() # home the hand 8 | 9 | time.sleep(1) 10 | 11 | for i in range(10): 12 | # finger1 vs finger2, thumb vs index 13 | finger1_xyz = [0.04, 0.08, 0.12] 14 | finger2_xyz = [0.04, 0.086, 0.12] 15 | 16 | # use inverse kinmetics 17 | hand.setj_finger1(hand.ik_finger1(finger1_xyz)) 18 | hand.setj_finger2(hand.ik_finger2(finger2_xyz)) 19 | 20 | time.sleep(2) 21 | hand.home() 22 | time.sleep(2) 23 | 24 | # finger1 vs finger3, thumb vs middle 25 | finger1_xyz = [0.01, 0.08, 0.12] 26 | finger3_xyz = [0.01, 0.086, 0.12] 27 | 28 | hand.setj_finger1(hand.ik_finger1(finger1_xyz)) 29 | hand.setj_finger3(hand.ik_finger3(finger3_xyz)) 30 | 31 | time.sleep(2) 32 | hand.home() 33 | time.sleep(2) 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /libgex/config.py: -------------------------------------------------------------------------------- 1 | PROTOCOL_VERSION = 2 2 | BAUDRATE = 1000000 # 1M bps -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .port_handler import * 23 | from .packet_handler import * 24 | from .group_sync_read import * 25 | from .group_sync_write import * 26 | from .group_bulk_read import * 27 | from .group_bulk_write import * 28 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/group_bulk_read.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | PARAM_NUM_DATA = 0 25 | PARAM_NUM_ADDRESS = 1 26 | PARAM_NUM_LENGTH = 2 27 | 28 | 29 | class GroupBulkRead: 30 | def __init__(self, port, ph): 31 | self.port = port 32 | self.ph = ph 33 | 34 | self.last_result = False 35 | self.is_param_changed = False 36 | self.param = [] 37 | self.data_dict = {} 38 | 39 | self.clearParam() 40 | 41 | def makeParam(self): 42 | if not self.data_dict: 43 | return 44 | 45 | self.param = [] 46 | 47 | for dxl_id in self.data_dict: 48 | if self.ph.getProtocolVersion() == 1.0: 49 | self.param.append(self.data_dict[dxl_id][2]) # LEN 50 | self.param.append(dxl_id) # ID 51 | self.param.append(self.data_dict[dxl_id][1]) # ADDR 52 | else: 53 | self.param.append(dxl_id) # ID 54 | self.param.append(DXL_LOBYTE(self.data_dict[dxl_id][1])) # ADDR_L 55 | self.param.append(DXL_HIBYTE(self.data_dict[dxl_id][1])) # ADDR_H 56 | self.param.append(DXL_LOBYTE(self.data_dict[dxl_id][2])) # LEN_L 57 | self.param.append(DXL_HIBYTE(self.data_dict[dxl_id][2])) # LEN_H 58 | 59 | def addParam(self, dxl_id, start_address, data_length): 60 | if dxl_id in self.data_dict: # dxl_id already exist 61 | return False 62 | 63 | data = [] # [0] * data_length 64 | self.data_dict[dxl_id] = [data, start_address, data_length] 65 | 66 | self.is_param_changed = True 67 | return True 68 | 69 | def removeParam(self, dxl_id): 70 | if dxl_id not in self.data_dict: # NOT exist 71 | return 72 | 73 | del self.data_dict[dxl_id] 74 | 75 | self.is_param_changed = True 76 | 77 | def clearParam(self): 78 | self.data_dict.clear() 79 | return 80 | 81 | def txPacket(self): 82 | if len(self.data_dict.keys()) == 0: 83 | return COMM_NOT_AVAILABLE 84 | 85 | if self.is_param_changed is True or not self.param: 86 | self.makeParam() 87 | 88 | if self.ph.getProtocolVersion() == 1.0: 89 | return self.ph.bulkReadTx(self.port, self.param, len(self.data_dict.keys()) * 3) 90 | else: 91 | return self.ph.bulkReadTx(self.port, self.param, len(self.data_dict.keys()) * 5) 92 | 93 | def rxPacket(self): 94 | self.last_result = False 95 | 96 | result = COMM_RX_FAIL 97 | 98 | if len(self.data_dict.keys()) == 0: 99 | return COMM_NOT_AVAILABLE 100 | 101 | for dxl_id in self.data_dict: 102 | self.data_dict[dxl_id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, dxl_id, 103 | self.data_dict[dxl_id][PARAM_NUM_LENGTH]) 104 | if result != COMM_SUCCESS: 105 | return result 106 | 107 | if result == COMM_SUCCESS: 108 | self.last_result = True 109 | 110 | return result 111 | 112 | def txRxPacket(self): 113 | result = self.txPacket() 114 | if result != COMM_SUCCESS: 115 | return result 116 | 117 | return self.rxPacket() 118 | 119 | def isAvailable(self, dxl_id, address, data_length): 120 | if self.last_result is False or dxl_id not in self.data_dict: 121 | return False 122 | 123 | start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] 124 | 125 | if (address < start_addr) or (start_addr + self.data_dict[dxl_id][PARAM_NUM_LENGTH] - data_length < address): 126 | return False 127 | 128 | return True 129 | 130 | def getData(self, dxl_id, address, data_length): 131 | if not self.isAvailable(dxl_id, address, data_length): 132 | return 0 133 | 134 | start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] 135 | 136 | if data_length == 1: 137 | return self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr] 138 | elif data_length == 2: 139 | return DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr], 140 | self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 1]) 141 | elif data_length == 4: 142 | return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 0], 143 | self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 1]), 144 | DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 2], 145 | self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 3])) 146 | else: 147 | return 0 148 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/group_bulk_write.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | 25 | class GroupBulkWrite: 26 | def __init__(self, port, ph): 27 | self.port = port 28 | self.ph = ph 29 | 30 | self.is_param_changed = False 31 | self.param = [] 32 | self.data_list = {} 33 | 34 | self.clearParam() 35 | 36 | def makeParam(self): 37 | if self.ph.getProtocolVersion() == 1.0 or not self.data_list: 38 | return 39 | 40 | self.param = [] 41 | 42 | for dxl_id in self.data_list: 43 | if not self.data_list[dxl_id]: 44 | return 45 | 46 | self.param.append(dxl_id) 47 | self.param.append(DXL_LOBYTE(self.data_list[dxl_id][1])) 48 | self.param.append(DXL_HIBYTE(self.data_list[dxl_id][1])) 49 | self.param.append(DXL_LOBYTE(self.data_list[dxl_id][2])) 50 | self.param.append(DXL_HIBYTE(self.data_list[dxl_id][2])) 51 | 52 | self.param.extend(self.data_list[dxl_id][0]) 53 | 54 | def addParam(self, dxl_id, start_address, data_length, data): 55 | if self.ph.getProtocolVersion() == 1.0: 56 | return False 57 | 58 | if dxl_id in self.data_list: # dxl_id already exist 59 | return False 60 | 61 | if len(data) > data_length: # input data is longer than set 62 | return False 63 | 64 | self.data_list[dxl_id] = [data, start_address, data_length] 65 | 66 | self.is_param_changed = True 67 | return True 68 | 69 | def removeParam(self, dxl_id): 70 | if self.ph.getProtocolVersion() == 1.0: 71 | return 72 | 73 | if dxl_id not in self.data_list: # NOT exist 74 | return 75 | 76 | del self.data_list[dxl_id] 77 | 78 | self.is_param_changed = True 79 | 80 | def changeParam(self, dxl_id, start_address, data_length, data): 81 | if self.ph.getProtocolVersion() == 1.0: 82 | return False 83 | 84 | if dxl_id not in self.data_list: # NOT exist 85 | return False 86 | 87 | if len(data) > data_length: # input data is longer than set 88 | return False 89 | 90 | self.data_list[dxl_id] = [data, start_address, data_length] 91 | 92 | self.is_param_changed = True 93 | return True 94 | 95 | def clearParam(self): 96 | if self.ph.getProtocolVersion() == 1.0: 97 | return 98 | 99 | self.data_list.clear() 100 | return 101 | 102 | def txPacket(self): 103 | if self.ph.getProtocolVersion() == 1.0 or len(self.data_list.keys()) == 0: 104 | return COMM_NOT_AVAILABLE 105 | 106 | if self.is_param_changed is True or len(self.param) == 0: 107 | self.makeParam() 108 | 109 | return self.ph.bulkWriteTxOnly(self.port, self.param, len(self.param)) 110 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/group_sync_read.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | 25 | class GroupSyncRead: 26 | def __init__(self, port, ph, start_address, data_length): 27 | self.port = port 28 | self.ph = ph 29 | self.start_address = start_address 30 | self.data_length = data_length 31 | 32 | self.last_result = False 33 | self.is_param_changed = False 34 | self.param = [] 35 | self.data_dict = {} 36 | 37 | self.clearParam() 38 | 39 | def makeParam(self): 40 | if self.ph.getProtocolVersion() == 1.0: 41 | return 42 | 43 | if not self.data_dict: # len(self.data_dict.keys()) == 0: 44 | return 45 | 46 | self.param = [] 47 | 48 | for dxl_id in self.data_dict: 49 | self.param.append(dxl_id) 50 | 51 | def addParam(self, dxl_id): 52 | if self.ph.getProtocolVersion() == 1.0: 53 | return False 54 | 55 | if dxl_id in self.data_dict: # dxl_id already exist 56 | return False 57 | 58 | self.data_dict[dxl_id] = [] # [0] * self.data_length 59 | 60 | self.is_param_changed = True 61 | return True 62 | 63 | def removeParam(self, dxl_id): 64 | if self.ph.getProtocolVersion() == 1.0: 65 | return 66 | 67 | if dxl_id not in self.data_dict: # NOT exist 68 | return 69 | 70 | del self.data_dict[dxl_id] 71 | 72 | self.is_param_changed = True 73 | 74 | def clearParam(self): 75 | if self.ph.getProtocolVersion() == 1.0: 76 | return 77 | 78 | self.data_dict.clear() 79 | 80 | def txPacket(self): 81 | if self.ph.getProtocolVersion() == 1.0 or len(self.data_dict.keys()) == 0: 82 | return COMM_NOT_AVAILABLE 83 | 84 | if self.is_param_changed is True or not self.param: 85 | self.makeParam() 86 | 87 | return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param, 88 | len(self.data_dict.keys()) * 1) 89 | 90 | def rxPacket(self): 91 | self.last_result = False 92 | 93 | if self.ph.getProtocolVersion() == 1.0: 94 | return COMM_NOT_AVAILABLE 95 | 96 | result = COMM_RX_FAIL 97 | 98 | if len(self.data_dict.keys()) == 0: 99 | return COMM_NOT_AVAILABLE 100 | 101 | for dxl_id in self.data_dict: 102 | self.data_dict[dxl_id], result, _ = self.ph.readRx(self.port, dxl_id, self.data_length) 103 | if result != COMM_SUCCESS: 104 | return result 105 | 106 | if result == COMM_SUCCESS: 107 | self.last_result = True 108 | 109 | return result 110 | 111 | def txRxPacket(self): 112 | if self.ph.getProtocolVersion() == 1.0: 113 | return COMM_NOT_AVAILABLE 114 | 115 | result = self.txPacket() 116 | if result != COMM_SUCCESS: 117 | return result 118 | 119 | return self.rxPacket() 120 | 121 | def isAvailable(self, dxl_id, address, data_length): 122 | if self.ph.getProtocolVersion() == 1.0 or self.last_result is False or dxl_id not in self.data_dict: 123 | return False 124 | 125 | if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): 126 | return False 127 | 128 | return True 129 | 130 | def getData(self, dxl_id, address, data_length): 131 | if not self.isAvailable(dxl_id, address, data_length): 132 | return 0 133 | 134 | if data_length == 1: 135 | return self.data_dict[dxl_id][address - self.start_address] 136 | elif data_length == 2: 137 | return DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address], 138 | self.data_dict[dxl_id][address - self.start_address + 1]) 139 | elif data_length == 4: 140 | return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 0], 141 | self.data_dict[dxl_id][address - self.start_address + 1]), 142 | DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 2], 143 | self.data_dict[dxl_id][address - self.start_address + 3])) 144 | else: 145 | return 0 146 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/group_sync_write.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | 25 | class GroupSyncWrite: 26 | def __init__(self, port, ph, start_address, data_length): 27 | self.port = port 28 | self.ph = ph 29 | self.start_address = start_address 30 | self.data_length = data_length 31 | 32 | self.is_param_changed = False 33 | self.param = [] 34 | self.data_dict = {} 35 | 36 | self.clearParam() 37 | 38 | def makeParam(self): 39 | if not self.data_dict: 40 | return 41 | 42 | self.param = [] 43 | 44 | for dxl_id in self.data_dict: 45 | if not self.data_dict[dxl_id]: 46 | return 47 | 48 | self.param.append(dxl_id) 49 | self.param.extend(self.data_dict[dxl_id]) 50 | 51 | def addParam(self, dxl_id, data): 52 | if dxl_id in self.data_dict: # dxl_id already exist 53 | return False 54 | 55 | if len(data) > self.data_length: # input data is longer than set 56 | return False 57 | 58 | self.data_dict[dxl_id] = data 59 | 60 | self.is_param_changed = True 61 | return True 62 | 63 | def removeParam(self, dxl_id): 64 | if dxl_id not in self.data_dict: # NOT exist 65 | return 66 | 67 | del self.data_dict[dxl_id] 68 | 69 | self.is_param_changed = True 70 | 71 | def changeParam(self, dxl_id, data): 72 | if dxl_id not in self.data_dict: # NOT exist 73 | return False 74 | 75 | if len(data) > self.data_length: # input data is longer than set 76 | return False 77 | 78 | self.data_dict[dxl_id] = data 79 | 80 | self.is_param_changed = True 81 | return True 82 | 83 | def clearParam(self): 84 | self.data_dict.clear() 85 | 86 | def txPacket(self): 87 | if len(self.data_dict.keys()) == 0: 88 | return COMM_NOT_AVAILABLE 89 | 90 | if self.is_param_changed is True or not self.param: 91 | self.makeParam() 92 | 93 | return self.ph.syncWriteTxOnly(self.port, self.start_address, self.data_length, self.param, 94 | len(self.data_dict.keys()) * (1 + self.data_length)) 95 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/packet_handler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .protocol1_packet_handler import * 23 | from .protocol2_packet_handler import * 24 | 25 | 26 | def PacketHandler(protocol_version): 27 | # FIXME: float or int-to-float comparison can generate weird behaviour 28 | if protocol_version == 1.0: 29 | return Protocol1PacketHandler() 30 | elif protocol_version == 2.0: 31 | return Protocol2PacketHandler() 32 | else: 33 | return Protocol2PacketHandler() 34 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/port_handler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | import time 23 | import serial 24 | import sys 25 | import platform 26 | 27 | LATENCY_TIMER = 16 28 | DEFAULT_BAUDRATE = 1000000 29 | 30 | 31 | class PortHandler(object): 32 | def __init__(self, port_name): 33 | self.is_open = False 34 | self.baudrate = DEFAULT_BAUDRATE 35 | self.packet_start_time = 0.0 36 | self.packet_timeout = 0.0 37 | self.tx_time_per_byte = 0.0 38 | 39 | self.is_using = False 40 | self.port_name = port_name 41 | self.ser = None 42 | 43 | def openPort(self): 44 | return self.setBaudRate(self.baudrate) 45 | 46 | def closePort(self): 47 | self.ser.close() 48 | self.is_open = False 49 | 50 | def clearPort(self): 51 | self.ser.flush() 52 | 53 | def setPortName(self, port_name): 54 | self.port_name = port_name 55 | 56 | def getPortName(self): 57 | return self.port_name 58 | 59 | def setBaudRate(self, baudrate): 60 | baud = self.getCFlagBaud(baudrate) 61 | 62 | if baud <= 0: 63 | # self.setupPort(38400) 64 | # self.baudrate = baudrate 65 | return False # TODO: setCustomBaudrate(baudrate) 66 | else: 67 | self.baudrate = baudrate 68 | return self.setupPort(baud) 69 | 70 | def getBaudRate(self): 71 | return self.baudrate 72 | 73 | def getBytesAvailable(self): 74 | return self.ser.in_waiting 75 | 76 | def readPort(self, length): 77 | if (sys.version_info > (3, 0)): 78 | return self.ser.read(length) 79 | else: 80 | return [ord(ch) for ch in self.ser.read(length)] 81 | 82 | def writePort(self, packet): 83 | return self.ser.write(packet) 84 | 85 | def setPacketTimeout(self, packet_length): 86 | self.packet_start_time = self.getCurrentTime() 87 | self.packet_timeout = (self.tx_time_per_byte * packet_length) + (LATENCY_TIMER * 2.0) + 2.0 88 | 89 | def setPacketTimeoutMillis(self, msec): 90 | self.packet_start_time = self.getCurrentTime() 91 | self.packet_timeout = msec 92 | 93 | def isPacketTimeout(self): 94 | if self.getTimeSinceStart() > self.packet_timeout: 95 | self.packet_timeout = 0 96 | return True 97 | 98 | return False 99 | 100 | def getCurrentTime(self): 101 | return round(time.time() * 1000000000) / 1000000.0 102 | 103 | def getTimeSinceStart(self): 104 | time_since = self.getCurrentTime() - self.packet_start_time 105 | if time_since < 0.0: 106 | self.packet_start_time = self.getCurrentTime() 107 | 108 | return time_since 109 | 110 | def setupPort(self, cflag_baud): 111 | if self.is_open: 112 | self.closePort() 113 | 114 | self.ser = serial.Serial( 115 | port=self.port_name, 116 | baudrate=self.baudrate, 117 | # parity = serial.PARITY_ODD, 118 | # stopbits = serial.STOPBITS_TWO, 119 | bytesize=serial.EIGHTBITS, 120 | timeout=0 121 | ) 122 | 123 | self.is_open = True 124 | 125 | self.ser.reset_input_buffer() 126 | 127 | self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 128 | 129 | return True 130 | 131 | def getCFlagBaud(self, baudrate): 132 | if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, 133 | 2000000, 2500000, 3000000, 3500000, 4000000]: 134 | return baudrate 135 | else: 136 | return -1 137 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/protocol1_packet_handler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | TXPACKET_MAX_LEN = 250 25 | RXPACKET_MAX_LEN = 250 26 | 27 | # for Protocol 1.0 Packet 28 | PKT_HEADER0 = 0 29 | PKT_HEADER1 = 1 30 | PKT_ID = 2 31 | PKT_LENGTH = 3 32 | PKT_INSTRUCTION = 4 33 | PKT_ERROR = 4 34 | PKT_PARAMETER0 = 5 35 | 36 | # Protocol 1.0 Error bit 37 | ERRBIT_VOLTAGE = 1 # Supplied voltage is out of the range (operating volatage set in the control table) 38 | ERRBIT_ANGLE = 2 # Goal position is written out of the range (from CW angle limit to CCW angle limit) 39 | ERRBIT_OVERHEAT = 4 # Temperature is out of the range (operating temperature set in the control table) 40 | ERRBIT_RANGE = 8 # Command(setting value) is out of the range for use. 41 | ERRBIT_CHECKSUM = 16 # Instruction packet checksum is incorrect. 42 | ERRBIT_OVERLOAD = 32 # The current load cannot be controlled by the set torque. 43 | ERRBIT_INSTRUCTION = 64 # Undefined instruction or delivering the action command without the reg_write command. 44 | 45 | 46 | class Protocol1PacketHandler(object): 47 | def getProtocolVersion(self): 48 | return 1.0 49 | 50 | def getTxRxResult(self, result): 51 | if result == COMM_SUCCESS: 52 | return "[TxRxResult] Communication success!" 53 | elif result == COMM_PORT_BUSY: 54 | return "[TxRxResult] Port is in use!" 55 | elif result == COMM_TX_FAIL: 56 | return "[TxRxResult] Failed transmit instruction packet!" 57 | elif result == COMM_RX_FAIL: 58 | return "[TxRxResult] Failed get status packet from device!" 59 | elif result == COMM_TX_ERROR: 60 | return "[TxRxResult] Incorrect instruction packet!" 61 | elif result == COMM_RX_WAITING: 62 | return "[TxRxResult] Now receiving status packet!" 63 | elif result == COMM_RX_TIMEOUT: 64 | return "[TxRxResult] There is no status packet!" 65 | elif result == COMM_RX_CORRUPT: 66 | return "[TxRxResult] Incorrect status packet!" 67 | elif result == COMM_NOT_AVAILABLE: 68 | return "[TxRxResult] Protocol does not support this function!" 69 | else: 70 | return "" 71 | 72 | def getRxPacketError(self, error): 73 | if error & ERRBIT_VOLTAGE: 74 | return "[RxPacketError] Input voltage error!" 75 | 76 | if error & ERRBIT_ANGLE: 77 | return "[RxPacketError] Angle limit error!" 78 | 79 | if error & ERRBIT_OVERHEAT: 80 | return "[RxPacketError] Overheat error!" 81 | 82 | if error & ERRBIT_RANGE: 83 | return "[RxPacketError] Out of range error!" 84 | 85 | if error & ERRBIT_CHECKSUM: 86 | return "[RxPacketError] Checksum error!" 87 | 88 | if error & ERRBIT_OVERLOAD: 89 | return "[RxPacketError] Overload error!" 90 | 91 | if error & ERRBIT_INSTRUCTION: 92 | return "[RxPacketError] Instruction code error!" 93 | 94 | return "" 95 | 96 | def txPacket(self, port, txpacket): 97 | checksum = 0 98 | total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH 99 | 100 | if port.is_using: 101 | return COMM_PORT_BUSY 102 | port.is_using = True 103 | 104 | # check max packet length 105 | if total_packet_length > TXPACKET_MAX_LEN: 106 | port.is_using = False 107 | return COMM_TX_ERROR 108 | 109 | # make packet header 110 | txpacket[PKT_HEADER0] = 0xFF 111 | txpacket[PKT_HEADER1] = 0xFF 112 | 113 | # add a checksum to the packet 114 | for idx in range(2, total_packet_length - 1): # except header, checksum 115 | checksum += txpacket[idx] 116 | 117 | txpacket[total_packet_length - 1] = ~checksum & 0xFF 118 | 119 | #print "[TxPacket] %r" % txpacket 120 | 121 | # tx packet 122 | port.clearPort() 123 | written_packet_length = port.writePort(txpacket) 124 | if total_packet_length != written_packet_length: 125 | port.is_using = False 126 | return COMM_TX_FAIL 127 | 128 | return COMM_SUCCESS 129 | 130 | def rxPacket(self, port): 131 | rxpacket = [] 132 | 133 | result = COMM_TX_FAIL 134 | checksum = 0 135 | rx_length = 0 136 | wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) 137 | 138 | while True: 139 | rxpacket.extend(port.readPort(wait_length - rx_length)) 140 | rx_length = len(rxpacket) 141 | if rx_length >= wait_length: 142 | # find packet header 143 | for idx in range(0, (rx_length - 1)): 144 | if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF): 145 | break 146 | 147 | if idx == 0: # found at the beginning of the packet 148 | if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( 149 | rxpacket[PKT_ERROR] > 0x7F): 150 | # unavailable ID or unavailable Length or unavailable Error 151 | # remove the first byte in the packet 152 | del rxpacket[0] 153 | rx_length -= 1 154 | continue 155 | 156 | # re-calculate the exact length of the rx packet 157 | if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): 158 | wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 159 | continue 160 | 161 | if rx_length < wait_length: 162 | # check timeout 163 | if port.isPacketTimeout(): 164 | if rx_length == 0: 165 | result = COMM_RX_TIMEOUT 166 | else: 167 | result = COMM_RX_CORRUPT 168 | break 169 | else: 170 | continue 171 | 172 | # calculate checksum 173 | for i in range(2, wait_length - 1): # except header, checksum 174 | checksum += rxpacket[i] 175 | checksum = ~checksum & 0xFF 176 | 177 | # verify checksum 178 | if rxpacket[wait_length - 1] == checksum: 179 | result = COMM_SUCCESS 180 | else: 181 | result = COMM_RX_CORRUPT 182 | break 183 | 184 | else: 185 | # remove unnecessary packets 186 | del rxpacket[0: idx] 187 | rx_length -= idx 188 | 189 | else: 190 | # check timeout 191 | if port.isPacketTimeout(): 192 | if rx_length == 0: 193 | result = COMM_RX_TIMEOUT 194 | else: 195 | result = COMM_RX_CORRUPT 196 | break 197 | 198 | port.is_using = False 199 | 200 | #print "[RxPacket] %r" % rxpacket 201 | 202 | return rxpacket, result 203 | 204 | # NOT for BulkRead 205 | def txRxPacket(self, port, txpacket): 206 | rxpacket = None 207 | error = 0 208 | 209 | # tx packet 210 | result = self.txPacket(port, txpacket) 211 | if result != COMM_SUCCESS: 212 | return rxpacket, result, error 213 | 214 | # (Instruction == BulkRead) == this function is not available. 215 | if txpacket[PKT_INSTRUCTION] == INST_BULK_READ: 216 | result = COMM_NOT_AVAILABLE 217 | 218 | # (ID == Broadcast ID) == no need to wait for status packet or not available 219 | if (txpacket[PKT_ID] == BROADCAST_ID): 220 | port.is_using = False 221 | return rxpacket, result, error 222 | 223 | # set packet timeout 224 | if txpacket[PKT_INSTRUCTION] == INST_READ: 225 | port.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) 226 | else: 227 | port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM 228 | 229 | # rx packet 230 | while True: 231 | rxpacket, result = self.rxPacket(port) 232 | if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: 233 | break 234 | 235 | if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: 236 | error = rxpacket[PKT_ERROR] 237 | 238 | return rxpacket, result, error 239 | 240 | def ping(self, port, dxl_id): 241 | model_number = 0 242 | error = 0 243 | 244 | txpacket = [0] * 6 245 | 246 | if dxl_id >= BROADCAST_ID: 247 | return model_number, COMM_NOT_AVAILABLE, error 248 | 249 | txpacket[PKT_ID] = dxl_id 250 | txpacket[PKT_LENGTH] = 2 251 | txpacket[PKT_INSTRUCTION] = INST_PING 252 | 253 | rxpacket, result, error = self.txRxPacket(port, txpacket) 254 | 255 | if result == COMM_SUCCESS: 256 | data_read, result, error = self.readTxRx(port, dxl_id, 0, 2) # Address 0 : Model Number 257 | if result == COMM_SUCCESS: 258 | model_number = DXL_MAKEWORD(data_read[0], data_read[1]) 259 | 260 | return model_number, result, error 261 | 262 | def broadcastPing(self, port): 263 | data_list = None 264 | return data_list, COMM_NOT_AVAILABLE 265 | 266 | def action(self, port, dxl_id): 267 | txpacket = [0] * 6 268 | 269 | txpacket[PKT_ID] = dxl_id 270 | txpacket[PKT_LENGTH] = 2 271 | txpacket[PKT_INSTRUCTION] = INST_ACTION 272 | 273 | _, result, _ = self.txRxPacket(port, txpacket) 274 | 275 | return result 276 | 277 | def reboot(self, port, dxl_id): 278 | return COMM_NOT_AVAILABLE, 0 279 | 280 | def factoryReset(self, port, dxl_id): 281 | txpacket = [0] * 6 282 | 283 | txpacket[PKT_ID] = dxl_id 284 | txpacket[PKT_LENGTH] = 2 285 | txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET 286 | 287 | _, result, error = self.txRxPacket(port, txpacket) 288 | 289 | return result, error 290 | 291 | def readTx(self, port, dxl_id, address, length): 292 | 293 | txpacket = [0] * 8 294 | 295 | if dxl_id >= BROADCAST_ID: 296 | return COMM_NOT_AVAILABLE 297 | 298 | txpacket[PKT_ID] = dxl_id 299 | txpacket[PKT_LENGTH] = 4 300 | txpacket[PKT_INSTRUCTION] = INST_READ 301 | txpacket[PKT_PARAMETER0 + 0] = address 302 | txpacket[PKT_PARAMETER0 + 1] = length 303 | 304 | result = self.txPacket(port, txpacket) 305 | 306 | # set packet timeout 307 | if result == COMM_SUCCESS: 308 | port.setPacketTimeout(length + 6) 309 | 310 | return result 311 | 312 | def readRx(self, port, dxl_id, length): 313 | result = COMM_TX_FAIL 314 | error = 0 315 | 316 | rxpacket = None 317 | data = [] 318 | 319 | while True: 320 | rxpacket, result = self.rxPacket(port) 321 | 322 | if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: 323 | break 324 | 325 | if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: 326 | error = rxpacket[PKT_ERROR] 327 | 328 | data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) 329 | 330 | return data, result, error 331 | 332 | def readTxRx(self, port, dxl_id, address, length): 333 | txpacket = [0] * 8 334 | data = [] 335 | 336 | if dxl_id >= BROADCAST_ID: 337 | return data, COMM_NOT_AVAILABLE, 0 338 | 339 | txpacket[PKT_ID] = dxl_id 340 | txpacket[PKT_LENGTH] = 4 341 | txpacket[PKT_INSTRUCTION] = INST_READ 342 | txpacket[PKT_PARAMETER0 + 0] = address 343 | txpacket[PKT_PARAMETER0 + 1] = length 344 | 345 | rxpacket, result, error = self.txRxPacket(port, txpacket) 346 | if result == COMM_SUCCESS: 347 | error = rxpacket[PKT_ERROR] 348 | 349 | data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) 350 | 351 | return data, result, error 352 | 353 | def read1ByteTx(self, port, dxl_id, address): 354 | return self.readTx(port, dxl_id, address, 1) 355 | 356 | def read1ByteRx(self, port, dxl_id): 357 | data, result, error = self.readRx(port, dxl_id, 1) 358 | data_read = data[0] if (result == COMM_SUCCESS) else 0 359 | return data_read, result, error 360 | 361 | def read1ByteTxRx(self, port, dxl_id, address): 362 | data, result, error = self.readTxRx(port, dxl_id, address, 1) 363 | data_read = data[0] if (result == COMM_SUCCESS) else 0 364 | return data_read, result, error 365 | 366 | def read2ByteTx(self, port, dxl_id, address): 367 | return self.readTx(port, dxl_id, address, 2) 368 | 369 | def read2ByteRx(self, port, dxl_id): 370 | data, result, error = self.readRx(port, dxl_id, 2) 371 | data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 372 | return data_read, result, error 373 | 374 | def read2ByteTxRx(self, port, dxl_id, address): 375 | data, result, error = self.readTxRx(port, dxl_id, address, 2) 376 | data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 377 | return data_read, result, error 378 | 379 | def read4ByteTx(self, port, dxl_id, address): 380 | return self.readTx(port, dxl_id, address, 4) 381 | 382 | def read4ByteRx(self, port, dxl_id): 383 | data, result, error = self.readRx(port, dxl_id, 4) 384 | data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), 385 | DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 386 | return data_read, result, error 387 | 388 | def read4ByteTxRx(self, port, dxl_id, address): 389 | data, result, error = self.readTxRx(port, dxl_id, address, 4) 390 | data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), 391 | DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 392 | return data_read, result, error 393 | 394 | def writeTxOnly(self, port, dxl_id, address, length, data): 395 | txpacket = [0] * (length + 7) 396 | 397 | txpacket[PKT_ID] = dxl_id 398 | txpacket[PKT_LENGTH] = length + 3 399 | txpacket[PKT_INSTRUCTION] = INST_WRITE 400 | txpacket[PKT_PARAMETER0] = address 401 | 402 | txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] 403 | 404 | result = self.txPacket(port, txpacket) 405 | port.is_using = False 406 | 407 | return result 408 | 409 | def writeTxRx(self, port, dxl_id, address, length, data): 410 | txpacket = [0] * (length + 7) 411 | 412 | txpacket[PKT_ID] = dxl_id 413 | txpacket[PKT_LENGTH] = length + 3 414 | txpacket[PKT_INSTRUCTION] = INST_WRITE 415 | txpacket[PKT_PARAMETER0] = address 416 | 417 | txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] 418 | rxpacket, result, error = self.txRxPacket(port, txpacket) 419 | 420 | return result, error 421 | 422 | def write1ByteTxOnly(self, port, dxl_id, address, data): 423 | data_write = [data] 424 | return self.writeTxOnly(port, dxl_id, address, 1, data_write) 425 | 426 | def write1ByteTxRx(self, port, dxl_id, address, data): 427 | data_write = [data] 428 | return self.writeTxRx(port, dxl_id, address, 1, data_write) 429 | 430 | def write2ByteTxOnly(self, port, dxl_id, address, data): 431 | data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] 432 | return self.writeTxOnly(port, dxl_id, address, 2, data_write) 433 | 434 | def write2ByteTxRx(self, port, dxl_id, address, data): 435 | data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] 436 | return self.writeTxRx(port, dxl_id, address, 2, data_write) 437 | 438 | def write4ByteTxOnly(self, port, dxl_id, address, data): 439 | data_write = [DXL_LOBYTE(DXL_LOWORD(data)), 440 | DXL_HIBYTE(DXL_LOWORD(data)), 441 | DXL_LOBYTE(DXL_HIWORD(data)), 442 | DXL_HIBYTE(DXL_HIWORD(data))] 443 | return self.writeTxOnly(port, dxl_id, address, 4, data_write) 444 | 445 | def write4ByteTxRx(self, port, dxl_id, address, data): 446 | data_write = [DXL_LOBYTE(DXL_LOWORD(data)), 447 | DXL_HIBYTE(DXL_LOWORD(data)), 448 | DXL_LOBYTE(DXL_HIWORD(data)), 449 | DXL_HIBYTE(DXL_HIWORD(data))] 450 | return self.writeTxRx(port, dxl_id, address, 4, data_write) 451 | 452 | def regWriteTxOnly(self, port, dxl_id, address, length, data): 453 | txpacket = [0] * (length + 7) 454 | 455 | txpacket[PKT_ID] = dxl_id 456 | txpacket[PKT_LENGTH] = length + 3 457 | txpacket[PKT_INSTRUCTION] = INST_REG_WRITE 458 | txpacket[PKT_PARAMETER0] = address 459 | 460 | txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] 461 | 462 | result = self.txPacket(port, txpacket) 463 | port.is_using = False 464 | 465 | return result 466 | 467 | def regWriteTxRx(self, port, dxl_id, address, length, data): 468 | txpacket = [0] * (length + 7) 469 | 470 | txpacket[PKT_ID] = dxl_id 471 | txpacket[PKT_LENGTH] = length + 3 472 | txpacket[PKT_INSTRUCTION] = INST_REG_WRITE 473 | txpacket[PKT_PARAMETER0] = address 474 | 475 | txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] 476 | 477 | _, result, error = self.txRxPacket(port, txpacket) 478 | 479 | return result, error 480 | 481 | def syncReadTx(self, port, start_address, data_length, param, param_length): 482 | return COMM_NOT_AVAILABLE 483 | 484 | def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): 485 | txpacket = [0] * (param_length + 8) 486 | # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM 487 | 488 | txpacket[PKT_ID] = BROADCAST_ID 489 | txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM 490 | txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE 491 | txpacket[PKT_PARAMETER0 + 0] = start_address 492 | txpacket[PKT_PARAMETER0 + 1] = data_length 493 | 494 | txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] 495 | 496 | _, result, _ = self.txRxPacket(port, txpacket) 497 | 498 | return result 499 | 500 | def bulkReadTx(self, port, param, param_length): 501 | txpacket = [0] * (param_length + 7) 502 | # 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM 503 | 504 | txpacket[PKT_ID] = BROADCAST_ID 505 | txpacket[PKT_LENGTH] = param_length + 3 # 3: INST 0x00 ... CHKSUM 506 | txpacket[PKT_INSTRUCTION] = INST_BULK_READ 507 | txpacket[PKT_PARAMETER0 + 0] = 0x00 508 | 509 | txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + param_length] = param[0: param_length] 510 | 511 | result = self.txPacket(port, txpacket) 512 | if result == COMM_SUCCESS: 513 | wait_length = 0 514 | i = 0 515 | while i < param_length: 516 | wait_length += param[i] + 7 517 | i += 3 518 | port.setPacketTimeout(wait_length) 519 | 520 | return result 521 | 522 | def bulkWriteTxOnly(self, port, param, param_length): 523 | return COMM_NOT_AVAILABLE 524 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/protocol2_packet_handler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | from .robotis_def import * 23 | 24 | TXPACKET_MAX_LEN = 1 * 1024 25 | RXPACKET_MAX_LEN = 1 * 1024 26 | 27 | # for Protocol 2.0 Packet 28 | PKT_HEADER0 = 0 29 | PKT_HEADER1 = 1 30 | PKT_HEADER2 = 2 31 | PKT_RESERVED = 3 32 | PKT_ID = 4 33 | PKT_LENGTH_L = 5 34 | PKT_LENGTH_H = 6 35 | PKT_INSTRUCTION = 7 36 | PKT_ERROR = 8 37 | PKT_PARAMETER0 = 8 38 | 39 | # Protocol 2.0 Error bit 40 | ERRNUM_RESULT_FAIL = 1 # Failed to process the instruction packet. 41 | ERRNUM_INSTRUCTION = 2 # Instruction error 42 | ERRNUM_CRC = 3 # CRC check error 43 | ERRNUM_DATA_RANGE = 4 # Data range error 44 | ERRNUM_DATA_LENGTH = 5 # Data length error 45 | ERRNUM_DATA_LIMIT = 6 # Data limit error 46 | ERRNUM_ACCESS = 7 # Access error 47 | 48 | ERRBIT_ALERT = 128 # When the device has a problem, this bit is set to 1. Check "Device Status Check" value. 49 | 50 | 51 | class Protocol2PacketHandler(object): 52 | def getProtocolVersion(self): 53 | return 2.0 54 | 55 | def getTxRxResult(self, result): 56 | if result == COMM_SUCCESS: 57 | return "[TxRxResult] Communication success!" 58 | elif result == COMM_PORT_BUSY: 59 | return "[TxRxResult] Port is in use!" 60 | elif result == COMM_TX_FAIL: 61 | return "[TxRxResult] Failed transmit instruction packet!" 62 | elif result == COMM_RX_FAIL: 63 | return "[TxRxResult] Failed get status packet from device!" 64 | elif result == COMM_TX_ERROR: 65 | return "[TxRxResult] Incorrect instruction packet!" 66 | elif result == COMM_RX_WAITING: 67 | return "[TxRxResult] Now receiving status packet!" 68 | elif result == COMM_RX_TIMEOUT: 69 | return "[TxRxResult] There is no status packet!" 70 | elif result == COMM_RX_CORRUPT: 71 | return "[TxRxResult] Incorrect status packet!" 72 | elif result == COMM_NOT_AVAILABLE: 73 | return "[TxRxResult] Protocol does not support this function!" 74 | else: 75 | return "" 76 | 77 | def getRxPacketError(self, error): 78 | if error & ERRBIT_ALERT: 79 | return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!" 80 | 81 | not_alert_error = error & ~ERRBIT_ALERT 82 | if not_alert_error == 0: 83 | return "" 84 | elif not_alert_error == ERRNUM_RESULT_FAIL: 85 | return "[RxPacketError] Failed to process the instruction packet!" 86 | 87 | elif not_alert_error == ERRNUM_INSTRUCTION: 88 | return "[RxPacketError] Undefined instruction or incorrect instruction!" 89 | 90 | elif not_alert_error == ERRNUM_CRC: 91 | return "[RxPacketError] CRC doesn't match!" 92 | 93 | elif not_alert_error == ERRNUM_DATA_RANGE: 94 | return "[RxPacketError] The data value is out of range!" 95 | 96 | elif not_alert_error == ERRNUM_DATA_LENGTH: 97 | return "[RxPacketError] The data length does not match as expected!" 98 | 99 | elif not_alert_error == ERRNUM_DATA_LIMIT: 100 | return "[RxPacketError] The data value exceeds the limit value!" 101 | 102 | elif not_alert_error == ERRNUM_ACCESS: 103 | return "[RxPacketError] Writing or Reading is not available to target address!" 104 | 105 | else: 106 | return "[RxPacketError] Unknown error code!" 107 | 108 | def updateCRC(self, crc_accum, data_blk_ptr, data_blk_size): 109 | crc_table = [0x0000, 110 | 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 111 | 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 112 | 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 113 | 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 114 | 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, 115 | 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, 116 | 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, 117 | 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, 118 | 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 119 | 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 120 | 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 121 | 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 122 | 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, 123 | 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, 124 | 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, 125 | 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, 126 | 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, 127 | 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 128 | 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 129 | 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 130 | 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, 131 | 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, 132 | 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, 133 | 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, 134 | 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 135 | 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 136 | 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 137 | 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 138 | 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, 139 | 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, 140 | 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, 141 | 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, 142 | 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 143 | 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 144 | 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 145 | 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 146 | 0x820D, 0x8207, 0x0202] 147 | 148 | for j in range(0, data_blk_size): 149 | i = ((crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF 150 | crc_accum = ((crc_accum << 8) ^ crc_table[i]) & 0xFFFF 151 | 152 | return crc_accum 153 | 154 | def addStuffing(self, packet): 155 | packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]) 156 | packet_length_out = packet_length_in 157 | 158 | temp = [0] * TXPACKET_MAX_LEN 159 | 160 | # FF FF FD XX ID LEN_L LEN_H 161 | temp[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] 162 | 163 | index = PKT_INSTRUCTION 164 | 165 | for i in range(0, packet_length_in - 2): # except CRC 166 | temp[index] = packet[i + PKT_INSTRUCTION] 167 | index = index + 1 168 | if packet[i + PKT_INSTRUCTION] == 0xFD \ 169 | and packet[i + PKT_INSTRUCTION - 1] == 0xFF \ 170 | and packet[i + PKT_INSTRUCTION - 2] == 0xFF: 171 | # FF FF FD 172 | temp[index] = 0xFD 173 | index = index + 1 174 | packet_length_out = packet_length_out + 1 175 | 176 | temp[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] 177 | temp[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] 178 | index = index + 2 179 | 180 | if packet_length_in != packet_length_out: 181 | packet = [0] * index 182 | 183 | packet[0: index] = temp[0: index] 184 | 185 | packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out) 186 | packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out) 187 | 188 | return packet 189 | 190 | def removeStuffing(self, packet): 191 | packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]) 192 | packet_length_out = packet_length_in 193 | 194 | index = PKT_INSTRUCTION 195 | for i in range(0, (packet_length_in - 2)): # except CRC 196 | if (packet[i + PKT_INSTRUCTION] == 0xFD) and (packet[i + PKT_INSTRUCTION + 1] == 0xFD) and ( 197 | packet[i + PKT_INSTRUCTION - 1] == 0xFF) and (packet[i + PKT_INSTRUCTION - 2] == 0xFF): 198 | # FF FF FD FD 199 | packet_length_out = packet_length_out - 1 200 | else: 201 | packet[index] = packet[i + PKT_INSTRUCTION] 202 | index += 1 203 | 204 | packet[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] 205 | packet[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] 206 | 207 | packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out) 208 | packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out) 209 | 210 | return packet 211 | 212 | def txPacket(self, port, txpacket): 213 | if port.is_using: 214 | return COMM_PORT_BUSY 215 | port.is_using = True 216 | 217 | # byte stuffing for header 218 | self.addStuffing(txpacket) 219 | 220 | # check max packet length 221 | total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7 222 | # 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H 223 | 224 | if total_packet_length > TXPACKET_MAX_LEN: 225 | port.is_using = False 226 | return COMM_TX_ERROR 227 | 228 | # make packet header 229 | txpacket[PKT_HEADER0] = 0xFF 230 | txpacket[PKT_HEADER1] = 0xFF 231 | txpacket[PKT_HEADER2] = 0xFD 232 | txpacket[PKT_RESERVED] = 0x00 233 | 234 | # add CRC16 235 | crc = self.updateCRC(0, txpacket, total_packet_length - 2) # 2: CRC16 236 | 237 | txpacket[total_packet_length - 2] = DXL_LOBYTE(crc) 238 | txpacket[total_packet_length - 1] = DXL_HIBYTE(crc) 239 | 240 | # tx packet 241 | port.clearPort() 242 | written_packet_length = port.writePort(txpacket) 243 | if total_packet_length != written_packet_length: 244 | port.is_using = False 245 | return COMM_TX_FAIL 246 | 247 | return COMM_SUCCESS 248 | 249 | def rxPacket(self, port): 250 | rxpacket = [] 251 | 252 | result = COMM_TX_FAIL 253 | rx_length = 0 254 | wait_length = 11 # minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) 255 | 256 | while True: 257 | rxpacket.extend(port.readPort(wait_length - rx_length)) 258 | rx_length = len(rxpacket) 259 | if rx_length >= wait_length: 260 | # find packet header 261 | for idx in range(0, (rx_length - 3)): 262 | if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF) and (rxpacket[idx + 2] == 0xFD) and ( 263 | rxpacket[idx + 3] != 0xFD): 264 | break 265 | 266 | if idx == 0: 267 | if (rxpacket[PKT_RESERVED] != 0x00) or (rxpacket[PKT_ID] > 0xFC) or ( 268 | DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN) or ( 269 | rxpacket[PKT_INSTRUCTION] != 0x55): 270 | # remove the first byte in the packet 271 | del rxpacket[0] 272 | rx_length -= 1 273 | continue 274 | 275 | if wait_length != (DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1): 276 | wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1 277 | continue 278 | 279 | if rx_length < wait_length: 280 | if port.isPacketTimeout(): 281 | if rx_length == 0: 282 | result = COMM_RX_TIMEOUT 283 | else: 284 | result = COMM_RX_CORRUPT 285 | break 286 | else: 287 | continue 288 | 289 | crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]) 290 | 291 | if self.updateCRC(0, rxpacket, wait_length - 2) == crc: 292 | result = COMM_SUCCESS 293 | else: 294 | result = COMM_RX_CORRUPT 295 | break 296 | 297 | else: 298 | # remove unnecessary packets 299 | del rxpacket[0: idx] 300 | rx_length -= idx 301 | 302 | else: 303 | if port.isPacketTimeout(): 304 | if rx_length == 0: 305 | result = COMM_RX_TIMEOUT 306 | else: 307 | result = COMM_RX_CORRUPT 308 | break 309 | 310 | port.is_using = False 311 | 312 | if result == COMM_SUCCESS: 313 | rxpacket = self.removeStuffing(rxpacket) 314 | 315 | return rxpacket, result 316 | 317 | # NOT for BulkRead / SyncRead instruction 318 | def txRxPacket(self, port, txpacket): 319 | rxpacket = None 320 | error = 0 321 | 322 | # tx packet 323 | result = self.txPacket(port, txpacket) 324 | if result != COMM_SUCCESS: 325 | return rxpacket, result, error 326 | 327 | # (Instruction == BulkRead or SyncRead) == this function is not available. 328 | if txpacket[PKT_INSTRUCTION] == INST_BULK_READ or txpacket[PKT_INSTRUCTION] == INST_SYNC_READ: 329 | result = COMM_NOT_AVAILABLE 330 | 331 | # (ID == Broadcast ID) == no need to wait for status packet or not available. 332 | # (Instruction == action) == no need to wait for status packet 333 | if txpacket[PKT_ID] == BROADCAST_ID or txpacket[PKT_INSTRUCTION] == INST_ACTION: 334 | port.is_using = False 335 | return rxpacket, result, error 336 | 337 | # set packet timeout 338 | if txpacket[PKT_INSTRUCTION] == INST_READ: 339 | port.setPacketTimeout(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + 11) 340 | else: 341 | port.setPacketTimeout(11) 342 | # HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H 343 | 344 | # rx packet 345 | while True: 346 | rxpacket, result = self.rxPacket(port) 347 | if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: 348 | break 349 | 350 | if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: 351 | error = rxpacket[PKT_ERROR] 352 | 353 | return rxpacket, result, error 354 | 355 | def ping(self, port, dxl_id): 356 | model_number = 0 357 | error = 0 358 | 359 | txpacket = [0] * 10 360 | 361 | if dxl_id >= BROADCAST_ID: 362 | return model_number, COMM_NOT_AVAILABLE, error 363 | 364 | txpacket[PKT_ID] = dxl_id 365 | txpacket[PKT_LENGTH_L] = 3 366 | txpacket[PKT_LENGTH_H] = 0 367 | txpacket[PKT_INSTRUCTION] = INST_PING 368 | 369 | rxpacket, result, error = self.txRxPacket(port, txpacket) 370 | if result == COMM_SUCCESS: 371 | model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]) 372 | 373 | return model_number, result, error 374 | 375 | def broadcastPing(self, port): 376 | data_list = {} 377 | 378 | STATUS_LENGTH = 14 379 | 380 | rx_length = 0 381 | wait_length = STATUS_LENGTH * MAX_ID 382 | 383 | txpacket = [0] * 10 384 | rxpacket = [] 385 | 386 | tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0; 387 | 388 | txpacket[PKT_ID] = BROADCAST_ID 389 | txpacket[PKT_LENGTH_L] = 3 390 | txpacket[PKT_LENGTH_H] = 0 391 | txpacket[PKT_INSTRUCTION] = INST_PING 392 | 393 | result = self.txPacket(port, txpacket) 394 | if result != COMM_SUCCESS: 395 | port.is_using = False 396 | return data_list, result 397 | 398 | # set rx timeout 399 | #port.setPacketTimeout(wait_length * 1) 400 | port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0); 401 | 402 | while True: 403 | rxpacket += port.readPort(wait_length - rx_length) 404 | rx_length = len(rxpacket) 405 | 406 | if port.isPacketTimeout(): # or rx_length >= wait_length 407 | break 408 | 409 | port.is_using = False 410 | 411 | if rx_length == 0: 412 | return data_list, COMM_RX_TIMEOUT 413 | 414 | while True: 415 | if rx_length < STATUS_LENGTH: 416 | return data_list, COMM_RX_CORRUPT 417 | 418 | # find packet header 419 | for idx in range(0, rx_length - 2): 420 | if rxpacket[idx] == 0xFF and rxpacket[idx + 1] == 0xFF and rxpacket[idx + 2] == 0xFD: 421 | break 422 | 423 | if idx == 0: # found at the beginning of the packet 424 | # verify CRC16 425 | crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1]) 426 | 427 | if self.updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc: 428 | result = COMM_SUCCESS 429 | 430 | data_list[rxpacket[PKT_ID]] = [ 431 | DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]), 432 | rxpacket[PKT_PARAMETER0 + 3]] 433 | 434 | del rxpacket[0: STATUS_LENGTH] 435 | rx_length = rx_length - STATUS_LENGTH 436 | 437 | if rx_length == 0: 438 | return data_list, result 439 | 440 | else: 441 | result = COMM_RX_CORRUPT 442 | 443 | # remove header (0xFF 0xFF 0xFD) 444 | del rxpacket[0: 3] 445 | rx_length = rx_length - 3 446 | 447 | else: 448 | # remove unnecessary packets 449 | del rxpacket[0: idx] 450 | rx_length = rx_length - idx 451 | 452 | # FIXME: unreachable code 453 | return data_list, result 454 | 455 | def action(self, port, dxl_id): 456 | txpacket = [0] * 10 457 | 458 | txpacket[PKT_ID] = dxl_id 459 | txpacket[PKT_LENGTH_L] = 3 460 | txpacket[PKT_LENGTH_H] = 0 461 | txpacket[PKT_INSTRUCTION] = INST_ACTION 462 | 463 | _, result, _ = self.txRxPacket(port, txpacket) 464 | return result 465 | 466 | def reboot(self, port, dxl_id): 467 | txpacket = [0] * 10 468 | 469 | txpacket[PKT_ID] = dxl_id 470 | txpacket[PKT_LENGTH_L] = 3 471 | txpacket[PKT_LENGTH_H] = 0 472 | txpacket[PKT_INSTRUCTION] = INST_REBOOT 473 | 474 | _, result, error = self.txRxPacket(port, txpacket) 475 | return result, error 476 | 477 | def clearMultiTurn(self, port, dxl_id): 478 | txpacket = [0] * 15 479 | 480 | txpacket[PKT_ID] = dxl_id 481 | txpacket[PKT_LENGTH_L] = 8 482 | txpacket[PKT_LENGTH_H] = 0 483 | txpacket[PKT_INSTRUCTION] = INST_CLEAR 484 | txpacket[PKT_PARAMETER0 + 0] = 0x01 485 | txpacket[PKT_PARAMETER0 + 1] = 0x44 486 | txpacket[PKT_PARAMETER0 + 2] = 0x58 487 | txpacket[PKT_PARAMETER0 + 3] = 0x4C 488 | txpacket[PKT_PARAMETER0 + 4] = 0x22 489 | 490 | _, result, error = self.txRxPacket(port, txpacket) 491 | return result, error 492 | 493 | def factoryReset(self, port, dxl_id, option): 494 | txpacket = [0] * 11 495 | 496 | txpacket[PKT_ID] = dxl_id 497 | txpacket[PKT_LENGTH_L] = 4 498 | txpacket[PKT_LENGTH_H] = 0 499 | txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET 500 | txpacket[PKT_PARAMETER0] = option 501 | 502 | _, result, error = self.txRxPacket(port, txpacket) 503 | return result, error 504 | 505 | def readTx(self, port, dxl_id, address, length): 506 | txpacket = [0] * 14 507 | 508 | if dxl_id >= BROADCAST_ID: 509 | return COMM_NOT_AVAILABLE 510 | 511 | txpacket[PKT_ID] = dxl_id 512 | txpacket[PKT_LENGTH_L] = 7 513 | txpacket[PKT_LENGTH_H] = 0 514 | txpacket[PKT_INSTRUCTION] = INST_READ 515 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 516 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 517 | txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(length) 518 | txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(length) 519 | 520 | result = self.txPacket(port, txpacket) 521 | 522 | # set packet timeout 523 | if result == COMM_SUCCESS: 524 | port.setPacketTimeout(length + 11) 525 | 526 | return result 527 | 528 | def readRx(self, port, dxl_id, length): 529 | result = COMM_TX_FAIL 530 | error = 0 531 | 532 | rxpacket = None 533 | data = [] 534 | 535 | while True: 536 | rxpacket, result = self.rxPacket(port) 537 | 538 | if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: 539 | break 540 | 541 | if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: 542 | error = rxpacket[PKT_ERROR] 543 | 544 | data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) 545 | 546 | return data, result, error 547 | 548 | def readTxRx(self, port, dxl_id, address, length): 549 | error = 0 550 | 551 | txpacket = [0] * 14 552 | data = [] 553 | 554 | if dxl_id >= BROADCAST_ID: 555 | return data, COMM_NOT_AVAILABLE, error 556 | 557 | txpacket[PKT_ID] = dxl_id 558 | txpacket[PKT_LENGTH_L] = 7 559 | txpacket[PKT_LENGTH_H] = 0 560 | txpacket[PKT_INSTRUCTION] = INST_READ 561 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 562 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 563 | txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(length) 564 | txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(length) 565 | 566 | rxpacket, result, error = self.txRxPacket(port, txpacket) 567 | if result == COMM_SUCCESS: 568 | error = rxpacket[PKT_ERROR] 569 | 570 | data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) 571 | 572 | return data, result, error 573 | 574 | def read1ByteTx(self, port, dxl_id, address): 575 | return self.readTx(port, dxl_id, address, 1) 576 | 577 | def read1ByteRx(self, port, dxl_id): 578 | data, result, error = self.readRx(port, dxl_id, 1) 579 | data_read = data[0] if (result == COMM_SUCCESS) else 0 580 | return data_read, result, error 581 | 582 | def read1ByteTxRx(self, port, dxl_id, address): 583 | data, result, error = self.readTxRx(port, dxl_id, address, 1) 584 | data_read = data[0] if (result == COMM_SUCCESS) else 0 585 | return data_read, result, error 586 | 587 | def read2ByteTx(self, port, dxl_id, address): 588 | return self.readTx(port, dxl_id, address, 2) 589 | 590 | def read2ByteRx(self, port, dxl_id): 591 | data, result, error = self.readRx(port, dxl_id, 2) 592 | data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 593 | return data_read, result, error 594 | 595 | def read2ByteTxRx(self, port, dxl_id, address): 596 | data, result, error = self.readTxRx(port, dxl_id, address, 2) 597 | data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 598 | return data_read, result, error 599 | 600 | def read4ByteTx(self, port, dxl_id, address): 601 | return self.readTx(port, dxl_id, address, 4) 602 | 603 | def read4ByteRx(self, port, dxl_id): 604 | data, result, error = self.readRx(port, dxl_id, 4) 605 | data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), 606 | DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 607 | return data_read, result, error 608 | 609 | def read4ByteTxRx(self, port, dxl_id, address): 610 | data, result, error = self.readTxRx(port, dxl_id, address, 4) 611 | data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), 612 | DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 613 | return data_read, result, error 614 | 615 | def writeTxOnly(self, port, dxl_id, address, length, data): 616 | txpacket = [0] * (length + 12) 617 | 618 | txpacket[PKT_ID] = dxl_id 619 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) 620 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) 621 | txpacket[PKT_INSTRUCTION] = INST_WRITE 622 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 623 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 624 | 625 | txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] 626 | 627 | result = self.txPacket(port, txpacket) 628 | port.is_using = False 629 | 630 | return result 631 | 632 | def writeTxRx(self, port, dxl_id, address, length, data): 633 | txpacket = [0] * (length + 12) 634 | 635 | txpacket[PKT_ID] = dxl_id 636 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) 637 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) 638 | txpacket[PKT_INSTRUCTION] = INST_WRITE 639 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 640 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 641 | 642 | txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] 643 | rxpacket, result, error = self.txRxPacket(port, txpacket) 644 | 645 | return result, error 646 | 647 | def write1ByteTxOnly(self, port, dxl_id, address, data): 648 | data_write = [data] 649 | return self.writeTxOnly(port, dxl_id, address, 1, data_write) 650 | 651 | def write1ByteTxRx(self, port, dxl_id, address, data): 652 | data_write = [data] 653 | return self.writeTxRx(port, dxl_id, address, 1, data_write) 654 | 655 | def write2ByteTxOnly(self, port, dxl_id, address, data): 656 | data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] 657 | return self.writeTxOnly(port, dxl_id, address, 2, data_write) 658 | 659 | def write2ByteTxRx(self, port, dxl_id, address, data): 660 | data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] 661 | return self.writeTxRx(port, dxl_id, address, 2, data_write) 662 | 663 | def write4ByteTxOnly(self, port, dxl_id, address, data): 664 | data_write = [DXL_LOBYTE(DXL_LOWORD(data)), 665 | DXL_HIBYTE(DXL_LOWORD(data)), 666 | DXL_LOBYTE(DXL_HIWORD(data)), 667 | DXL_HIBYTE(DXL_HIWORD(data))] 668 | return self.writeTxOnly(port, dxl_id, address, 4, data_write) 669 | 670 | def write4ByteTxRx(self, port, dxl_id, address, data): 671 | data_write = [DXL_LOBYTE(DXL_LOWORD(data)), 672 | DXL_HIBYTE(DXL_LOWORD(data)), 673 | DXL_LOBYTE(DXL_HIWORD(data)), 674 | DXL_HIBYTE(DXL_HIWORD(data))] 675 | return self.writeTxRx(port, dxl_id, address, 4, data_write) 676 | 677 | def regWriteTxOnly(self, port, dxl_id, address, length, data): 678 | txpacket = [0] * (length + 12) 679 | 680 | txpacket[PKT_ID] = dxl_id 681 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) 682 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) 683 | txpacket[PKT_INSTRUCTION] = INST_REG_WRITE 684 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 685 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 686 | 687 | txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] 688 | 689 | result = self.txPacket(port, txpacket) 690 | port.is_using = False 691 | 692 | return result 693 | 694 | def regWriteTxRx(self, port, dxl_id, address, length, data): 695 | txpacket = [0] * (length + 12) 696 | 697 | txpacket[PKT_ID] = dxl_id 698 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) 699 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) 700 | txpacket[PKT_INSTRUCTION] = INST_REG_WRITE 701 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) 702 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) 703 | 704 | txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] 705 | 706 | _, result, error = self.txRxPacket(port, txpacket) 707 | 708 | return result, error 709 | 710 | def syncReadTx(self, port, start_address, data_length, param, param_length): 711 | txpacket = [0] * (param_length + 14) 712 | # 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 713 | 714 | txpacket[PKT_ID] = BROADCAST_ID 715 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE( 716 | param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 717 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE( 718 | param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 719 | txpacket[PKT_INSTRUCTION] = INST_SYNC_READ 720 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) 721 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) 722 | txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) 723 | txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) 724 | 725 | txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] 726 | 727 | result = self.txPacket(port, txpacket) 728 | if result == COMM_SUCCESS: 729 | port.setPacketTimeout((11 + data_length) * param_length) 730 | 731 | return result 732 | 733 | def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): 734 | txpacket = [0] * (param_length + 14) 735 | # 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 736 | 737 | txpacket[PKT_ID] = BROADCAST_ID 738 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE( 739 | param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 740 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE( 741 | param_length + 7) # 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H 742 | txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE 743 | txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) 744 | txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) 745 | txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) 746 | txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) 747 | 748 | txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] 749 | 750 | _, result, _ = self.txRxPacket(port, txpacket) 751 | 752 | return result 753 | 754 | def bulkReadTx(self, port, param, param_length): 755 | txpacket = [0] * (param_length + 10) 756 | # 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H 757 | 758 | txpacket[PKT_ID] = BROADCAST_ID 759 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H 760 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H 761 | txpacket[PKT_INSTRUCTION] = INST_BULK_READ 762 | 763 | txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] 764 | 765 | result = self.txPacket(port, txpacket) 766 | if result == COMM_SUCCESS: 767 | wait_length = 0 768 | i = 0 769 | while i < param_length: 770 | wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10 771 | i += 5 772 | port.setPacketTimeout(wait_length) 773 | 774 | return result 775 | 776 | def bulkWriteTxOnly(self, port, param, param_length): 777 | txpacket = [0] * (param_length + 10) 778 | # 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H 779 | 780 | txpacket[PKT_ID] = BROADCAST_ID 781 | txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H 782 | txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3) # 3: INST CRC16_L CRC16_H 783 | txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE 784 | 785 | txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] 786 | 787 | _, result, _ = self.txRxPacket(port, txpacket) 788 | 789 | return result 790 | -------------------------------------------------------------------------------- /libgex/dynamixel_sdk/robotis_def.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | ################################################################################ 5 | # Copyright 2017 ROBOTIS CO., LTD. 6 | # 7 | # Licensed under the Apache License, Version 2.0 (the "License"); 8 | # you may not use this file except in compliance with the License. 9 | # You may obtain a copy of the License at 10 | # 11 | # http://www.apache.org/licenses/LICENSE-2.0 12 | # 13 | # Unless required by applicable law or agreed to in writing, software 14 | # distributed under the License is distributed on an "AS IS" BASIS, 15 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | # See the License for the specific language governing permissions and 17 | # limitations under the License. 18 | ################################################################################ 19 | 20 | # Author: Ryu Woon Jung (Leon) 21 | 22 | BROADCAST_ID = 0xFE # 254 23 | MAX_ID = 0xFC # 252 24 | 25 | # Instruction for DXL Protocol 26 | INST_PING = 1 27 | INST_READ = 2 28 | INST_WRITE = 3 29 | INST_REG_WRITE = 4 30 | INST_ACTION = 5 31 | INST_FACTORY_RESET = 6 32 | INST_CLEAR = 16 33 | INST_SYNC_WRITE = 131 # 0x83 34 | INST_BULK_READ = 146 # 0x92 35 | # --- Only for 2.0 --- 36 | INST_REBOOT = 8 37 | INST_STATUS = 85 # 0x55 38 | INST_SYNC_READ = 130 # 0x82 39 | INST_BULK_WRITE = 147 # 0x93 40 | 41 | # Communication Result 42 | COMM_SUCCESS = 0 # tx or rx packet communication success 43 | COMM_PORT_BUSY = -1000 # Port is busy (in use) 44 | COMM_TX_FAIL = -1001 # Failed transmit instruction packet 45 | COMM_RX_FAIL = -1002 # Failed get status packet 46 | COMM_TX_ERROR = -2000 # Incorrect instruction packet 47 | COMM_RX_WAITING = -3000 # Now recieving status packet 48 | COMM_RX_TIMEOUT = -3001 # There is no status packet 49 | COMM_RX_CORRUPT = -3002 # Incorrect status packet 50 | COMM_NOT_AVAILABLE = -9000 # 51 | 52 | 53 | # Macro for Control Table Value 54 | def DXL_MAKEWORD(a, b): 55 | return (a & 0xFF) | ((b & 0xFF) << 8) 56 | 57 | 58 | def DXL_MAKEDWORD(a, b): 59 | return (a & 0xFFFF) | (b & 0xFFFF) << 16 60 | 61 | 62 | def DXL_LOWORD(l): 63 | return l & 0xFFFF 64 | 65 | 66 | def DXL_HIWORD(l): 67 | return (l >> 16) & 0xFFFF 68 | 69 | 70 | def DXL_LOBYTE(w): 71 | return w & 0xFF 72 | 73 | 74 | def DXL_HIBYTE(w): 75 | return (w >> 8) & 0xFF 76 | -------------------------------------------------------------------------------- /libgex/ex12/kinematics.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import os.path as osp 3 | import numpy as np 4 | abs_path = osp.dirname(osp.abspath(__file__)) 5 | 6 | 7 | 8 | class KinEX12: 9 | def __init__(self) -> None: 10 | self.name = 'EX12' 11 | p.connect(p.DIRECT) 12 | self.offset_z = 3 13 | 14 | self.bullet_hand = p.loadURDF(osp.join(abs_path, "urdf/ex12.urdf"), useFixedBase=True, basePosition=[0, 0, self.offset_z]) 15 | for i in range(20): 16 | p.stepSimulation() 17 | 18 | self.thumb_link_id = 4 19 | self.thumb_joint_ids = [0, 1, 2, 3] 20 | 21 | self.index_link_id = 9 22 | self.index_joint_ids = [5, 6, 7, 8] 23 | 24 | self.middle_link_id = 14 25 | self.middle_joint_ids = [10, 11, 12, 13] 26 | 27 | self.direction_finger1 = [1, -1, -1, -1] 28 | self.direction_finger2 = [-1, 1, 1, 1] 29 | self.direction_finger3 = [-1, -1, -1, -1] 30 | 31 | def fk_finger1(self, q=[0]*4): 32 | """ 33 | finger1 正运动学，3自由度 34 | """ 35 | # 匹配关节方向，degree to rad 36 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger1)] 37 | 38 | 39 | for i, joint_position in zip(self.thumb_joint_ids, q): 40 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 41 | 42 | for i in range(20): 43 | p.stepSimulation() 44 | 45 | ee_pos = p.getLinkState(self.bullet_hand, self.thumb_link_id, computeForwardKinematics=1)[4] 46 | 47 | ee_pos = list(ee_pos) 48 | ee_pos[2] -= self.offset_z 49 | 50 | return ee_pos 51 | 52 | def ik_finger1(self, xyz): 53 | # 计算逆运动学 54 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.thumb_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 55 | 56 | q = joint_positions[:4] 57 | # 匹配关节方向，rad to degree 58 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger1)] 59 | 60 | return q 61 | 62 | 63 | def fk_finger2(self, q=[0]*4): 64 | """ 65 | finger2 正运动学，4自由度 66 | """ 67 | # 匹配关节方向，degree to rad 68 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger2)] 69 | 70 | 71 | for i, joint_position in zip(self.index_joint_ids, q): 72 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 73 | 74 | for i in range(20): 75 | p.stepSimulation() 76 | 77 | ee_pos = p.getLinkState(self.bullet_hand, self.index_link_id, computeForwardKinematics=1)[4] 78 | 79 | ee_pos = list(ee_pos) 80 | ee_pos[2] -= self.offset_z 81 | 82 | return ee_pos 83 | 84 | def ik_finger2(self, xyz): 85 | # 计算逆运动学 86 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.index_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 87 | 88 | q = joint_positions[4:8] 89 | 90 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger2)] 91 | 92 | return q 93 | 94 | def fk_finger3(self, q=[0]*4): 95 | """ 96 | finger3 正运动学，4自由度 97 | """ 98 | # 匹配关节方向，degree to rad 99 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger3)] 100 | 101 | 102 | for i, joint_position in zip(self.middle_joint_ids, q): 103 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 104 | 105 | for i in range(20): 106 | p.stepSimulation() 107 | 108 | ee_pos = p.getLinkState(self.bullet_hand, self.middle_link_id, computeForwardKinematics=1)[4] 109 | 110 | ee_pos = list(ee_pos) 111 | ee_pos[2] -= self.offset_z 112 | 113 | return ee_pos 114 | 115 | def ik_finger3(self, xyz): 116 | # 计算逆运动学 117 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.middle_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 118 | 119 | q = joint_positions[8:12] 120 | # 匹配关节方向，rad to degree 121 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger3)] 122 | 123 | return q 124 | 125 | if __name__ == "__main__": 126 | kin = KinGX11() 127 | -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link1.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link10.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link10.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link11.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link11.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link12.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link12.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link13.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link13.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link14.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link14.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link15.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link15.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link2.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link3.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link4.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link5.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link6.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link7.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link8.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link8.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/Link9.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/Link9.STL -------------------------------------------------------------------------------- /libgex/ex12/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/ex12/meshes/base_link.STL -------------------------------------------------------------------------------- /libgex/ex12/test_ik.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data 3 | 4 | import time 5 | 6 | 7 | p.connect(p.GUI) 8 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally 9 | 10 | # p.loadURDF("plane.urdf") 11 | 12 | # 取消碰撞检测 13 | hand = p.loadURDF("urdf/ex12.urdf", useFixedBase=True, basePosition=[0, 0, 0]) 14 | p.setGravity(0, 0, -9.8) 15 | 16 | 17 | valid_joints = [] 18 | for i in range(p.getNumJoints(hand)): 19 | info = p.getJointInfo(hand, i) 20 | print(info[1]) 21 | if info[2] == p.JOINT_REVOLUTE: 22 | 23 | valid_joints.append(i) 24 | 25 | 26 | print(valid_joints) 27 | # 增加三个xyz的拖动条 28 | 29 | tx = p.addUserDebugParameter(f"TX", 0, 0.3, 0.02) 30 | ty = p.addUserDebugParameter(f"TY", 0, 0.3, 0.06) 31 | tz = p.addUserDebugParameter(f"TZ", 0., 0.6, 0.1) 32 | 33 | sliders = [tx, ty, tz] 34 | 35 | # 逆运动学 36 | while True: 37 | xyz = [p.readUserDebugParameter(i) for i in sliders] 38 | 39 | # 计算逆运动学 40 | joint_positions = p.calculateInverseKinematics(hand, 14, xyz, maxNumIterations=300, residualThreshold=1e-4) 41 | 42 | joint_positions = [0]*12 43 | 44 | # print(len(joint_positions)) 45 | # 设置关节位置 46 | for i, joint_position in zip(valid_joints, joint_positions): 47 | p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, joint_position) 48 | 49 | p.stepSimulation() 50 | 51 | xyz_now5 = p.getLinkState(hand, 14, computeForwardKinematics=1)[4] 52 | # xyz_now5 = p.getLinkState(hand, 3, computeForwardKinematics=1)[0] 53 | 54 | print(f'target: {xyz[0]:.3f}, {xyz[1]:.3f}, {xyz[2]:.3f}, now5: {xyz_now5[0]:.3f}, {xyz_now5[1]:.3f}, {xyz_now5[2]:.3f}') 55 | 56 | time.sleep(1./240.) 57 | # while True: 58 | # p.stepSimulation() 59 | # time.sleep(1./240.) 60 | 61 | -------------------------------------------------------------------------------- /libgex/ex12/urdf/ex12.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 33 | 35 | 36 | 37 | 38 | 41 | 42 | 44 | 45 | 46 | 47 | 49 | 50 | 53 | 55 | 62 | 63 | 64 | 67 | 68 | 70 | 71 | 73 | 75 | 76 | 77 | 78 | 81 | 82 | 84 | 85 | 86 | 87 | 90 | 93 | 95 | 97 | 99 | 104 | 105 | 107 | 108 | 111 | 113 | 120 | 121 | 122 | 125 | 126 | 128 | 129 | 131 | 133 | 134 | 135 | 136 | 139 | 140 | 142 | 143 | 144 | 145 | 148 | 151 | 153 | 155 | 157 | 162 | 163 | 165 | 166 | 169 | 171 | 178 | 179 | 180 | 183 | 184 | 186 | 187 | 189 | 191 | 192 | 193 | 194 | 197 | 198 | 200 | 201 | 202 | 203 | 206 | 209 | 211 | 213 | 215 | 220 | 221 | 223 | 224 | 227 | 229 | 236 | 237 | 238 | 241 | 242 | 244 | 245 | 247 | 249 | 250 | 251 | 252 | 255 | 256 | 258 | 259 | 260 | 261 | 264 | 267 | 269 | 271 | 273 | 278 | 279 | 281 | 282 | 285 | 287 | 294 | 295 | 296 | 299 | 300 | 302 | 303 | 305 | 307 | 308 | 309 | 310 | 313 | 314 | 316 | 317 | 318 | 319 | 322 | 325 | 327 | 329 | 331 | 332 | 334 | 335 | 338 | 340 | 347 | 348 | 349 | 352 | 353 | 355 | 356 | 358 | 360 | 361 | 362 | 363 | 366 | 367 | 369 | 370 | 371 | 372 | 375 | 378 | 380 | 382 | 384 | 389 | 390 | 392 | 393 | 396 | 398 | 405 | 406 | 407 | 410 | 411 | 413 | 414 | 416 | 418 | 419 | 420 | 421 | 424 | 425 | 427 | 428 | 429 | 430 | 433 | 436 | 438 | 440 | 442 | 447 | 448 | 450 | 451 | 454 | 456 | 463 | 464 | 465 | 468 | 469 | 471 | 472 | 474 | 476 | 477 | 478 | 479 | 482 | 483 | 485 | 486 | 487 | 488 | 491 | 494 | 496 | 498 | 500 | 505 | 506 | 508 | 509 | 512 | 514 | 521 | 522 | 523 | 526 | 527 | 529 | 530 | 532 | 534 | 535 | 536 | 537 | 540 | 541 | 543 | 544 | 545 | 546 | 549 | 552 | 554 | 556 | 558 | 563 | 564 | 566 | 567 | 570 | 572 | 579 | 580 | 581 | 584 | 585 | 587 | 588 | 590 | 592 | 593 | 594 | 595 | 598 | 599 | 601 | 602 | 603 | 604 | 607 | 610 | 612 | 614 | 616 | 617 | 619 | 620 | 623 | 625 | 632 | 633 | 634 | 637 | 638 | 640 | 641 | 643 | 645 | 646 | 647 | 648 | 651 | 652 | 654 | 655 | 656 | 657 | 660 | 663 | 665 | 667 | 669 | 674 | 675 | 677 | 678 | 681 | 683 | 690 | 691 | 692 | 695 | 696 | 698 | 699 | 701 | 703 | 704 | 705 | 706 | 709 | 710 | 712 | 713 | 714 | 715 | 718 | 721 | 723 | 725 | 727 | 732 | 733 | 735 | 736 | 739 | 741 | 748 | 749 | 750 | 753 | 754 | 756 | 757 | 759 | 761 | 762 | 763 | 764 | 767 | 768 | 770 | 771 | 772 | 773 | 776 | 779 | 781 | 783 | 785 | 790 | 791 | 793 | 794 | 797 | 799 | 806 | 807 | 808 | 811 | 812 | 814 | 815 | 817 | 819 | 820 | 821 | 822 | 825 | 826 | 828 | 829 | 830 | 831 | 834 | 837 | 839 | 841 | 843 | 848 | 849 | 851 | 852 | 855 | 857 | 864 | 865 | 866 | 869 | 870 | 872 | 873 | 875 | 877 | 878 | 879 | 880 | 883 | 884 | 886 | 887 | 888 | 889 | 892 | 895 | 897 | 899 | 901 | 902 | -------------------------------------------------------------------------------- /libgex/gx11/kinematics.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import os.path as osp 3 | import numpy as np 4 | abs_path = osp.dirname(osp.abspath(__file__)) 5 | 6 | 7 | 8 | class KinGX11: 9 | def __init__(self) -> None: 10 | self.name = 'GX11' 11 | p.connect(p.DIRECT) 12 | 13 | self.bullet_hand = p.loadURDF(osp.join(abs_path, "urdf/gx11.urdf"), useFixedBase=True, basePosition=[0, 0, 0]) 14 | for i in range(20): 15 | p.stepSimulation() 16 | 17 | self.thumb_link_id = 3 18 | self.thumb_joint_ids = [0, 1, 2] 19 | 20 | self.index_link_id = 8 21 | self.index_joint_ids = [4, 5, 6, 7] 22 | 23 | self.middle_link_id = 13 24 | self.middle_joint_ids = [9, 10, 11, 12] 25 | 26 | self.direction_finger1 = [1, 1, 1] 27 | self.direction_finger2 = [1, -1, -1, -1] 28 | self.direction_finger3 = [-1, -1, 1, 1] 29 | 30 | def fk_finger1(self, q=[0]*3): 31 | """ 32 | finger1 正运动学，3自由度 33 | """ 34 | # 匹配关节方向，degree to rad 35 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger1)] 36 | 37 | for i, joint_position in zip(self.thumb_joint_ids, q): 38 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 39 | 40 | for i in range(20): 41 | p.stepSimulation() 42 | 43 | ee_pos = p.getLinkState(self.bullet_hand, self.thumb_link_id, computeForwardKinematics=1)[4] 44 | 45 | return ee_pos 46 | 47 | def ik_finger1(self, xyz): 48 | # 计算逆运动学 49 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.thumb_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 50 | 51 | q = joint_positions[:3] 52 | # 匹配关节方向，rad to degree 53 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger1)] 54 | 55 | return q 56 | 57 | 58 | def fk_finger2(self, q=[0]*4): 59 | """ 60 | finger2 正运动学，4自由度 61 | """ 62 | # 匹配关节方向，degree to rad 63 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger2)] 64 | 65 | for i, joint_position in zip(self.index_joint_ids, q): 66 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 67 | 68 | for i in range(20): 69 | p.stepSimulation() 70 | 71 | ee_pos = p.getLinkState(self.bullet_hand, self.index_link_id, computeForwardKinematics=1)[4] 72 | 73 | return ee_pos 74 | 75 | def ik_finger2(self, xyz): 76 | # 计算逆运动学 77 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.index_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 78 | 79 | q = joint_positions[3:7] 80 | 81 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger2)] 82 | 83 | return q 84 | 85 | def fk_finger3(self, q=[0]*4): 86 | """ 87 | finger3 正运动学，4自由度 88 | """ 89 | # 匹配关节方向，degree to rad 90 | q = [q_*d*np.pi/180 for q_, d in zip(q, self.direction_finger3)] 91 | 92 | for i, joint_position in zip(self.middle_joint_ids, q): 93 | p.setJointMotorControl2(self.bullet_hand, i, p.POSITION_CONTROL, joint_position) 94 | 95 | for i in range(20): 96 | p.stepSimulation() 97 | 98 | ee_pos = p.getLinkState(self.bullet_hand, self.middle_link_id, computeForwardKinematics=1)[4] 99 | 100 | return ee_pos 101 | 102 | def ik_finger3(self, xyz): 103 | # 计算逆运动学 104 | joint_positions = p.calculateInverseKinematics(self.bullet_hand, self.middle_link_id, xyz, maxNumIterations=300, residualThreshold=1e-4) 105 | 106 | q = joint_positions[7:11] 107 | # 匹配关节方向，rad to degree 108 | q = [q_*d*180/np.pi for q_, d in zip(q, self.direction_finger3)] 109 | 110 | return q 111 | 112 | if __name__ == "__main__": 113 | kin = KinGX11() 114 | -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link1.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link10.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link10.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link11.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link11.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link12.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link12.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link13.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link13.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link14.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link14.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link2.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link3.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link4.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link5.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link6.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link7.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link8.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link8.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/Link9.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/Link9.STL -------------------------------------------------------------------------------- /libgex/gx11/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Democratizing-Dexterous/libgex/1edde3c08b44b2952b5643b2b171a0958ab21988/libgex/gx11/meshes/base_link.STL -------------------------------------------------------------------------------- /libgex/gx11/test_ik.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data 3 | 4 | import time 5 | 6 | 7 | p.connect(p.GUI) 8 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally 9 | 10 | # p.loadURDF("plane.urdf") 11 | 12 | hand = p.loadURDF("urdf/gx11.urdf", useFixedBase=True, basePosition=[0, 0, 0]) 13 | p.setGravity(0, 0, -9.8) 14 | 15 | 16 | valid_joints = [] 17 | for i in range(p.getNumJoints(hand)): 18 | info = p.getJointInfo(hand, i) 19 | print(info[1]) 20 | if info[2] == p.JOINT_REVOLUTE: 21 | 22 | valid_joints.append(i) 23 | 24 | 25 | # 增加三个xyz的拖动条 26 | 27 | tx = p.addUserDebugParameter(f"TX", 0, 0.3, 0.03) 28 | ty = p.addUserDebugParameter(f"TY", 0, 0.3, 0.06) 29 | tz = p.addUserDebugParameter(f"TZ", 0., 0.6, 0.1) 30 | 31 | sliders = [tx, ty, tz] 32 | 33 | # 逆运动学 34 | while True: 35 | xyz = [p.readUserDebugParameter(i) for i in sliders] 36 | 37 | # 计算逆运动学 38 | joint_positions = p.calculateInverseKinematics(hand, 3, xyz, maxNumIterations=300, residualThreshold=1e-4) 39 | 40 | # print(len(joint_positions)) 41 | # 设置关节位置 42 | for i, joint_position in zip(valid_joints, joint_positions): 43 | p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, joint_position) 44 | 45 | p.stepSimulation() 46 | 47 | xyz_now5 = p.getLinkState(hand, 3, computeForwardKinematics=1)[4] 48 | # xyz_now1 = p.getLinkState(hand, 3, computeForwardKinematics=1)[0] 49 | 50 | # print(f'target: {xyz[0]:.3f}, {xyz[1]:.3f}, {xyz[2]:.3f}, now5: {xyz_now5[0]:.3f}, {xyz_now5[1]:.3f}, {xyz_now5[2]:.3f}') 51 | 52 | time.sleep(1./240.) 53 | # while True: 54 | # p.stepSimulation() 55 | # time.sleep(1./240.) 56 | 57 | -------------------------------------------------------------------------------- /libgex/gx11/urdf/finger1.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 39 | 40 | 42 | 43 | 44 | 45 | 47 | 48 | 51 | 53 | 60 | 61 | 62 | 65 | 66 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 77 | 78 | 80 | 81 | 82 | 83 | 86 | 89 | 91 | 93 | 95 | 100 | 101 | 103 | 104 | 107 | 109 | 116 | 117 | 118 | 121 | 122 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 133 | 134 | 136 | 137 | 138 | 139 | 142 | 145 | 147 | 149 | 151 | 156 | 157 | 159 | 160 | 163 | 165 | 172 | 173 | 174 | 177 | 178 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 189 | 190 | 192 | 193 | 194 | 195 | 198 | 201 | 203 | 205 | 207 | 212 | 213 | 215 | 216 | 219 | 221 | 228 | 229 | 230 | 233 | 234 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 245 | 246 | 248 | 249 | 250 | 251 | 254 | 257 | 259 | 261 | 263 | 268 | 269 | 270 | -------------------------------------------------------------------------------- /libgex/gx11/urdf/gx11.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 39 | 40 | 42 | 43 | 44 | 45 | 47 | 48 | 51 | 53 | 60 | 61 | 62 | 65 | 66 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 77 | 78 | 80 | 81 | 82 | 83 | 86 | 89 | 91 | 93 | 95 | 100 | 101 | 103 | 104 | 107 | 109 | 116 | 117 | 118 | 121 | 122 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 133 | 134 | 136 | 137 | 138 | 139 | 142 | 145 | 147 | 149 | 151 | 156 | 157 | 159 | 160 | 163 | 165 | 172 | 173 | 174 | 177 | 178 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 189 | 190 | 192 | 193 | 194 | 195 | 198 | 201 | 203 | 205 | 207 | 212 | 213 | 215 | 216 | 219 | 221 | 228 | 229 | 230 | 233 | 234 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 245 | 246 | 248 | 249 | 250 | 251 | 254 | 257 | 259 | 261 | 263 | 268 | 269 | 271 | 272 | 275 | 277 | 284 | 285 | 286 | 289 | 290 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 301 | 302 | 304 | 305 | 306 | 307 | 310 | 313 | 315 | 317 | 319 | 324 | 325 | 327 | 328 | 331 | 333 | 340 | 341 | 342 | 345 | 346 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 357 | 358 | 360 | 361 | 362 | 363 | 366 | 369 | 371 | 373 | 375 | 380 | 381 | 383 | 384 | 387 | 389 | 396 | 397 | 398 | 401 | 402 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 413 | 414 | 416 | 417 | 418 | 419 | 422 | 425 | 427 | 429 | 431 | 436 | 437 | 439 | 440 | 443 | 445 | 452 | 453 | 454 | 457 | 458 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 469 | 470 | 472 | 473 | 474 | 475 | 478 | 481 | 483 | 485 | 487 | 492 | 493 | 495 | 496 | 499 | 501 | 508 | 509 | 510 | 513 | 514 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 525 | 526 | 528 | 529 | 530 | 531 | 534 | 537 | 539 | 541 | 543 | 544 | 546 | 547 | 550 | 552 | 559 | 560 | 561 | 564 | 565 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 576 | 577 | 579 | 580 | 581 | 582 | 585 | 588 | 590 | 592 | 594 | 599 | 600 | 602 | 603 | 606 | 608 | 615 | 616 | 617 | 620 | 621 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 632 | 633 | 635 | 636 | 637 | 638 | 641 | 644 | 646 | 648 | 650 | 655 | 656 | 658 | 659 | 662 | 664 | 671 | 672 | 673 | 676 | 677 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 688 | 689 | 691 | 692 | 693 | 694 | 697 | 700 | 702 | 704 | 706 | 711 | 712 | 714 | 715 | 718 | 720 | 727 | 728 | 729 | 732 | 733 | 735 | 736 | 737 | 738 | 739 | 740 | 741 | 744 | 745 | 747 | 748 | 749 | 750 | 753 | 756 | 758 | 760 | 762 | 767 | 768 | 770 | 771 | 774 | 776 | 783 | 784 | 785 | 788 | 789 | 791 | 792 | 793 | 794 | 795 | 796 | 797 | 800 | 801 | 803 | 804 | 805 | 806 | 809 | 812 | 814 | 816 | 818 | 819 | -------------------------------------------------------------------------------- /libgex/libex12.py: -------------------------------------------------------------------------------- 1 | import time 2 | from .dynamixel_sdk import PortHandler, PacketHandler 3 | from .motor import Motor 4 | import sys 5 | import numpy as np 6 | from .config import BAUDRATE, PROTOCOL_VERSION 7 | from .ex12 import kinematics 8 | 9 | class Glove: 10 | def __init__(self, port) -> None: 11 | self.is_connected = False 12 | self.port = port 13 | self.name = 'EX12' 14 | self.kin = kinematics.KinEX12() 15 | 16 | def connect(self, goal_pwm=300, init=True): 17 | """ 18 | 连接Hand，并且使能每个电机为默认的力控位置模式 19 | """ 20 | 21 | portHandler = PortHandler(self.port) 22 | packetHandler = PacketHandler(PROTOCOL_VERSION) 23 | 24 | if portHandler.openPort() and portHandler.setBaudRate(BAUDRATE): 25 | print(f'Open {self.port} Success...') 26 | self.is_connected = True 27 | else: 28 | print(f'Failed...') 29 | self.is_connected = False 30 | sys.exit(0) 31 | 32 | self.portHandler = portHandler 33 | self.packetHandler = packetHandler 34 | 35 | self.motors = [Motor(i+1, portHandler, packetHandler) for i in range(12)] 36 | 37 | if init: 38 | for m in self.motors: 39 | m.init_config(goal_pwm=goal_pwm) 40 | 41 | print(f'{self.name} init done...') 42 | 43 | 44 | def off(self): 45 | """ 46 | 失能所有电机 47 | """ 48 | for m in self.motors: 49 | m.torq_off() 50 | 51 | def on(self): 52 | """ 53 | 使能所有电机 54 | """ 55 | for m in self.motors: 56 | m.torq_on() 57 | 58 | 59 | def home(self): 60 | """ 61 | GX11会到原点 62 | """ 63 | motors = self.motors 64 | for m in motors: 65 | m.set_pos(0) 66 | time.sleep(1) 67 | 68 | def getj(self): 69 | """ 70 | 获取GX11关节角度，单位度 71 | """ 72 | js = [m.get_pos() for m in self.motors] 73 | 74 | js_rect = [] 75 | for j in js: 76 | if j > 180: 77 | j -= 360 78 | if j < -180: 79 | j += 360 80 | js_rect.append(j) 81 | 82 | 83 | return js_rect 84 | 85 | def setj(self, js): 86 | """ 87 | 设置GX11关节角度，单位度 88 | """ 89 | for m, j in zip(self.motors, js): 90 | m.set_pos(j) 91 | 92 | def getj_finger1(self): 93 | """ 94 | 获取GX11指1关节角度，单位度 95 | """ 96 | js = [m.get_pos() for m in self.motors[0:4]] 97 | js_rect = [] 98 | for j in js: 99 | if j > 180: 100 | j -= 360 101 | if j < -180: 102 | j += 360 103 | js_rect.append(j) 104 | 105 | 106 | return js_rect 107 | 108 | def setj_finger1(self, js): 109 | """ 110 | 设置GX11指1关节角度，单位度 111 | """ 112 | for m, j in zip(self.motors[0:4], js): 113 | m.set_pos(j) 114 | 115 | def getj_finger2(self): 116 | """ 117 | 获取GX11指2关节角度，单位度 118 | """ 119 | js = [m.get_pos() for m in self.motors[4:8]] 120 | js_rect = [] 121 | for j in js: 122 | if j > 180: 123 | j -= 360 124 | if j < -180: 125 | j += 360 126 | js_rect.append(j) 127 | 128 | 129 | return js_rect 130 | 131 | def setj_finger2(self, js): 132 | """ 133 | 设置GX11指2关节角度，单位度 134 | """ 135 | for m, j in zip(self.motors[4:8], js): 136 | m.set_pos(j) 137 | 138 | def getj_finger3(self): 139 | """ 140 | 获取GX11指3关节角度，单位度 141 | """ 142 | js = [m.get_pos() for m in self.motors[8:12]] 143 | js_rect = [] 144 | for j in js: 145 | if j > 180: 146 | j -= 360 147 | if j < -180: 148 | j += 360 149 | js_rect.append(j) 150 | 151 | 152 | return js_rect 153 | 154 | def setj_finger3(self, js): 155 | """ 156 | 设置GX11指3关节角度，单位度 157 | """ 158 | for m, j in zip(self.motors[8:12], js): 159 | m.set_pos(j) 160 | 161 | def set_zero_whole_hand(self): 162 | """ 163 | GX11手掌设置编码器以当前位置归0，慎用 164 | """ 165 | for m in self.motors[4:12]: 166 | m.set_zero() 167 | 168 | 169 | def fk_finger1(self): 170 | """获取finger1的正运动学，单位m""" 171 | xyz = self.kin.fk_finger1(self.getj_finger1()) 172 | 173 | return xyz 174 | 175 | def ik_finger1(self, xyz): 176 | """获取finger1的逆运动学，单位m""" 177 | q = self.kin.ik_finger1(xyz) 178 | 179 | return q 180 | 181 | 182 | def fk_finger2(self): 183 | """获取finger2的正运动学，单位m""" 184 | xyz = self.kin.fk_finger2(self.getj_finger2()) 185 | 186 | return xyz 187 | 188 | def ik_finger2(self, xyz): 189 | """获取finger2的逆运动学，单位m""" 190 | q = self.kin.ik_finger2(xyz) 191 | 192 | return q 193 | 194 | def fk_finger3(self): 195 | """获取finger3的正运动学，单位m""" 196 | xyz = self.kin.fk_finger3(self.getj_finger3()) 197 | 198 | return xyz 199 | 200 | def ik_finger3(self, xyz): 201 | """获取finger3的逆运动学，单位m""" 202 | q = self.kin.ik_finger3(xyz) 203 | 204 | return q -------------------------------------------------------------------------------- /libgex/libgx11.py: -------------------------------------------------------------------------------- 1 | import time 2 | from .dynamixel_sdk import PortHandler, PacketHandler 3 | from .motor import Motor 4 | import sys 5 | import numpy as np 6 | from .config import BAUDRATE, PROTOCOL_VERSION 7 | from .gx11 import kinematics 8 | 9 | class Hand: 10 | def __init__(self, port) -> None: 11 | self.is_connected = False 12 | self.port = port 13 | self.name = 'GX11' 14 | self.kin = kinematics.KinGX11() 15 | 16 | def connect(self, goal_pwm=300): 17 | """ 18 | 连接Hand，并且使能每个电机为默认的力控位置模式 19 | """ 20 | 21 | portHandler = PortHandler(self.port) 22 | packetHandler = PacketHandler(PROTOCOL_VERSION) 23 | 24 | if portHandler.openPort() and portHandler.setBaudRate(BAUDRATE): 25 | print(f'Open {self.port} Success...') 26 | self.is_connected = True 27 | else: 28 | print(f'Failed...') 29 | self.is_connected = False 30 | sys.exit(0) 31 | 32 | self.portHandler = portHandler 33 | self.packetHandler = packetHandler 34 | 35 | self.motors = [Motor(i+1, portHandler, packetHandler) for i in range(11)] 36 | 37 | for m in self.motors: 38 | m.init_config(goal_pwm=goal_pwm) 39 | 40 | print(f'{self.name} init done...') 41 | 42 | def off(self): 43 | """ 44 | 失能所有电机 45 | """ 46 | for m in self.motors: 47 | m.torq_off() 48 | 49 | def on(self): 50 | """ 51 | 使能所有电机 52 | """ 53 | for m in self.motors: 54 | m.torq_on() 55 | 56 | 57 | def home(self): 58 | """ 59 | GX11会到原点 60 | """ 61 | motors = self.motors 62 | for m in motors: 63 | m.set_pos(0) 64 | time.sleep(1) 65 | 66 | def getj(self): 67 | """ 68 | 获取GX11关节角度，单位度 69 | """ 70 | js = [m.get_pos() for m in self.motors] 71 | return js 72 | 73 | def setj(self, js): 74 | """ 75 | 设置GX11关节角度，单位度 76 | """ 77 | for m, j in zip(self.motors, js): 78 | m.set_pos(j) 79 | 80 | def getj_finger1(self): 81 | """ 82 | 获取GX11指1关节角度，单位度 83 | """ 84 | js = [m.get_pos() for m in self.motors[0:3]] 85 | return js 86 | 87 | def setj_finger1(self, js): 88 | """ 89 | 设置GX11指1关节角度，单位度 90 | """ 91 | for m, j in zip(self.motors[0:3], js): 92 | m.set_pos(j) 93 | 94 | def getj_finger2(self): 95 | """ 96 | 获取GX11指2关节角度，单位度 97 | """ 98 | js = [m.get_pos() for m in self.motors[3:7]] 99 | return js 100 | 101 | def setj_finger2(self, js): 102 | """ 103 | 设置GX11指2关节角度，单位度 104 | """ 105 | for m, j in zip(self.motors[3:7], js): 106 | m.set_pos(j) 107 | 108 | def getj_finger3(self): 109 | """ 110 | 获取GX11指3关节角度，单位度 111 | """ 112 | js = [m.get_pos() for m in self.motors[7:11]] 113 | return js 114 | 115 | def setj_finger3(self, js): 116 | """ 117 | 设置GX11指3关节角度，单位度 118 | """ 119 | for m, j in zip(self.motors[7:11], js): 120 | m.set_pos(j) 121 | 122 | def set_zero_whole_hand(self): 123 | """ 124 | GX11手掌设置编码器以当前位置归0，慎用 125 | """ 126 | for m in self.motors: 127 | m.set_zero() 128 | 129 | 130 | def fk_finger1(self): 131 | """获取finger1的正运动学，单位m""" 132 | xyz = self.kin.fk_finger1(self.getj_finger1()) 133 | 134 | return xyz 135 | 136 | def ik_finger1(self, xyz): 137 | """获取finger1的逆运动学，单位m""" 138 | q = self.kin.ik_finger1(xyz) 139 | 140 | return q 141 | 142 | 143 | def fk_finger2(self): 144 | """获取finger2的正运动学，单位m""" 145 | xyz = self.kin.fk_finger2(self.getj_finger2()) 146 | 147 | return xyz 148 | 149 | def ik_finger2(self, xyz): 150 | """获取finger2的逆运动学，单位m""" 151 | q = self.kin.ik_finger2(xyz) 152 | 153 | return q 154 | 155 | def fk_finger3(self): 156 | """获取finger3的正运动学，单位m""" 157 | xyz = self.kin.fk_finger3(self.getj_finger3()) 158 | 159 | return xyz 160 | 161 | def ik_finger3(self, xyz): 162 | """获取finger3的逆运动学，单位m""" 163 | q = self.kin.ik_finger3(xyz) 164 | 165 | return q -------------------------------------------------------------------------------- /libgex/motor.py: -------------------------------------------------------------------------------- 1 | from .dynamixel_sdk import * 2 | import os 3 | import time 4 | import numpy as np 5 | 6 | class Motor: 7 | def __init__(self, id, portHandler, packetHandler) -> None: 8 | self.id = id 9 | self.unit_scale = 0.087891 # degree 10 | self.port_handler = portHandler 11 | self.packet_handler = packetHandler 12 | 13 | self.addr_torq_enable = 64 14 | self.addr_led = 65 15 | self.addr_present_pos = 132 16 | self.addr_present_current = 126 17 | self.addr_goal_pos = 116 18 | self.addr_curr_limit = 38 19 | self.addr_pos_p = 84 20 | self.addr_pos_d = 80 21 | self.addr_vel_limit = 44 22 | 23 | self.addr_home_off = 20 24 | self.addr_goal_curr = 102 25 | self.addr_goal_pwm = 100 26 | 27 | # 运行模式 28 | self.addr_operating_mode = 11 29 | self.curr_operating_mode = 0 # 电流模式 30 | self.vel_operating_mode = 1 31 | self.pos_operating_mode = 3 32 | self.extpos_operating_mode = 4 33 | self.currpos_operating_mode = 5 # 推荐使用，位置力控模式 34 | self.pwm_operating_mode = 16 35 | 36 | self.curr_limit = 1200 37 | self.curr_max = 1200 38 | 39 | def led_on(self): 40 | """ 41 | 开启LED 42 | """ 43 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_led, 1) 44 | 45 | def led_off(self): 46 | """ 47 | 关闭LED 48 | """ 49 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_led, 0) 50 | 51 | def torq_on(self): 52 | """ 53 | 打开电机使能 54 | """ 55 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_torq_enable, 1) 56 | 57 | def torq_off(self): 58 | """ 59 | 关闭电机使能 60 | """ 61 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_torq_enable, 0) 62 | 63 | def get_pos(self): 64 | """ 65 | 获取电机位置，单位度 66 | """ 67 | pos = self.packet_handler.read4ByteTxRx(self.port_handler, self.id, self.addr_present_pos) 68 | pos = pos[0] 69 | if pos > 2**31: 70 | pos = pos - 2**32 71 | else: 72 | pos = pos 73 | return pos*self.unit_scale 74 | 75 | def set_zero(self): 76 | """ 77 | 设置电机归0 78 | """ 79 | self.packet_handler.write4ByteTxRx(self.port_handler, self.id, self.addr_home_off, 0) 80 | pos = self.get_pos() 81 | self.packet_handler.write4ByteTxRx(self.port_handler, self.id, self.addr_home_off, -int(pos/self.unit_scale)) 82 | 83 | def set_curr_limit(self, curr=800): 84 | # 设置最大电流，单位mA 85 | self.packet_handler.write2ByteTxRx(self.port_handler, self.id, self.addr_curr_limit, curr) 86 | if curr > self.curr_max: 87 | curr = self.curr_max 88 | self.curr_limit = curr 89 | 90 | def pos_force_mode(self, goal_current=600, goal_pwm=200): 91 | """ 92 | 设置力控位置模式 93 | """ 94 | self.torq_off() 95 | # 力控位置模式 96 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_operating_mode, self.currpos_operating_mode) 97 | 98 | # https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/ 99 | if goal_current > self.curr_limit: 100 | goal_current = self.curr_limit 101 | # 设置goal current 102 | self.packet_handler.write2ByteTxRx(self.port_handler, self.id, self.addr_goal_curr, goal_current) 103 | 104 | if goal_pwm > 885: 105 | goal_pwm = 885 106 | 107 | # 设置goal pwm 108 | self.packet_handler.write2ByteTxRx(self.port_handler, self.id, self.addr_goal_pwm, goal_pwm) # 885 Max 109 | self.torq_on() 110 | 111 | def force_mode(self): 112 | """ 113 | 设置电流模式 114 | """ 115 | self.torq_off() 116 | # 力控模式 117 | self.packet_handler.write1ByteTxRx(self.port_handler, self.id, self.addr_operating_mode, self.curr_operating_mode) 118 | 119 | 120 | def init_config(self, curr_limit=1500, goal_current=1000, goal_pwm=800): 121 | """ 122 | 电机初始配置，LED闪烁，设置为力控位置模式，并使能电机，goal_current影响力的大小，goal_pwm影响速度 123 | """ 124 | for i in range(2): 125 | self.led_on() 126 | time.sleep(0.1) 127 | self.led_off() 128 | time.sleep(0.1) 129 | self.set_curr_limit(curr_limit) 130 | self.pos_force_mode(goal_current, goal_pwm) 131 | 132 | 133 | def set_pos(self, pos): 134 | """ 135 | 设置目标角度，单位度 136 | """ 137 | self.packet_handler.write4ByteTxRx(self.port_handler, self.id, self.addr_goal_pos, int(pos/self.unit_scale)) 138 | 139 | def set_curr(self, cur): 140 | """ 141 | 设置目标电流，单位mA 142 | """ 143 | self.packet_handler.write2ByteTxRx(self.port_handler, self.id, self.addr_goal_curr, cur) 144 | 145 | 146 | 147 | -------------------------------------------------------------------------------- /libgex/utils.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import serial.tools 3 | import serial.tools.list_ports 4 | 5 | def search_ports(): 6 | """ 7 | search for serial ports, COM* for Windows, ttyACM* or ttyUSB* for Linux 8 | """ 9 | ports = serial.tools.list_ports.comports() 10 | 11 | if len(ports) == 0: 12 | print('No COM ports found!') 13 | return None 14 | 15 | for p in ports: 16 | print(p.device, p.serial_number) 17 | 18 | return ports 19 | 20 | 21 | if __name__ == "__main__": 22 | ports = search_ports() -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ## Updates 2 | * 2025-06-06: Our paper is released in [arxiv](https://arxiv.org/pdf/2506.04982). You can cite by: 3 | ```bibtex 4 | @misc{dong2025gexdemocratizingdexterityfullyactuated, 5 | title={GEX: Democratizing Dexterity with Fully-Actuated Dexterous Hand and Exoskeleton Glove}, 6 | author={Yunlong Dong and Xing Liu and Jun Wan and Zelin Deng}, 7 | year={2025}, 8 | eprint={2506.04982}, 9 | archivePrefix={arXiv}, 10 | primaryClass={cs.RO}, 11 | url={https://arxiv.org/abs/2506.04982}, 12 | } 13 | ``` 14 | 15 | ## Python Library to Control Dexterous Hand GX11 and Exoskeleton Glove EX12 16 | Libgex uses dynamixel SDK to control the motors in Python via OpenRB-150 control board through TTL at 1M bps. And libgex uses PyBullet to calculate the forward kinematics and inverse kinematics of the GX11 and EX12. 17 | 18 | ### Python Dependencies 19 | ```bash 20 | pip install pyserial 21 | pip install pybullet 22 | ``` 23 | 24 | ### GX11 and EX12 URDF 25 | Drag folder `libgex/gx11` or `libgex/ex11` into http://urdf.robotsfan.com/ to visualize the URDF. 26 | 27 | GX11 has 3 joints in finger1(thumb), 4 joints in finger2(index), 4 joints in finger3(middle). 28 | 29 | 30 | 31 | Or use https://github.com/openrr/urdf-viz to visualize the URDF and display the link name and axes. 32 | 33 | EX12 has 4 joints in finger1(thumb), 4 joints in finger2(index), 4 joints in finger3(middle). 34 | 35 | The 4 joints design in thumb enable the exoskeleton glove to be better attached to the human thumb tip. 36 | 37 | 38 | 39 | ### Control GX11 40 | 41 | ```bash 42 | python demo_gx11.py 43 | ``` 44 | 45 | 46 | 47 | ### Control EX12 48 | ```bash 49 | python demo_ex12.py 50 | ``` 51 | 52 | ### Code Example 53 | ```bash 54 | from libgex.libgx11 import Hand 55 | import time 56 | 57 | hand = Hand(port='/dev/ttyACM0') # COM* for Windows, ttyACM* or ttyUSB* for Linux, if in Linux, check the port number with `ls /dev/tty*`, then run `sudo chmod 777 /dev/ttyACM*` 58 | hand.connect(goal_pwm=600) # will torque on all the motors, goal_pwm changes the speed, max 855 59 | 60 | hand.home() # home the hand 61 | 62 | hand.motors[0].setj(90) # unit degree 63 | 64 | print(hand.getj()) # print all the joints angles in degree 65 | ``` 66 | 67 | ```bash 68 | from libgex.libex12 import Glove 69 | from libgex.utils import search_ports 70 | import time 71 | 72 | 73 | port = search_ports()[0] 74 | 75 | glove = Glove('/dev/ttyACM1') # COM* for Windows, ttyACM* or ttyUSB* for Linux, if in Linux, check the port number with `ls /dev/tty*`, then run `sudo chmod 777 /dev/ttyACM*` 76 | glove.connect(init=False) # do not torque on glove yet. 77 | 78 | print(glove.fk_finger1()) # get the thumb tip xyz position in base_link frame (bottom of the palm), unit m 79 | 80 | ``` 81 | 82 | All the contorl commands are in `libgex/libgx11.py` and `libgex/libex12.py` 83 | 84 | 85 | ### Hardware 86 | 87 | #### EX12 88 | 89 | https://github.com/Democratizing-Dexterous/ExoskeletonGloveEX12 90 | 91 | #### GX11 92 | 93 | https://github.com/Democratizing-Dexterous/DexterousHandGX11 94 | 95 | 96 | ### Acknowledgement 97 | 98 | https://github.com/ROBOTIS-GIT/DynamixelSDK 99 | 100 | Leap Hand 101 | -------------------------------------------------------------------------------- /zero_hand.py: -------------------------------------------------------------------------------- 1 | from libgex.libgx11 import Hand 2 | from libgex.utils import search_ports 3 | import time 4 | 5 | 6 | 7 | hand = Hand(port="/dev/ttyACM0") # COM* for Windows, ttyACM* or ttyUSB* for Linux 8 | hand.connect() 9 | 10 | print(hand.getj()) 11 | # set hand zero, call off() first 12 | hand.off() 13 | hand.set_zero_whole_hand() 14 | print(hand.getj()) 15 | 16 | 17 | 18 | 19 | --------------------------------------------------------------------------------