├── LICENSE ├── README.md ├── animaquinauf_0.0.8.zip ├── xarm ├── __init__.py ├── __pycache__ │ ├── __init__.cpython-311.pyc │ └── version.cpython-311.pyc ├── core │ ├── __init__.py │ ├── __pycache__ │ │ └── __init__.cpython-311.pyc │ ├── comm │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-311.pyc │ │ │ ├── base.cpython-311.pyc │ │ │ ├── serial_port.cpython-311.pyc │ │ │ └── socket_port.cpython-311.pyc │ │ ├── base.py │ │ ├── serial_port.py │ │ ├── socket_port.py │ │ └── uxbus_cmd_protocol.py │ ├── config │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-311.pyc │ │ │ ├── x_code.cpython-311.pyc │ │ │ └── x_config.cpython-311.pyc │ │ ├── x_code.py │ │ └── x_config.py │ ├── utils │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-311.pyc │ │ │ ├── convert.cpython-311.pyc │ │ │ ├── crc16.cpython-311.pyc │ │ │ └── log.cpython-311.pyc │ │ ├── convert.py │ │ ├── crc16.py │ │ ├── debug_print.py │ │ └── log.py │ ├── version.py │ └── wrapper │ │ ├── __init__.py │ │ ├── __pycache__ │ │ ├── __init__.cpython-311.pyc │ │ ├── uxbus_cmd.cpython-311.pyc │ │ ├── uxbus_cmd_ser.cpython-311.pyc │ │ └── uxbus_cmd_tcp.cpython-311.pyc │ │ ├── uxbus_cmd.py │ │ ├── uxbus_cmd_ser.py │ │ └── uxbus_cmd_tcp.py ├── tools │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-311.pyc │ │ └── threads.cpython-311.pyc │ ├── blockly │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-311.pyc │ │ │ ├── _blockly_base.cpython-311.pyc │ │ │ ├── _blockly_handler.cpython-311.pyc │ │ │ ├── _blockly_highlight.cpython-311.pyc │ │ │ ├── _blockly_node.cpython-311.pyc │ │ │ └── _blockly_tool.cpython-311.pyc │ │ ├── _blockly_base.py │ │ ├── _blockly_handler.py │ │ ├── _blockly_highlight.py │ │ ├── _blockly_node.py │ │ └── _blockly_tool.py │ ├── blockly_tool.py │ ├── gcode.py │ ├── list_ports.py │ ├── modbus_tcp.py │ ├── threads.py │ └── utils.py ├── version.py ├── wrapper │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-311.pyc │ │ └── xarm_api.cpython-311.pyc │ ├── studio_api.py │ └── xarm_api.py └── x3 │ ├── __init__.py │ ├── __pycache__ │ ├── __init__.cpython-311.pyc │ ├── base.cpython-311.pyc │ ├── base_board.cpython-311.pyc │ ├── code.cpython-311.pyc │ ├── decorator.cpython-311.pyc │ ├── events.cpython-311.pyc │ ├── ft_sensor.cpython-311.pyc │ ├── gpio.cpython-311.pyc │ ├── grammar_async.cpython-311.pyc │ ├── gripper.cpython-311.pyc │ ├── modbus_tcp.cpython-311.pyc │ ├── parse.cpython-311.pyc │ ├── record.cpython-311.pyc │ ├── robotiq.cpython-311.pyc │ ├── servo.cpython-311.pyc │ ├── studio.cpython-311.pyc │ ├── track.cpython-311.pyc │ ├── utils.cpython-311.pyc │ └── xarm.cpython-311.pyc │ ├── base.py │ ├── base_board.py │ ├── code.py │ ├── decorator.py │ ├── events.py │ ├── ft_sensor.py │ ├── gpio.py │ ├── grammar_async.py │ ├── grammar_coroutine.py │ ├── gripper.py │ ├── modbus_tcp.py │ ├── parse.py │ ├── record.py │ ├── report.py │ ├── robotiq.py │ ├── servo.py │ ├── studio.py │ ├── track.py │ ├── utils.py │ └── xarm.py └── xarm6-start.blend /README.md: -------------------------------------------------------------------------------- 1 | # Animaquina v0.0.8 Pre-Alpha for Ufactory Robots 2 | by Luis Pacheco 3 | 4 | Animaquina is a robot Integrated Development Environment (IDE) designed for controlling and simulating Robots within Blender. It leverages Blender's powerful 3D capabilities to provide a versatile platform for robot programming and visualization. 5 | 6 | ## Version Information 7 | - Compatible with Blender 4.2 8 | - Only compatible with Ufactory robots (Xarm6 and 850 tested) 9 | - Requires a startup file for each robot model 10 | - This is a pre-alpha version and may contain bugs or incomplete features. 11 | 12 | ## Installation 13 | 14 | 1. **Use Blender Launcher**: It is advised to use Blender Launcher to keep an isolated version of Blender and Python. 15 | 2. Open Blender 4.2. 16 | 3. Locate your Blender 4.2 Python lib folder. This is typically found in: 17 | - Windows: `C:\Program Files\Blender Foundation\Blender 4.2\4.2\python\lib` 18 | - macOS: `/Applications/Blender.app/Contents/Resources/4.2/python/lib` 19 | - Linux: `/usr/share/blender/4.2/python/lib` 20 | 4. Copy both the xarm folder into the Blender Python lib folder. 21 | 5. Go to **Edit** > **Preferences** in Blender. 22 | 6. Click on the **Add-ons** tab. 23 | 7. In the top-right corner of the Add-ons panel, click on **Install**. 24 | 8. Navigate to and select the Animaquinauf_0.0.8 pre-alpha ZIP file. 25 | 9. Click **Install Add-on**. 26 | 10. Locate "Animaquina" in the add-ons list and enable it by checking the box next to its name. 27 | 28 | ## Usage 29 | 30 | 1. Select the robot you have from the drop down list this is either ufxarm6_twin, uf850. 31 | 2. Set the IP address of your obot in the Animaquina panel. 32 | 3. In the target selection dropdown, select the 'tcp_target' . 33 | 4. Click the **Connect** button to establish a connection with the robot. 34 | 5. Click the **Update Pose** button to synchronize the digital twin's TCP with the selected robot's TCP. This action updates the current joint rotation and TCP position of the digital twin robot. 35 | 6. On the info panel, you can view the current joint rotation and TCP position of the digital twin. 36 | 7. To move the robot via Blender: 37 | a. Manipulate the `tcp_target` object in the 3D viewport. 38 | b. Click the **GoTarget** button to send the command and move the physical robot to the new position. 39 | 8. Enable the **RealTime** option to see the digital twin robot moving in sync with the physical robot. 40 | 9. To manually manipulate the physical robot: 41 | a. Click on **Manual Mode** to enable physical manipulation of the robot. 42 | b. If **RealTime** is enabled, you will see the robot's movements updated in real-time in the 3D viewport. 43 | 10. To digitize features in the cell: 44 | a. Move the robot's TCP to the desired position. 45 | b. Click the **addMarker** button to save the current TCP position as a marker. 46 | 11. To run a toolpath: 47 | a. Select a mesh object in the 3D viewport. 48 | b. Click the **Path** button to execute the toolpath. 49 | 50 | 51 | **Warning:** The order of vertices in the mesh object matters for the toolpath execution. Ensure the vertices are in the correct order before using this function. 52 | 53 | ## Features 54 | 55 | - IP-based robot connection 56 | - Real-time robot manipulation via Blender's 3D viewport 57 | - Digital twin synchronization with physical robot 58 | - Real-time movement visualization 59 | - Manual mode for physical robot manipulation 60 | - Live update of digital twin in manual mode 61 | - Marker placement for digitizing cell features 62 | - Toolpath execution based on mesh object vertices 63 | - Display of current joint rotation and TCP position in the info panel 64 | 65 | ## Known Issues and Safety Warnings 66 | 67 | ⚠️ **IMPORTANT: USE AT YOUR OWN RISK!** ⚠️ 68 | 69 | The developer is not responsible for any damage to property, personal injury, or death resulting from the use of this software. 70 | 71 | - **No collision detection:** The software does not detect or prevent collisions between the robot and its environment. 72 | - **No angle limits:** Joint angle limits are not enforced, which could lead to unexpected or dangerous movements. 73 | - **Real-time mode bug:** In real-time mode, you may need to wiggle the mouse to see the robot updating in the viewport. 74 | - **Limited safety features:** Only speed and acceleration are capped. There are NO comprehensive safety measures programmed. 75 | - **Lack of comprehensive safety protocols:** This pre-alpha version does not include industry-standard safety features typically found in production-ready robotic control software. 76 | 77 | ## Additional Safety Precautions 78 | 79 | 1. Always maintain a safe distance from the robot during operation. 80 | 2. Use physical safety barriers and emergency stop systems. 81 | 3. Thoroughly test all movements in a controlled, safe environment before executing them on the actual robot. 82 | 4. Ensure that all users are properly trained in robot safety and operation. 83 | 5. Never use this software in production environments or near personnel without proper risk assessment and mitigation strategies. 84 | 85 | ## Compatibility 86 | 87 | - Blender version: 4.2 88 | - Ufactory models only 89 | - tested on windows 90 | 91 | ## License 92 | 93 | GPL License 94 | 95 | ## Authors 96 | 97 | - Luis Pacheco 98 | 99 | 100 | ## Support 101 | 102 | For support or to report bugs, please [contact our support team/create an issue on our GitHub repository]. 103 | 104 | ## Install 105 | https://youtu.be/NPsnpP_cLk0 106 | 107 | **Note:** This is a pre-alpha version intended for development and testing purposes only. It contains significant limitations and safety risks. Do not use in production environments or without expert supervision. 108 | 109 | ## Acknowledgments 110 | 111 | This project builds on the ideas and inspiration from several similar tools in architectural robotics / animation. 112 | -------------------------------------------------------------------------------- /animaquinauf_0.0.8.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/animaquinauf_0.0.8.zip -------------------------------------------------------------------------------- /xarm/__init__.py: -------------------------------------------------------------------------------- 1 | from .wrapper import XArmAPI 2 | from .version import __version__ 3 | -------------------------------------------------------------------------------- /xarm/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/__pycache__/version.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/__pycache__/version.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/__init__.py: -------------------------------------------------------------------------------- 1 | from .config.x_code import ControllerWarn, ControllerError, ServoError 2 | from .config.x_config import XCONF 3 | -------------------------------------------------------------------------------- /xarm/core/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/comm/__init__.py: -------------------------------------------------------------------------------- 1 | try: 2 | from .serial_port import SerialPort 3 | except: 4 | SerialPort = None 5 | from .socket_port import SocketPort 6 | -------------------------------------------------------------------------------- /xarm/core/comm/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/comm/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/comm/__pycache__/base.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/comm/__pycache__/base.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/comm/__pycache__/serial_port.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/comm/__pycache__/serial_port.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/comm/__pycache__/socket_port.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/comm/__pycache__/socket_port.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/comm/base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import time 10 | import queue 11 | import socket 12 | import select 13 | import threading 14 | from ..utils.log import logger 15 | from ..utils import convert 16 | 17 | 18 | class RxParse(object): 19 | def __init__(self, rx_que, fb_que=None): 20 | self.rx_que = rx_que 21 | self.fb_que = fb_que 22 | 23 | def flush(self, fromid=-1, toid=-1): 24 | pass 25 | 26 | def put(self, data, is_report=False): 27 | if not is_report and data[6] == 0xFF: 28 | if not self.fb_que: 29 | return 30 | self.fb_que.put(data) 31 | else: 32 | self.rx_que.put(data) 33 | 34 | 35 | class Port(threading.Thread): 36 | def __init__(self, rxque_max, fb_que=None): 37 | super(Port, self).__init__() 38 | self.daemon = True 39 | self.rx_que = queue.Queue(rxque_max) 40 | self.fb_que = fb_que 41 | self.write_lock = threading.Lock() 42 | self._connected = False 43 | self.com = None 44 | self.rx_parse = RxParse(self.rx_que, self.fb_que) 45 | self.com_read = None 46 | self.com_write = None 47 | self.port_type = '' 48 | self.buffer_size = 1 49 | self.heartbeat_thread = None 50 | self.alive = True 51 | 52 | @property 53 | def connected(self): 54 | return self._connected 55 | 56 | def run(self): 57 | if self.port_type == 'report-socket': 58 | self.recv_report_proc() 59 | else: 60 | self.recv_proc() 61 | # self.recv_loop() 62 | 63 | def close(self): 64 | self.alive = False 65 | if 'socket' in self.port_type: 66 | try: 67 | self.com.shutdown(socket.SHUT_RDWR) 68 | except: 69 | pass 70 | try: 71 | self.com.close() 72 | except: 73 | pass 74 | 75 | def flush(self, fromid=-1, toid=-1): 76 | if not self.connected: 77 | return -1 78 | while not(self.rx_que.empty()): 79 | self.rx_que.queue.clear() 80 | self.rx_parse.flush(fromid, toid) 81 | return 0 82 | 83 | def write(self, data): 84 | if not self.connected: 85 | return -1 86 | try: 87 | with self.write_lock: 88 | logger.verbose('[{}] send: {}'.format(self.port_type, data)) 89 | self.com_write(data) 90 | return 0 91 | except Exception as e: 92 | self._connected = False 93 | logger.error("[{}] send error: {}".format(self.port_type, e)) 94 | return -1 95 | 96 | def read(self, timeout=None): 97 | if not self.connected: 98 | return -1 99 | try: 100 | buf = self.rx_que.get(timeout=timeout) 101 | logger.verbose('[{}] recv: {}'.format(self.port_type, buf)) 102 | return buf 103 | except: 104 | return -1 105 | # if not self.connected: 106 | # return -1 107 | # if not self.rx_que.empty(): 108 | # buf = self.rx_que.get(timeout=timeout) 109 | # logger.verbose('[{}] recv: {}'.format(self.port_type, buf)) 110 | # return buf 111 | # else: 112 | # return -1 113 | 114 | # def recv_loop(self): 115 | # self.alive = True 116 | # logger.debug('[{}] recv thread start'.format(self.port_type)) 117 | # try: 118 | # while self.connected and self.alive: 119 | # if 'socket' in self.port_type: 120 | # ready_input, ready_output, ready_exception = select.select([self.com], [], []) 121 | # for indata in ready_input: 122 | # if indata == self.com: 123 | # rx_data = self.com_read(self.buffer_size) 124 | # break 125 | # else: 126 | # continue 127 | # else: 128 | # rx_data = self.com_read(self.com.in_waiting or self.buffer_size) 129 | # self.rx_parse.put(rx_data) 130 | # except Exception as e: 131 | # if self.alive: 132 | # logger.error('[{}] recv error: {}'.format(self.port_type, e)) 133 | # finally: 134 | # self.close() 135 | # logger.debug('[{}] recv thread had stopped'.format(self.port_type)) 136 | # self._connected = False 137 | 138 | def recv_report_proc(self): 139 | self.alive = True 140 | logger.debug('[{}] recv thread start'.format(self.port_type)) 141 | failed_read_count = 0 142 | timeout_count = 0 143 | size = 0 144 | data_num = 0 145 | buffer = b'' 146 | size_is_not_confirm = False 147 | 148 | data_prev_us = 0 149 | data_curr_us = 0 150 | data_max_interval_us = 0 151 | data_over_us = 205 * 1000 # over 205ms, cnts++ 152 | data_over_cnts = 0 153 | 154 | recv_prev_us = 0 155 | recv_curr_us = 0 156 | recv_max_interval_us = 0 157 | recv_over_us = 300 * 1000 # over 300ms, cnts++ 158 | recv_over_cnts = 0 159 | 160 | try: 161 | while self.connected and self.alive: 162 | try: 163 | data = self.com_read(4 - data_num if size == 0 else (size - data_num)) 164 | except socket.timeout: 165 | timeout_count += 1 166 | if timeout_count > 3: 167 | self._connected = False 168 | logger.error('[{}] socket read timeout'.format(self.port_type)) 169 | break 170 | continue 171 | else: 172 | if len(data) == 0: 173 | failed_read_count += 1 174 | if failed_read_count > 5: 175 | self._connected = False 176 | logger.error('[{}] socket read failed, len=0'.format(self.port_type)) 177 | break 178 | time.sleep(0.1) 179 | continue 180 | data_num += len(data) 181 | buffer += data 182 | if size == 0: 183 | if data_num != 4: 184 | continue 185 | size = convert.bytes_to_u32(buffer[0:4]) 186 | if size == 233: 187 | size_is_not_confirm = True 188 | size = 245 189 | logger.info('report_data_size: {}, size_is_not_confirm={}'.format(size, size_is_not_confirm)) 190 | else: 191 | if data_num < size: 192 | continue 193 | if size_is_not_confirm: 194 | size_is_not_confirm = True 195 | if convert.bytes_to_u32(buffer[233:237]) == 233: 196 | size = 233 197 | buffer = buffer[233:] 198 | continue 199 | 200 | if convert.bytes_to_u32(buffer[0:4]) != size and not (size_is_not_confirm and size == 245 and convert.bytes_to_u32(buffer[0:4]) == 233): 201 | logger.error('report data error, close, length={}, size={}'.format(convert.bytes_to_u32(buffer[0:4]), size)) 202 | break 203 | 204 | # # buffer[494:502] 205 | # data_curr_us = convert.bytes_to_u64(buffer[-8:]) 206 | # recv_curr_us = time.monotonic() * 1000000 207 | # 208 | # if data_prev_us != 0 and recv_prev_us != 0: 209 | # data_interval_us = data_curr_us - data_prev_us 210 | # data_over_cnts += 1 if data_interval_us > data_over_us else 0 211 | # 212 | # recv_interval_us = recv_curr_us - recv_prev_us 213 | # recv_over_cnts += 1 if recv_interval_us > recv_over_us else 0 214 | # 215 | # print_flag = False 216 | # 217 | # if data_interval_us > data_max_interval_us: 218 | # data_max_interval_us = data_interval_us 219 | # print_flag = True 220 | # elif data_interval_us > data_over_us: 221 | # print_flag = True 222 | # 223 | # if recv_interval_us > recv_max_interval_us: 224 | # recv_max_interval_us = recv_interval_us 225 | # print_flag = True 226 | # elif recv_interval_us > recv_over_us: 227 | # print_flag = True 228 | # 229 | # if print_flag: 230 | # print('[RECV] Di={}, Dmax={}, Dcnts={}, Ri={}, Rmax={}, Rcnts={}'.format( 231 | # data_interval_us / 1000, data_max_interval_us / 1000, data_over_cnts, 232 | # recv_interval_us / 1000, recv_max_interval_us / 1000, recv_over_cnts 233 | # )) 234 | # data_prev_us = data_curr_us 235 | # recv_prev_us = recv_curr_us 236 | 237 | if self.rx_que.qsize() > 1: 238 | self.rx_que.get() 239 | self.rx_parse.put(buffer, True) 240 | buffer = b'' 241 | data_num = 0 242 | 243 | timeout_count = 0 244 | failed_read_count = 0 245 | except Exception as e: 246 | if self.alive: 247 | logger.error('[{}] recv error: {}'.format(self.port_type, e)) 248 | finally: 249 | self.close() 250 | logger.debug('[{}] recv thread had stopped'.format(self.port_type)) 251 | self._connected = False 252 | 253 | def recv_proc(self): 254 | self.alive = True 255 | logger.debug('[{}] recv thread start'.format(self.port_type)) 256 | is_main_tcp = self.port_type == 'main-socket' 257 | is_main_serial = self.port_type == 'main-serial' 258 | try: 259 | failed_read_count = 0 260 | buffer = b'' 261 | while self.connected and self.alive: 262 | if is_main_tcp: 263 | try: 264 | rx_data = self.com_read(self.buffer_size) 265 | except socket.timeout: 266 | continue 267 | if len(rx_data) == 0: 268 | failed_read_count += 1 269 | if failed_read_count > 5: 270 | self._connected = False 271 | logger.error('[{}] socket read failed, len=0'.format(self.port_type)) 272 | break 273 | time.sleep(0.1) 274 | continue 275 | buffer += rx_data 276 | while True: 277 | if len(buffer) < 6: 278 | break 279 | length = convert.bytes_to_u16(buffer[4:6]) + 6 280 | if len(buffer) < length: 281 | break 282 | rx_data = buffer[:length] 283 | buffer = buffer[length:] 284 | self.rx_parse.put(rx_data) 285 | elif is_main_serial: 286 | rx_data = self.com_read(self.com.in_waiting or self.buffer_size) 287 | self.rx_parse.put(rx_data) 288 | else: 289 | break 290 | failed_read_count = 0 291 | except Exception as e: 292 | if self.alive: 293 | logger.error('[{}] recv error: {}'.format(self.port_type, e)) 294 | finally: 295 | self.close() 296 | logger.debug('[{}] recv thread had stopped'.format(self.port_type)) 297 | self._connected = False 298 | # if self.heartbeat_thread: 299 | # try: 300 | # self.heartbeat_thread.join() 301 | # except: 302 | # pass 303 | 304 | -------------------------------------------------------------------------------- /xarm/core/comm/serial_port.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import serial 12 | from ..utils.log import logger 13 | from .base import Port 14 | from .uxbus_cmd_protocol import Ux2HexProtocol 15 | from ..config.x_config import XCONF 16 | 17 | 18 | class SerialPort(Port): 19 | def __init__(self, port, baud=XCONF.SerialConf.SERIAL_BAUD, 20 | rxque_max=XCONF.SerialConf.UXBUS_RXQUE_MAX, protocol=XCONF.SerialConf.UX2_HEX_PROTOCOL): 21 | super(SerialPort, self).__init__(rxque_max) 22 | self.port_type = 'main-serial' 23 | try: 24 | self.com = serial.Serial(port=port, baudrate=baud) 25 | if not self.com.isOpen(): 26 | self._connected = False 27 | raise Exception('serial is not open') 28 | logger.info('{} connect {}:{} success'.format(self.port_type, port, baud)) 29 | 30 | self._connected = True 31 | 32 | self.buffer_size = 1 33 | 34 | if protocol == XCONF.SerialConf.UX2_HEX_PROTOCOL: 35 | self.rx_parse = Ux2HexProtocol(self.rx_que, 36 | XCONF.SerialConf.UXBUS_DEF_FROMID, 37 | XCONF.SerialConf.UXBUS_DEF_TOID) 38 | self.com_read = self.com.read 39 | self.com_write = self.com.write 40 | self.start() 41 | except Exception as e: 42 | logger.info('{} connect {}:{} failed, {}'.format(self.port_type, port, baud, e)) 43 | self._connected = False 44 | 45 | -------------------------------------------------------------------------------- /xarm/core/comm/socket_port.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import queue 12 | import os 13 | import socket 14 | import struct 15 | import platform 16 | import threading 17 | import time 18 | from ..utils.log import logger 19 | from .base import Port 20 | from ..config.x_config import XCONF 21 | 22 | # try: 23 | # if platform.system() == 'Linux': 24 | # import fcntl 25 | # else: 26 | # fcntl = None 27 | # except: 28 | # fcntl = None 29 | # 30 | # 31 | # def is_xarm_local_ip(ip): 32 | # try: 33 | # if platform.system() == 'Linux' and fcntl: 34 | # def _get_ip(s, ifname): 35 | # try: 36 | # return socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', ifname[:15]))[20:24]) 37 | # except: 38 | # pass 39 | # return '' 40 | # sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 41 | # # gentoo system netcard name 42 | # if ip == _get_ip(sock, b'enp1s0'): 43 | # return True 44 | # # rasp system netcard name 45 | # if ip == _get_ip(sock, b'eth0'): 46 | # return True 47 | # except: 48 | # pass 49 | # return False 50 | 51 | 52 | def get_all_ips(): 53 | addrs = ['localhost', '127.0.0.1'] 54 | addrs = set(addrs) 55 | try: 56 | for ip in socket.gethostbyname_ex(socket.gethostname())[2]: 57 | try: 58 | if not ip.startswith('127.'): 59 | addrs.add(ip) 60 | except: 61 | pass 62 | except: 63 | pass 64 | try: 65 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 66 | sock.settimeout(3) 67 | sock.connect(('8.8.8.8', 53)) 68 | addrs.add(sock.getsockname()[0]) 69 | except: 70 | pass 71 | return addrs 72 | 73 | 74 | class HeartBeatThread(threading.Thread): 75 | def __init__(self, sock_class): 76 | threading.Thread.__init__(self) 77 | self.sock_class = sock_class 78 | self.daemon = True 79 | 80 | def run(self): 81 | logger.debug('{} heartbeat thread start'.format(self.sock_class.port_type)) 82 | heat_data = bytes([0, 0, 0, 1, 0, 2, 0, 0]) 83 | 84 | while self.sock_class.connected: 85 | if self.sock_class.write(heat_data) == -1: 86 | break 87 | time.sleep(1) 88 | logger.debug('{} heartbeat thread had stopped'.format(self.sock_class.port_type)) 89 | 90 | 91 | class SocketPort(Port): 92 | def __init__(self, server_ip, server_port, rxque_max=XCONF.SocketConf.TCP_RX_QUE_MAX, heartbeat=False, 93 | buffer_size=XCONF.SocketConf.TCP_CONTROL_BUF_SIZE, forbid_uds=False, fb_que=None): 94 | is_main_tcp = server_port == XCONF.SocketConf.TCP_CONTROL_PORT or server_port == XCONF.SocketConf.TCP_CONTROL_PORT + 1 95 | super(SocketPort, self).__init__(rxque_max, fb_que) 96 | if is_main_tcp: 97 | self.port_type = 'main-socket' 98 | # self.com.setsockopt(socket.SOL_SOCKET, socket.SO_RCVTIMEO, 5) 99 | else: 100 | self.port_type = 'report-socket' 101 | try: 102 | socket.setdefaulttimeout(1) 103 | use_uds = False 104 | # if not forbid_uds and platform.system() == 'Linux' and is_xarm_local_ip(server_ip): 105 | # if not forbid_uds and platform.system() == 'Linux' and server_ip in get_all_ips(): 106 | if not forbid_uds and platform.system() == 'Linux': 107 | uds_path = os.path.join('/tmp/xarmcontroller_uds_{}'.format(server_port)) 108 | if os.path.exists(uds_path): 109 | try: 110 | self.com = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) 111 | self.com.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 112 | self.com.setblocking(True) 113 | self.com.settimeout(1) 114 | self.com.connect(uds_path) 115 | logger.info('{} connect {} success, uds_{}'.format(self.port_type, server_ip, server_port)) 116 | use_uds = True 117 | except Exception as e: 118 | pass 119 | # logger.error('use uds error, {}'.format(e)) 120 | else: 121 | pass 122 | if not use_uds: 123 | self.com = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 124 | self.com.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 125 | # self.com.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) 126 | # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 30) 127 | # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10) 128 | # self.com.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 3) 129 | self.com.setblocking(True) 130 | self.com.settimeout(1) 131 | self.com.connect((server_ip, server_port)) 132 | logger.info('{} connect {} success'.format(self.port_type, server_ip)) 133 | # logger.info('{} connect {}:{} success'.format(self.port_type, server_ip, server_port)) 134 | 135 | self._connected = True 136 | self.buffer_size = buffer_size 137 | # time.sleep(1) 138 | 139 | self.com_read = self.com.recv 140 | self.com_write = self.com.send 141 | self.write_lock = threading.Lock() 142 | self.start() 143 | if heartbeat: 144 | self.heartbeat_thread = HeartBeatThread(self) 145 | self.heartbeat_thread.start() 146 | except Exception as e: 147 | logger.info('{} connect {} failed, {}'.format(self.port_type, server_ip, e)) 148 | # logger.error('{} connect {}:{} failed, {}'.format(self.port_type, server_ip, server_port, e)) 149 | self._connected = False 150 | 151 | -------------------------------------------------------------------------------- /xarm/core/comm/uxbus_cmd_protocol.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | from ..utils import crc16 12 | from ..utils.log import logger 13 | 14 | # ux2_hex_protocol define 15 | UX2HEX_RXSTART_FROMID = 0 16 | UX2HEX_RXSTART_TOID = 1 17 | UX2HEX_RXSTATE_LEN = 2 18 | UX2HEX_RXSTATE_DATA = 3 19 | UX2HEX_RXSTATE_CRC1 = 4 20 | UX2HEX_RXSTATE_CRC2 = 5 21 | UX2HEX_RXLEN_MAX = 50 22 | 23 | 24 | class Ux2HexProtocol(object): 25 | """ 26 | fromid and toid: broadcast address is 0xFF 27 | """ 28 | def __init__(self, rx_que, fromid, toid): 29 | self.rx_que = rx_que 30 | self.rxstate = UX2HEX_RXSTART_FROMID 31 | self.data_idx = 0 32 | self.len = 0 33 | self.fromid = fromid 34 | self.toid = toid 35 | self.rxbuf = None 36 | 37 | # wipe cache , set from_id and to_id 38 | def flush(self, fromid=-1, toid=-1): 39 | self.rxstate = UX2HEX_RXSTART_FROMID 40 | self.data_idx = 0 41 | self.len = 0 42 | if fromid != -1: 43 | self.fromid = fromid 44 | if toid != -1: 45 | self.toid = toid 46 | 47 | def put(self, rxstr, length=0): 48 | if length == 0: 49 | length = len(rxstr) 50 | if len(rxstr) < length: 51 | logger.error('len(rxstr) < length') 52 | 53 | for i in range(length): 54 | rxch = bytes([rxstr[i]]) 55 | # print_hex(self.DB_FLG, rxch, 1) 56 | # print('state:%d' % (self.rxstate)) 57 | if UX2HEX_RXSTART_FROMID == self.rxstate: 58 | if self.toid == rxch[0] or 255 == self.toid: 59 | self.rxbuf = rxch 60 | self.rxstate = UX2HEX_RXSTART_TOID 61 | 62 | elif UX2HEX_RXSTART_TOID == self.rxstate: 63 | if self.fromid == rxch[0] or self.fromid == 0xFF: 64 | self.rxbuf += rxch 65 | self.rxstate = UX2HEX_RXSTATE_LEN 66 | else: 67 | self.rxstate = UX2HEX_RXSTART_FROMID 68 | 69 | elif UX2HEX_RXSTATE_LEN == self.rxstate: 70 | if rxch[0] < UX2HEX_RXLEN_MAX: 71 | self.rxbuf += rxch 72 | self.len = rxch[0] 73 | self.data_idx = 0 74 | self.rxstate = UX2HEX_RXSTATE_DATA 75 | else: 76 | self.rxstate = UX2HEX_RXSTART_FROMID 77 | 78 | elif UX2HEX_RXSTATE_DATA == self.rxstate: 79 | if self.data_idx < self.len: 80 | self.rxbuf += rxch 81 | self.data_idx += 1 82 | if self.data_idx == self.len: 83 | self.rxstate = UX2HEX_RXSTATE_CRC1 84 | else: 85 | self.rxstate = UX2HEX_RXSTART_FROMID 86 | 87 | elif UX2HEX_RXSTATE_CRC1 == self.rxstate: 88 | self.rxbuf += rxch 89 | self.rxstate = UX2HEX_RXSTATE_CRC2 90 | 91 | elif UX2HEX_RXSTATE_CRC2 == self.rxstate: 92 | self.rxbuf += rxch 93 | self.rxstate = UX2HEX_RXSTART_FROMID 94 | crc = crc16.crc_modbus(self.rxbuf[:self.len + 3]) 95 | if crc[0] == self.rxbuf[self.len + 3] and crc[1] == self.rxbuf[self.len + 4]: 96 | if self.rx_que.full(): 97 | self.rx_que.get() 98 | self.rx_que.put(self.rxbuf) 99 | # print(self.rxbuf) 100 | 101 | -------------------------------------------------------------------------------- /xarm/core/config/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/config/__init__.py -------------------------------------------------------------------------------- /xarm/core/config/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/config/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/config/__pycache__/x_code.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/config/__pycache__/x_code.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/config/__pycache__/x_config.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/config/__pycache__/x_config.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/config/x_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import math 10 | 11 | 12 | class XCONF(object): 13 | ARM_AXIS_NUM = 7 14 | MAX_CMD_NUM = 1024 15 | 16 | TRACK_ID = 1 17 | GRIPPER_ID = 8 18 | 19 | TGPIO_HOST_ID = 9 20 | LINEER_TRACK_HOST_ID = 11 21 | 22 | def __init__(self): 23 | pass 24 | 25 | class Robot: 26 | class Axis: 27 | XARM5 = 5 28 | XARM6 = 6 29 | XARM7 = 7 30 | 31 | class Type: 32 | XARM6_X1 = 1 33 | XARM7_X2 = 2 34 | XARM7_X3 = 3 35 | XARM7_X3MIR = 4 36 | XARM5_X4 = 5 37 | XARM6_X4 = 6 38 | XARM7_X4 = 7 39 | XARM6_X8 = 8 40 | XARM6_X9 = 9 41 | XARM6_X11 = 11 42 | XARM6_X12 = 12 43 | XARM7_X13 = 13 44 | XARM5_X4_1305 = 51305 45 | XARM6_X4_1305 = 61305 46 | XARM7_X4_1305 = 71305 47 | 48 | JOINT_LIMITS = { 49 | Axis.XARM5: { 50 | Type.XARM5_X4: [ 51 | (-2 * math.pi, 2 * math.pi), 52 | (-2.059488, 2.094395), # (-2.18, 2.18), 53 | (-3.92699, 0.191986), # (-4.01, 0.1), 54 | (-1.692969, math.pi), # (-1.75, math.pi), 55 | (-2 * math.pi, 2 * math.pi) 56 | ], 57 | Type.XARM5_X4_1305: [ 58 | (-2 * math.pi, 2 * math.pi), 59 | (-2.042035, 2.024581), 60 | (-3.822271, 0.174532), 61 | (-1.692969, math.pi), # (-1.75, math.pi), 62 | (-2 * math.pi, 2 * math.pi) 63 | ], 64 | }, 65 | Axis.XARM6: { 66 | Type.XARM6_X4: [ 67 | (-2 * math.pi, 2 * math.pi), 68 | (-2.059488, 2.094395), # (-2.18, 2.18), 69 | (-3.92699, 0.191986), # (-4.01, 0.1), 70 | (-2 * math.pi, 2 * math.pi), 71 | (-1.692969, math.pi), # (-1.75, math.pi), 72 | (-2 * math.pi, 2 * math.pi) 73 | ], 74 | Type.XARM6_X4_1305: [ 75 | (-2 * math.pi, 2 * math.pi), 76 | (-2.042035, 2.024581), 77 | (-3.822271, 0.174532), 78 | (-2 * math.pi, 2 * math.pi), 79 | (-1.692969, math.pi), # (-1.75, math.pi), 80 | (-2 * math.pi, 2 * math.pi) 81 | ], 82 | Type.XARM6_X8: [ 83 | (-2 * math.pi, 2 * math.pi), 84 | (-2.059488, 2.094395), # (-2.18, 2.18), 85 | (-0.191986, 3.92699), 86 | (-2 * math.pi, 2 * math.pi), 87 | (-1.692969, math.pi), # (-1.75, math.pi), 88 | (-2 * math.pi, 2 * math.pi) 89 | ], 90 | Type.XARM6_X9: [ 91 | (-2 * math.pi, 2 * math.pi), 92 | (-2.6179938779914944, 2.6179938779914944), 93 | (-0.061086523819801536, 5.235987755982989), 94 | (-2 * math.pi, 2 * math.pi), 95 | (-2.1642082724729685, 2.1642082724729685), 96 | (-2 * math.pi, 2 * math.pi), 97 | ], 98 | Type.XARM6_X11: [ 99 | (-2 * math.pi, 2 * math.pi), 100 | (-2 * math.pi, 2 * math.pi), 101 | (-2 * math.pi, 2 * math.pi), 102 | (-2 * math.pi, 2 * math.pi), 103 | (-1.692969, math.pi), 104 | (-2 * math.pi, 2 * math.pi), 105 | ], 106 | Type.XARM6_X12: [ 107 | (-2 * math.pi, 2 * math.pi), 108 | (-2.303834612632515, 2.303834612632515), 109 | (-4.223696789826278, 0.061086523819801536), 110 | (-2 * math.pi, 2 * math.pi), 111 | (-2.1642082724729685, 2.1642082724729685), 112 | (-2 * math.pi, 2 * math.pi), 113 | ], 114 | 115 | }, 116 | Axis.XARM7: { 117 | Type.XARM7_X3: [ 118 | (-2 * math.pi, 2 * math.pi), 119 | (-2.059488, 2.094395), # (-2.18, 2.18), 120 | (-2 * math.pi, 2 * math.pi), 121 | (-3.92699, 0.191986), # (-4.01, 0.1), 122 | (-2 * math.pi, 2 * math.pi), 123 | (-1.692969, math.pi), # (-1.75, math.pi), 124 | (-2 * math.pi, 2 * math.pi) 125 | ], 126 | Type.XARM7_X4: [ 127 | (-2 * math.pi, 2 * math.pi), 128 | (-2.059488, 2.094395), # (-2.18, 2.18), 129 | (-2 * math.pi, 2 * math.pi), 130 | (-0.191986, 3.92699), # (-0.1, 4.01), 131 | (-2 * math.pi, 2 * math.pi), 132 | (-1.692969, math.pi), # (-1.75, math.pi), 133 | (-2 * math.pi, 2 * math.pi) 134 | ], 135 | Type.XARM7_X4_1305: [ 136 | (-2 * math.pi, 2 * math.pi), 137 | (-2.042035, 2.024581), 138 | (-2 * math.pi, 2 * math.pi), 139 | (-0.10472, 3.92699), 140 | (-2 * math.pi, 2 * math.pi), 141 | (-1.692969, math.pi), # (-1.75, math.pi), 142 | (-2 * math.pi, 2 * math.pi) 143 | ], 144 | Type.XARM7_X13: [ 145 | (-2 * math.pi, 2 * math.pi), 146 | (-2.094395, 2.059488), # (-2.18, 2.18), 147 | (-2 * math.pi, 2 * math.pi), 148 | (-3.92699, 0.191986), 149 | (-2 * math.pi, 2 * math.pi), 150 | (-math.pi, 1.692969), 151 | (-2 * math.pi, 2 * math.pi) 152 | ] 153 | } 154 | } 155 | TCP_LIMITS = { 156 | Axis.XARM5: { 157 | Type.XARM5_X4: [ 158 | (-750, 750), 159 | (-750, 750), 160 | (-400, 1000), 161 | (math.pi, math.pi), 162 | (0, 0), 163 | (-math.pi, math.pi) 164 | ], 165 | }, 166 | Axis.XARM6: { 167 | Type.XARM6_X1: [ 168 | (-750, 750), 169 | (-750, 750), 170 | (-400, 1000), 171 | (-math.pi, math.pi), 172 | (-math.pi, math.pi), 173 | (-math.pi, math.pi) 174 | ], 175 | Type.XARM6_X4: [ 176 | (-750, 750), 177 | (-750, 750), 178 | (-400, 1000), 179 | (-math.pi, math.pi), 180 | (-math.pi, math.pi), 181 | (-math.pi, math.pi) 182 | ], 183 | Type.XARM6_X8: [ 184 | (-1000, 1000), 185 | (-1000, 1000), 186 | (-600, 1200), 187 | (-math.pi, math.pi), 188 | (-math.pi, math.pi), 189 | (-math.pi, math.pi) 190 | ], 191 | Type.XARM6_X9: [ 192 | (-500, 500), 193 | (-500, 500), 194 | (-150, 750), 195 | (-math.pi, math.pi), 196 | (-math.pi, math.pi), 197 | (-math.pi, math.pi) 198 | ], 199 | Type.XARM6_X11: [ 200 | (-900, 900), 201 | (-900, 900), 202 | (-900, 1200), 203 | (-math.pi, math.pi), 204 | (-math.pi, math.pi), 205 | (-math.pi, math.pi) 206 | ], 207 | Type.XARM6_X12: [ 208 | (-1000, 1000), 209 | (-1000, 1000), 210 | (-400, 1300), 211 | (-math.pi, math.pi), 212 | (-math.pi, math.pi), 213 | (-math.pi, math.pi) 214 | ], 215 | }, 216 | Axis.XARM7: { 217 | Type.XARM7_X3: [ 218 | (-750, 750), 219 | (-750, 750), 220 | (-400, 1000), 221 | (-math.pi, math.pi), 222 | (-math.pi, math.pi), 223 | (-math.pi, math.pi) 224 | ], 225 | Type.XARM7_X4: [ 226 | (-750, 750), 227 | (-750, 750), 228 | (-400, 1000), 229 | (-math.pi, math.pi), 230 | (-math.pi, math.pi), 231 | (-math.pi, math.pi) 232 | ], 233 | Type.XARM7_X13: [ 234 | (-750, 750), 235 | (-750, 750), 236 | (-400, 1000), 237 | (-math.pi, math.pi), 238 | (-math.pi, math.pi), 239 | (-math.pi, math.pi) 240 | ] 241 | } 242 | } 243 | 244 | class SerialConf: 245 | SERIAL_BAUD = 2000000 # 921600 246 | UXBUS_RXQUE_MAX = 10 247 | UXBUS_DEF_FROMID = 0xAA 248 | UXBUS_DEF_TOID = 0x55 249 | UX2_HEX_PROTOCOL = 1 250 | UX2_STR_PROTOCOL = 2 251 | UX1_HEX_PROTOCOL = 3 252 | UX1_STR_PROTOCOL = 4 253 | 254 | class SocketConf: 255 | TCP_CONTROL_PORT = 502 256 | TCP_REPORT_NORM_PORT = 30001 257 | TCP_REPORT_RICH_PORT = 30002 258 | TCP_REPORT_REAL_PORT = 30003 259 | TCP_RX_QUE_MAX = 1024 260 | TCP_CONTROL_BUF_SIZE = 1024 261 | TCP_REPORT_REAL_BUF_SIZE = 87 262 | TCP_REPORT_NORMAL_BUF_SIZE = 133 263 | TCP_REPORT_RICH_BUF_SIZE = 233 264 | 265 | class UxbusReg: 266 | GET_VERSION = 1 267 | GET_ROBOT_SN = 2 268 | CHECK_VERIFY = 3 269 | RELOAD_DYNAMICS = 4 270 | GET_REPORT_TAU_OR_I = 5 271 | GET_TCP_ROTATION_RADIUS = 6 272 | GET_ALLOW_APPROX_MOTION = 7 273 | 274 | SYSTEM_CONTROL = 10 275 | MOTION_EN = 11 276 | SET_STATE = 12 277 | GET_STATE = 13 278 | GET_CMDNUM = 14 279 | GET_ERROR = 15 280 | CLEAN_ERR = 16 281 | CLEAN_WAR = 17 282 | SET_BRAKE = 18 283 | SET_MODE = 19 284 | 285 | MOVE_LINE = 21 286 | MOVE_LINEB = 22 287 | MOVE_JOINT = 23 288 | MOVE_JOINTB = 24 289 | MOVE_HOME = 25 290 | SLEEP_INSTT = 26 291 | MOVE_CIRCLE = 27 292 | MOVE_LINE_TOOL = 28 293 | MOVE_SERVOJ = 29 294 | MOVE_SERVO_CART = 30 295 | 296 | SET_TCP_JERK = 31 297 | SET_TCP_MAXACC = 32 298 | SET_JOINT_JERK = 33 299 | SET_JOINT_MAXACC = 34 300 | SET_TCP_OFFSET = 35 301 | SET_LOAD_PARAM = 36 302 | SET_COLLIS_SENS = 37 303 | SET_TEACH_SENS = 38 304 | CLEAN_CONF = 39 305 | SAVE_CONF = 40 306 | 307 | GET_TCP_POSE = 41 308 | GET_JOINT_POS = 42 309 | GET_IK = 43 310 | GET_FK = 44 311 | IS_JOINT_LIMIT = 45 312 | IS_TCP_LIMIT = 46 313 | 314 | SET_REDUCED_TRSV = 47 315 | SET_REDUCED_P2PV = 48 316 | GET_REDUCED_MODE = 49 317 | SET_REDUCED_MODE = 50 318 | SET_GRAVITY_DIR = 51 319 | SET_LIMIT_XYZ = 52 320 | GET_REDUCED_STATE = 53 321 | 322 | SET_SERVOT = 54 # no longer supported 323 | GET_JOINT_TAU = 55 324 | SET_SAFE_LEVEL = 56 325 | GET_SAFE_LEVEL = 57 326 | 327 | SET_REDUCED_JRANGE = 58 328 | SET_FENSE_ON = 59 329 | SET_COLLIS_REB = 60 330 | 331 | SET_TRAJ_RECORD = 61 332 | SAVE_TRAJ = 62 333 | LOAD_TRAJ = 63 334 | PLAY_TRAJ = 64 335 | GET_TRAJ_RW_STATUS = 65 336 | SET_ALLOW_APPROX_MOTION = 66 337 | GET_DH = 67 338 | SET_DH = 68 339 | GET_MOVEMENT = 69 340 | 341 | REPORT_TAU_OR_I = 70 342 | SET_TIMER = 71 343 | CANCEL_TIMER = 72 344 | SET_WORLD_OFFSET = 73 345 | CNTER_RESET = 74 346 | CNTER_PLUS = 75 347 | 348 | CAL_POSE_OFFSET = 76 349 | 350 | SET_SELF_COLLIS_CHECK = 77 351 | SET_COLLIS_TOOL = 78 352 | SET_SIMULATION_ROBOT = 79 353 | SET_CARTV_CONTINUE = 80 354 | 355 | VC_SET_JOINTV = 81 356 | VC_SET_CARTV = 82 357 | MOVE_RELATIVE = 83 358 | 359 | GET_TCP_POSE_AA = 91 360 | MOVE_LINE_AA = 92 361 | MOVE_SERVO_CART_AA = 93 362 | 363 | SERVO_W16B = 101 364 | SERVO_R16B = 102 365 | SERVO_W32B = 103 366 | SERVO_R32B = 104 367 | SERVO_ZERO = 105 368 | SERVO_DBMSG = 106 369 | SERVO_ERROR = 107 370 | 371 | CALI_TCP_POSE = 111 372 | CALI_TCP_ORIENT = 112 373 | CALI_WRLD_ORIENT = 113 374 | CALI_WRLD_POSE = 114 375 | IDEN_FRIC = 115 376 | 377 | TGPIO_MB_TIOUT = 123 378 | TGPIO_MODBUS = 124 379 | TGPIO_ERR = 125 380 | TGPIO_W16B = 127 381 | TGPIO_R16B = 128 382 | TGPIO_W32B = 129 383 | TGPIO_R32B = 130 384 | 385 | CGPIO_GET_DIGIT = 131 386 | CGPIO_GET_ANALOG1 = 132 387 | CGPIO_GET_ANALOG2 = 133 388 | CGPIO_SET_DIGIT = 134 389 | CGPIO_SET_ANALOG1 = 135 390 | CGPIO_SET_ANALOG2 = 136 391 | CGPIO_SET_IN_FUN = 137 392 | CGPIO_SET_OUT_FUN = 138 393 | CGPIO_GET_STATE = 139 394 | 395 | GET_PWR_VERSION = 140 396 | GET_HD_TYPES = 141 397 | DELAYED_CGPIO_SET = 142 398 | DELAYED_TGPIO_SET = 143 399 | POSITION_CGPIO_SET = 144 400 | POSITION_TGPIO_SET = 145 401 | SET_IO_STOP_RESET = 146 402 | POSITION_CGPIO_SET_ANALOG = 147 403 | 404 | FTSENSOR_GET_DATA_OLD = 150 # only available in firmware version < 1.8.3 405 | FTSENSOR_GET_DATA = 200 406 | FTSENSOR_ENABLE = 201 407 | FTSENSOR_SET_APP = 202 408 | FTSENSOR_GET_APP = 203 409 | IDEN_LOAD = 204 410 | FTSENSOR_CALI_LOAD_OFFSET = 205 411 | FTSENSOR_SET_ZERO = 206 412 | IMPEDANCE_CONFIG = 207 413 | FORCE_CTRL_PID = 208 414 | FORCE_CTRL_CONFIG = 209 415 | IMPEDANCE_CTRL_MBK = 210 416 | IMPEDANCE_CTRL_CONFIG = 211 417 | FTSENSOR_GET_CONFIG = 212 418 | 419 | GET_TRAJ_SPEEDING = 230 420 | GET_MAX_JOINT_VELOCITY = 231 421 | SET_COMMON_PARAM = 232 422 | GET_COMMON_PARAM = 233 423 | GET_COMMON_INFO = 234 424 | 425 | TGPIO_COM_TIOUT = 240 426 | TGPIO_COM_DATA = 241 427 | 428 | FEEDBACK_CHECK = 253 429 | SET_FEEDBACK_TYPE = 254 430 | 431 | class UxbusConf: 432 | SET_TIMEOUT = 2000 # ms 433 | GET_TIMEOUT = 2000 # ms 434 | 435 | class ServoConf: 436 | CON_EN = 0x0100 437 | CON_MODE = 0x0101 438 | CON_DIR = 0x0102 439 | 440 | SV3MOD_POS = 0 441 | SV3MOD_SPD = 1 442 | SV3MOD_FOS = 2 443 | SV3_SAVE = 0x1000 444 | 445 | BRAKE = 0x0104 446 | GET_TEMP = 0x000E 447 | ERR_CODE = 0x000F 448 | OVER_TEMP = 0x0108 449 | CURR_CURR = 0x0001 450 | POS_KP = 0x0200 451 | POS_FWDKP = 0x0201 452 | POS_PWDTC = 0x0202 453 | SPD_KP = 0x0203 454 | SPD_KI = 0x0204 455 | CURR_KP = 0x090C 456 | CURR_KI = 0x090D 457 | SPD_IFILT = 0x030C 458 | SPD_OFILT = 0x030D 459 | POS_CMDILT = 0x030E 460 | CURR_IFILT = 0x0401 461 | POS_KD = 0x0205 462 | POS_ACCT = 0x0300 463 | POS_DECT = 0x0301 464 | POS_STHT = 0x0302 465 | POS_SPD = 0x0303 466 | MT_ID = 0x1600 467 | BAUDRATE = 0x0601 468 | SOFT_REBOOT = 0x0607 469 | TAGET_TOQ = 0x050a 470 | CURR_TOQ = 0x050c 471 | TOQ_SPD = 0x050e 472 | TAGET_POS = 0x0700 473 | CURR_POS = 0x0702 474 | HARD_VER = 0x0800 475 | SOFT_VER = 0x0801 476 | MT_TYPE = 0x0802 477 | MT_ZERO = 0x0817 478 | RESET_PVL = 0x0813 479 | CAL_ZERO = 0x080C 480 | ERR_SWITCH = 0x0910 481 | RESET_ERR = 0x0109 482 | SV3_BRO_ID = 0xFF 483 | 484 | MODBUS_BAUDRATE = 0x0A0B 485 | TOOL_MB_TIMEOUT = 0x0A0E 486 | TI2_IN = 0x0A12 487 | TI2_TIME = 0x0A13 488 | DIGITAL_IN = 0x0A14 489 | DIGITAL_OUT = 0x0A15 490 | ANALOG_IO1 = 0x0A16 491 | ANALOG_IO2 = 0x0A17 492 | 493 | BACK_ORIGIN = 0x0A0A 494 | STOP_TRACK = 0x0A0E 495 | 496 | class UxbusState: 497 | ERR_CODE = 1 # 有尚未清除的错误 498 | WAR_CODE = 2 # 有尚未清除的警告 499 | ERR_TOUT = 3 # 获取结果超时 500 | ERR_LENG = 4 # TCP回复长度错误 501 | ERR_NUM = 5 # TCP回复序号错误 502 | ERR_PROT = 6 # TCP协议标志错误 503 | ERR_FUN = 7 # TCP回复指令和发送指令不匹配 504 | ERR_NOTTCP = 8 # 发送错误 505 | STATE_NOT_READY = 9 # 未准备好运动 506 | INVALID = 10 # 结果无效或执行失败 507 | ERR_OTHER = 11 # 其它错误 508 | ERR_PARAM = 12 # 参数错误 509 | 510 | class TrajState: 511 | IDLE = 0 512 | LOADING = 1 513 | LOAD_SUCCESS = 2 514 | LOAD_FAIL = 3 515 | SAVING = 4 516 | SAVE_SUCCESS = 5 517 | SAVE_FAIL = 6 518 | 519 | class BioGripperState: 520 | IS_STOP = 0 521 | IS_MOTION = 1 522 | IS_DETECTED = 2 523 | IS_FAULT = 3 524 | IS_NOT_ENABLED = 0 525 | IS_ENABLING = 1 526 | IS_ENABLED = 2 527 | 528 | class CollisionToolType: 529 | NONE = 0 530 | XARM_GRIPPER = 1 531 | XARM_VACUUM_GRIPPER = 2 532 | XARM_BIO_GRIPPER = 3 533 | ROBOTIQ_2F85 = 4 534 | ROBOTIQ_2F140 = 5 535 | 536 | USE_PRIMITIVES = 20 # just for judgement, threshold. 537 | CYLINDER = 21 # radius, height 538 | BOX = 22 # x, y, z in tool coordinate direction 539 | 540 | class FeedbackType: 541 | MOTION_START = 1 542 | MOTION_FINISH = 2 543 | TRIGGER = 4 544 | OTHER_START = 32 545 | OTHER_FINISH = 64 546 | 547 | class FeedbackCode: 548 | SUCCESS = 0 549 | FAILURE = 1 550 | DISCARD = 2 551 | 552 | 553 | 554 | -------------------------------------------------------------------------------- /xarm/core/utils/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | -------------------------------------------------------------------------------- /xarm/core/utils/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/utils/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/utils/__pycache__/convert.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/utils/__pycache__/convert.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/utils/__pycache__/crc16.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/utils/__pycache__/crc16.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/utils/__pycache__/log.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/utils/__pycache__/log.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/utils/convert.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | # 5 | # ./common_modules/common_type.py 6 | # Copyright (C) 2018.4 - UFactory. 7 | # Author: Jimy Zhang 8 | # 9 | # 10 | 11 | import struct 12 | 13 | 14 | def fp32_to_bytes(data, is_big_endian=False): 15 | """小端字节序""" 16 | return bytes(struct.pack('>f' if is_big_endian else 'i' if is_big_endian else ' 0 27 | ret = int32_to_bytes(data[0]) 28 | for i in range(1, n): 29 | ret += int32_to_bytes(data[i]) 30 | return ret 31 | 32 | 33 | def bytes_to_fp32(data): 34 | """小端字节序""" 35 | byte = bytes([data[0]]) 36 | byte += bytes([data[1]]) 37 | byte += bytes([data[2]]) 38 | byte += bytes([data[3]]) 39 | ret = struct.unpack(' 0 46 | ret = fp32_to_bytes(data[0]) 47 | for i in range(1, n): 48 | ret += fp32_to_bytes(data[i]) 49 | return ret 50 | 51 | 52 | def bytes_to_fp32s(data, n): 53 | """小端字节序""" 54 | ret = [0] * n 55 | for i in range(n): 56 | ret[i] = bytes_to_fp32(data[i * 4:i * 4 + 4]) 57 | return ret 58 | 59 | 60 | def u16_to_bytes(data): 61 | """大端字节序""" 62 | bts = bytes([data // 256 % 256]) 63 | bts += bytes([data % 256]) 64 | return bts 65 | 66 | 67 | def u16s_to_bytes(data, num): 68 | """大端字节序""" 69 | bts = b'' 70 | if num != 0: 71 | bts = u16_to_bytes(data[0]) 72 | for i in range(1, num): 73 | bts += u16_to_bytes(data[i]) 74 | return bts 75 | 76 | 77 | def bytes_to_u16(data): 78 | """大端字节序""" 79 | data_u16 = data[0] << 8 | data[1] 80 | return data_u16 81 | 82 | 83 | def bytes_to_u16s(data, n): 84 | """大端字节序""" 85 | ret = [0] * n 86 | for i in range(n): 87 | ret[i] = bytes_to_u16(data[i * 2: i * 2 + 2]) 88 | return ret 89 | 90 | 91 | def bytes_to_16s(data, n): 92 | """大端字节序""" 93 | ret = [0] * n 94 | for i in range(n): 95 | ret[i] = struct.unpack('>h', bytes(data[i * 2: i * 2 + 2]))[0] 96 | return ret 97 | 98 | 99 | def bytes_to_u32(data): 100 | """大端字节序""" 101 | data_u32 = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3] 102 | return data_u32 103 | 104 | 105 | def bytes_to_u64(data): 106 | data_u64 = data[0] << 56 | data[1] << 48 | data[2] << 40 | data[3] << 32 | data[4] << 24 | data[5] << 16 | data[6] << 8 | data[7] 107 | return data_u64 108 | 109 | 110 | def bytes_to_num32(data, fmt='>l'): 111 | byte = bytes([data[0]]) 112 | byte += bytes([data[1]]) 113 | byte += bytes([data[2]]) 114 | byte += bytes([data[3]]) 115 | ret = struct.unpack(fmt, byte) 116 | return ret[0] 117 | 118 | 119 | def bytes_to_long_big(data): 120 | """大端字节序""" 121 | return bytes_to_num32(data, '>l') 122 | 123 | def bytes_to_int32(data): 124 | return int.from_bytes(data, byteorder='big', signed=True) -------------------------------------------------------------------------------- /xarm/core/utils/crc16.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # 4 | # ./common_modules/crc16.py 5 | # Copyright (C) 2018.4 - UFactory. 6 | # Author: Jimy Zhang 7 | # 8 | # 9 | 10 | 11 | CRC_TABLE_H = ( 12 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 13 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 14 | 0x40, 0x01, 0xC0, 15 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 16 | 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 17 | 0xC0, 0x80, 0x41, 18 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 19 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 20 | 0x41, 0x01, 0xC0, 21 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 22 | 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 23 | 0xC1, 0x81, 0x40, 24 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 25 | 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 26 | 0x40, 0x01, 0xC0, 27 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 28 | 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 29 | 0xC0, 0x80, 0x41, 30 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 31 | 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 32 | 0x40, 0x01, 0xC0, 33 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 34 | 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 35 | 0xC0, 0x80, 0x41, 36 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 37 | 0xC1, 0x81, 0x40) 38 | 39 | CRC_TABLE_L = ( 40 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 41 | 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 42 | 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 43 | 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 44 | 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 45 | 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 46 | 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 47 | 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 48 | 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 49 | 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 50 | 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 51 | 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 52 | 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 53 | 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 54 | 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 55 | 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 56 | 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 57 | 0x80, 0x40) 58 | 59 | 60 | def crc_modbus(data): 61 | leng = len(data) 62 | init_crch = 0xFF 63 | init_crcl = 0xFF 64 | i = 0 65 | 66 | while leng > 0: 67 | index = init_crch ^ data[i] 68 | i += 1 69 | init_crch = init_crcl ^ CRC_TABLE_H[index] 70 | init_crcl = CRC_TABLE_L[index] 71 | leng -= 1 72 | s = init_crch << 8 | init_crcl 73 | crc = bytes([s // 256 % 256]) 74 | crc += bytes([s % 256]) 75 | return crc 76 | 77 | -------------------------------------------------------------------------------- /xarm/core/utils/debug_print.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: UTF-8 -*- 3 | 4 | # 5 | # ./common_modules/debug_print.py 6 | # Copyright (C) 2018.4 - UFactory. 7 | # Author: Jimy Zhang 8 | # 9 | # 10 | 11 | 12 | def print_hex(str, data, len): 13 | for i in range(len): 14 | str += ('%0.2X ' % data[i]) 15 | print(str) 16 | 17 | 18 | def print_nvect(str, data, len): 19 | for i in range(len): 20 | str += ('%0.3f ' % data[i]) 21 | print(str) 22 | -------------------------------------------------------------------------------- /xarm/core/utils/log.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import logging 10 | import functools 11 | import sys 12 | import os 13 | 14 | log_path = os.path.join(os.path.expanduser('~'), '.UFACTORY', 'log', 'xarm', 'sdk') 15 | if not os.path.exists(log_path): 16 | os.makedirs(log_path) 17 | 18 | logging.VERBOSE = 5 19 | logging.addLevelName(logging.VERBOSE, 'VERBOSE') 20 | 21 | 22 | class Logger(logging.Logger): 23 | logger_fmt = '{}[%(levelname)s][%(asctime)s][%(filename)s:%(lineno)d] - - %(message)s' 24 | logger_date_fmt = '%Y-%m-%d %H:%M:%S' 25 | stream_handler_fmt = logger_fmt.format('[SDK]') 26 | stream_handler_date_fmt = logger_date_fmt 27 | stream_handler = logging.StreamHandler(sys.stdout) 28 | stream_handler.setLevel(logging.VERBOSE) 29 | stream_handler.setFormatter(logging.Formatter(stream_handler_fmt, stream_handler_date_fmt)) 30 | 31 | logger = logging.Logger(__name__) 32 | logger.setLevel(logging.VERBOSE) 33 | logger.addHandler(stream_handler) 34 | 35 | def __new__(cls, *args, **kwargs): 36 | if not hasattr(cls, 'logger'): 37 | cls.logger = super(Logger, cls).__new__(cls, *args, **kwargs) 38 | return cls.logger 39 | 40 | logger = Logger(__name__) 41 | logger.setLevel(logging.WARNING) 42 | 43 | logger.VERBOSE = logging.VERBOSE 44 | logger.DEBUG = logging.DEBUG 45 | logger.INFO = logging.INFO 46 | logger.WARN = logging.WARN 47 | logger.WARNING = logging.WARNING 48 | logger.ERROR = logging.ERROR 49 | logger.CRITICAL = logging.CRITICAL 50 | 51 | logger.verbose = functools.partial(logger.log, logger.VERBOSE) 52 | 53 | # findCaller = logger.findCaller 54 | # 55 | # 56 | # def log(msg, *args, **kwargs): 57 | # level = kwargs.pop('level', logger.INFO) 58 | # if logger.findCaller == findCaller: 59 | # rv = findCaller(kwargs.pop('stack_info', False)) 60 | # logger.findCaller = lambda x: rv 61 | # logger.log(level, msg, *args, **kwargs) 62 | # if logger.findCaller != findCaller: 63 | # logger.findCaller = findCaller 64 | # 65 | # logger.verbose = functools.partial(log, level=logger.VERBOSE) 66 | # logger.debug = functools.partial(log, level=logger.DEBUG) 67 | # logger.info = functools.partial(log, level=logger.INFO) 68 | # logger.warning = functools.partial(log, level=logger.WARNING) 69 | # logger.error = functools.partial(log, level=logger.ERROR) 70 | # logger.critical = functools.partial(log, level=logger.CRITICAL) 71 | 72 | colors = { 73 | 'none': '{}', 74 | 'white': '\033[30m{}\033[0m', 75 | 'red': '\033[31m{}\033[0m', 76 | 'green': '\033[32m{}\033[0m', 77 | 'orange': '\033[33m{}\033[0m', 78 | 'blue': '\033[34m{}\033[0m', 79 | 'purple': '\033[35m{}\033[0m', 80 | 'cyan': '\033[36m{}\033[0m', 81 | 'light_gray': '\033[37m{}\033[0m', 82 | 'dark_gray': '\033[90m{}\033[0m', 83 | 'light_red': '\033[91m{}\033[0m', 84 | 'light_green': '\033[92m{}\033[0m', 85 | 'yellow': '\033[93m{}\033[0m', 86 | 'light_blue': '\033[94m{}\033[0m', 87 | 'pink': '\033[95m{}\033[0m', 88 | 'light_cyan': '\033[96m{}\033[0m', 89 | } 90 | 91 | 92 | def pretty_print(*args, sep=' ', end='\n', file=None, color='none'): 93 | msg = '' 94 | for arg in args: 95 | msg += arg + sep 96 | msg = msg.rstrip(sep) 97 | # msg = colors.get(color, '{}').format(msg) 98 | print(msg, end=end, file=file) 99 | -------------------------------------------------------------------------------- /xarm/core/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '0.1.0' 2 | -------------------------------------------------------------------------------- /xarm/core/wrapper/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | from .uxbus_cmd_ser import UxbusCmdSer 11 | from .uxbus_cmd_tcp import UxbusCmdTcp 12 | -------------------------------------------------------------------------------- /xarm/core/wrapper/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/wrapper/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/wrapper/__pycache__/uxbus_cmd.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/wrapper/__pycache__/uxbus_cmd.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/wrapper/__pycache__/uxbus_cmd_ser.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/wrapper/__pycache__/uxbus_cmd_ser.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/wrapper/__pycache__/uxbus_cmd_tcp.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/core/wrapper/__pycache__/uxbus_cmd_tcp.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/core/wrapper/uxbus_cmd_ser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import time 12 | from ..utils import crc16 13 | from .uxbus_cmd import UxbusCmd 14 | from ..config.x_config import XCONF 15 | 16 | 17 | def debug_log_datas(datas, label=''): 18 | print('{}:'.format(label), end=' ') 19 | for i in range(len(datas)): 20 | print('{:x}'.format(datas[i]).zfill(2), end=' ') 21 | # print(hex(rx_data[i]), end=',') 22 | print() 23 | 24 | 25 | class UxbusCmdSer(UxbusCmd): 26 | def __init__(self, arm_port, fromid=XCONF.SerialConf.UXBUS_DEF_FROMID, toid=XCONF.SerialConf.UXBUS_DEF_TOID): 27 | super(UxbusCmdSer, self).__init__() 28 | self.arm_port = arm_port 29 | self.fromid = fromid 30 | self.toid = toid 31 | arm_port.flush(fromid, toid) 32 | self._has_err_warn = False 33 | 34 | @property 35 | def has_err_warn(self): 36 | return self._has_err_warn 37 | 38 | @has_err_warn.setter 39 | def has_err_warn(self, value): 40 | self._has_err_warn = value 41 | 42 | def set_protocol_identifier(self, protocol_identifier): 43 | return 0 44 | 45 | def get_protocol_identifier(self): 46 | return 0 47 | 48 | def check_protocol_header(self, data, t_trans_id, t_prot_id, t_unit_id): 49 | return 0 50 | 51 | def check_private_protocol(self, data): 52 | self._state_is_ready = not (data[3] & 0x10) 53 | if data[3] & 0x08: 54 | return XCONF.UxbusState.INVALID 55 | if data[3] & 0x40: 56 | self._has_err_warn = True 57 | return XCONF.UxbusState.ERR_CODE 58 | elif data[3] & 0x20: 59 | self._has_err_warn = True 60 | return XCONF.UxbusState.WAR_CODE 61 | else: 62 | self._has_err_warn = False 63 | return 0 64 | 65 | def send_modbus_request(self, reg, txdata, num, prot_id=-1, t_id=None): 66 | send_data = bytes([self.fromid, self.toid, num + 1, reg]) 67 | for i in range(num): 68 | send_data += bytes([txdata[i]]) 69 | send_data += crc16.crc_modbus(send_data) 70 | self.arm_port.flush() 71 | if self._debug: 72 | debug_log_datas(send_data, label='send') 73 | return self.arm_port.write(send_data) 74 | 75 | def recv_modbus_response(self, t_funcode, t_trans_id, num, timeout, t_prot_id=-1, ret_raw=False): 76 | ret = [0] * 254 if num == -1 else [0] * (num + 1) 77 | expired = time.monotonic() + timeout 78 | ret[0] = XCONF.UxbusState.ERR_TOUT 79 | while time.monotonic() < expired: 80 | remaining = expired - time.monotonic() 81 | rx_data = self.arm_port.read(remaining) 82 | if rx_data != -1 and len(rx_data) > 5: 83 | if self._debug: 84 | debug_log_datas(rx_data, label='recv') 85 | ret[0] = self.check_private_protocol(rx_data) 86 | num = rx_data[2] if num == -1 else num 87 | length = len(rx_data) - 4 88 | for i in range(num): 89 | if i >= length: 90 | break 91 | ret[i + 1] = rx_data[i + 4] 92 | return ret 93 | time.sleep(0.001) 94 | return ret 95 | -------------------------------------------------------------------------------- /xarm/core/wrapper/uxbus_cmd_tcp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Jimy Zhang 8 | # Author: Vinman 9 | 10 | 11 | import time 12 | import struct 13 | from ..utils import convert 14 | from .uxbus_cmd import UxbusCmd, lock_require 15 | from ..config.x_config import XCONF 16 | 17 | STANDARD_MODBUS_TCP_PROTOCOL = 0x00 18 | PRIVATE_MODBUS_TCP_PROTOCOL = 0x02 19 | TRANSACTION_ID_MAX = 65535 # cmd序号 最大值 20 | 21 | 22 | def debug_log_datas(datas, label=''): 23 | print('{}:'.format(label), end=' ') 24 | for i in range(len(datas)): 25 | print('{:x}'.format(datas[i]).zfill(2), end=' ') 26 | # print('0x{}'.format('{:x}'.format(datas[i]).zfill(2)), end=' ') 27 | # print(hex(rx_data[i]), end=',') 28 | print() 29 | 30 | 31 | class UxbusCmdTcp(UxbusCmd): 32 | def __init__(self, arm_port, set_feedback_key_tranid=None): 33 | super(UxbusCmdTcp, self).__init__(set_feedback_key_tranid=set_feedback_key_tranid) 34 | self.arm_port = arm_port 35 | self._has_err_warn = False 36 | self._last_comm_time = time.monotonic() 37 | self._transaction_id = 1 38 | self._protocol_identifier = PRIVATE_MODBUS_TCP_PROTOCOL 39 | 40 | @property 41 | def has_err_warn(self): 42 | return self._has_err_warn 43 | 44 | @has_err_warn.setter 45 | def has_err_warn(self, value): 46 | self._has_err_warn = value 47 | 48 | @lock_require 49 | def set_protocol_identifier(self, protocol_identifier): 50 | if self._protocol_identifier != protocol_identifier: 51 | self._protocol_identifier = protocol_identifier 52 | print('change protocol identifier to {}'.format(self._protocol_identifier)) 53 | return 0 54 | 55 | def get_protocol_identifier(self): 56 | return self._protocol_identifier 57 | 58 | def _get_trans_id(self): 59 | return self._transaction_id 60 | 61 | def check_protocol_header(self, data, t_trans_id, t_prot_id, t_unit_id): 62 | trans_id = convert.bytes_to_u16(data[0:2]) 63 | prot_id = convert.bytes_to_u16(data[2:4]) 64 | # length = convert.bytes_to_u16(data[4:6]) 65 | unit_id = data[6] # standard(unit_id), private(funcode) 66 | if trans_id != t_trans_id: 67 | return XCONF.UxbusState.ERR_NUM 68 | if prot_id != t_prot_id: 69 | return XCONF.UxbusState.ERR_PROT 70 | if unit_id != t_unit_id: 71 | return XCONF.UxbusState.ERR_FUN 72 | # if len(data) != length + 6: 73 | # return XCONF.UxbusState.ERR_LENG 74 | return 0 75 | 76 | def check_private_protocol(self, data): 77 | state = data[7] 78 | self._state_is_ready = not (state & 0x10) 79 | if state & 0x08: 80 | return XCONF.UxbusState.INVALID 81 | if state & 0x40: 82 | self._has_err_warn = True 83 | return XCONF.UxbusState.ERR_CODE 84 | if state & 0x20: 85 | self._has_err_warn = True 86 | return XCONF.UxbusState.WAR_CODE 87 | self._has_err_warn = False 88 | return 0 89 | 90 | def send_modbus_request(self, unit_id, pdu_data, pdu_len, prot_id=-1, t_id=None): 91 | trans_id = self._transaction_id if t_id is None else t_id 92 | prot_id = self._protocol_identifier if prot_id < 0 else prot_id 93 | send_data = convert.u16_to_bytes(trans_id) 94 | send_data += convert.u16_to_bytes(prot_id) 95 | send_data += convert.u16_to_bytes(pdu_len + 1) 96 | send_data += bytes([unit_id]) 97 | for i in range(pdu_len): 98 | send_data += bytes([pdu_data[i]]) 99 | self.arm_port.flush() 100 | if self._debug: 101 | debug_log_datas(send_data, label='send({})'.format(unit_id)) 102 | ret = self.arm_port.write(send_data) 103 | if ret != 0: 104 | return -1 105 | if t_id is None: 106 | self._transaction_id = self._transaction_id % TRANSACTION_ID_MAX + 1 107 | return trans_id 108 | 109 | def recv_modbus_response(self, t_unit_id, t_trans_id, num, timeout, t_prot_id=-1, ret_raw=False): 110 | prot_id = self._protocol_identifier if t_prot_id < 0 else t_prot_id 111 | ret = [0] * 320 if num == -1 else [0] * (num + 1) 112 | ret[0] = XCONF.UxbusState.ERR_TOUT 113 | expired = time.monotonic() + timeout 114 | while time.monotonic() < expired: 115 | remaining = expired - time.monotonic() 116 | rx_data = self.arm_port.read(remaining) 117 | if rx_data == -1: 118 | time.sleep(0.001) 119 | continue 120 | self._last_comm_time = time.monotonic() 121 | if self._debug: 122 | debug_log_datas(rx_data, label='recv({})'.format(t_unit_id)) 123 | code = self.check_protocol_header(rx_data, t_trans_id, prot_id, t_unit_id) 124 | if code != 0: 125 | if code != XCONF.UxbusState.ERR_NUM: 126 | ret[0] = code 127 | return ret 128 | else: 129 | continue 130 | if prot_id != STANDARD_MODBUS_TCP_PROTOCOL and not ret_raw: 131 | # Private Modbus TCP Protocol 132 | ret[0] = self.check_private_protocol(rx_data) 133 | num = convert.bytes_to_u16(rx_data[4:6]) - 2 134 | ret = ret[:num + 1] if len(ret) >= num + 1 else [ret[0]] * (num + 1) 135 | length = len(rx_data) - 8 136 | for i in range(num): 137 | if i >= length: 138 | break 139 | ret[i + 1] = rx_data[i + 8] 140 | else: 141 | # Standard Modbus TCP Protocol 142 | ret[0] = 0 143 | num = convert.bytes_to_u16(rx_data[4:6]) + 6 144 | ret = ret[:num + 1] if len(ret) >= num + 1 else [ret[0]] * (num + 1) 145 | length = len(rx_data) 146 | for i in range(num): 147 | if i >= length: 148 | break 149 | ret[i + 1] = rx_data[i] 150 | return ret 151 | return ret 152 | 153 | # def send_hex_request(self, send_data): 154 | # trans_id = int('0x' + str(send_data[0]) + str(send_data[1]), 16) 155 | # data_str = b'' 156 | # for data in send_data: 157 | # data_str += bytes.fromhex(data) 158 | # send_data = data_str 159 | # self.arm_port.flush() 160 | # if self._debug: 161 | # debug_log_datas(send_data, label='send') 162 | # ret = self.arm_port.write(send_data) 163 | # if ret != 0: 164 | # return -1 165 | # return trans_id 166 | 167 | # def recv_hex_request(self, t_trans_id, timeout, t_prot_id=-1): 168 | # prot_id = 2 169 | # expired = time.monotonic() + timeout 170 | # while time.monotonic() < expired: 171 | # remaining = expired - time.monotonic() 172 | # rx_data = self.arm_port.read(remaining) 173 | # if rx_data == -1: 174 | # time.sleep(0.001) 175 | # continue 176 | # self._last_comm_time = time.monotonic() 177 | # if self._debug: 178 | # debug_log_datas(rx_data, label='recv') 179 | # code = self.check_protocol_header(rx_data, t_trans_id, prot_id, rx_data[6]) 180 | # if code != 0: 181 | # if code != XCONF.UxbusState.ERR_NUM: 182 | # return code 183 | # else: 184 | # continue 185 | # break 186 | # if rx_data != -1 and len(rx_data) > 0: 187 | # recv_datas = [] 188 | # for i in range(len(rx_data)): 189 | # recv_datas.append('{:x}'.format(rx_data[i]).zfill(2)) 190 | # return recv_datas 191 | # else: 192 | # return rx_data if rx_data else 3 193 | 194 | ####################### Standard Modbus TCP API ######################## 195 | @lock_require 196 | def __standard_modbus_tcp_request(self, pdu, unit_id=0x01): 197 | ret = self.send_modbus_request(unit_id, pdu, len(pdu), prot_id=STANDARD_MODBUS_TCP_PROTOCOL) 198 | if ret == -1: 199 | return XCONF.UxbusState.ERR_NOTTCP, b'' 200 | ret = self.recv_modbus_response(unit_id, ret, -1, 10000, t_prot_id=STANDARD_MODBUS_TCP_PROTOCOL) 201 | code, recv_data = ret[0], bytes(ret[1:]) 202 | if code == 0 and recv_data[7] == pdu[0] + 0x80: # len(recv_data) == 9 203 | # print('request exception, exp={}, res={}'.format(recv_data[8], recv_data)) 204 | return recv_data[8] + 0x80, recv_data 205 | # elif code != 0: 206 | # print('recv timeout, len={}, res={}'.format(len(recv_data), recv_data)) 207 | return code, recv_data 208 | 209 | def __read_bits(self, addr, quantity, func_code=0x01): 210 | assert func_code == 0x01 or func_code == 0x02 211 | pdu = struct.pack('>BHH', func_code, addr, quantity) 212 | code, res_data = self.__standard_modbus_tcp_request(pdu) 213 | if code == 0 and len(res_data) == 9 + (quantity + 7) // 8: 214 | return code, [(res_data[9 + i // 8] >> (i % 8) & 0x01) for i in range(quantity)] 215 | else: 216 | return code, res_data 217 | 218 | def __read_registers(self, addr, quantity, func_code=0x03, is_signed=False): 219 | assert func_code == 0x03 or func_code == 0x04 220 | pdu = struct.pack('>BHH', func_code, addr, quantity) 221 | code, res_data = self.__standard_modbus_tcp_request(pdu) 222 | if code == 0 and len(res_data) == 9 + quantity * 2: 223 | return 0, list(struct.unpack('>{}{}'.format(quantity, 'h' if is_signed else 'H'), res_data[9:])) 224 | else: 225 | return code, res_data 226 | 227 | def read_coil_bits(self, addr, quantity): 228 | """ 229 | func_code: 0x01 230 | """ 231 | return self.__read_bits(addr, quantity, func_code=0x01) 232 | 233 | def read_input_bits(self, addr, quantity): 234 | """ 235 | func_code: 0x02 236 | """ 237 | return self.__read_bits(addr, quantity, func_code=0x02) 238 | 239 | def read_holding_registers(self, addr, quantity, is_signed=False): 240 | """ 241 | func_code: 0x03 242 | """ 243 | return self.__read_registers(addr, quantity, func_code=0x03, is_signed=is_signed) 244 | 245 | def read_input_registers(self, addr, quantity, is_signed=False): 246 | """ 247 | func_code: 0x04 248 | """ 249 | return self.__read_registers(addr, quantity, func_code=0x04, is_signed=is_signed) 250 | 251 | def write_single_coil_bit(self, addr, bit_val): 252 | """ 253 | func_code: 0x05 254 | """ 255 | pdu = struct.pack('>BHH', 0x05, addr, 0xFF00 if bit_val else 0x0000) 256 | return self.__standard_modbus_tcp_request(pdu)[0] 257 | 258 | def write_single_holding_register(self, addr, reg_val): 259 | """ 260 | func_code: 0x06 261 | """ 262 | # pdu = struct.pack('>BHH', 0x06, addr, reg_val) 263 | pdu = struct.pack('>BH', 0x06, addr) 264 | pdu += convert.u16_to_bytes(reg_val) 265 | return self.__standard_modbus_tcp_request(pdu)[0] 266 | 267 | def write_multiple_coil_bits(self, addr, bits): 268 | """ 269 | func_code: 0x0F 270 | """ 271 | datas = [0] * ((len(bits) + 7) // 8) 272 | for i in range(len(bits)): 273 | if bits[i]: 274 | datas[i // 8] |= (1 << (i % 8)) 275 | pdu = struct.pack('>BHHB{}B'.format(len(datas)), 0x0F, addr, len(bits), len(datas), *datas) 276 | return self.__standard_modbus_tcp_request(pdu)[0] 277 | 278 | def write_multiple_holding_registers(self, addr, regs): 279 | """ 280 | func_code: 0x10 281 | """ 282 | # pdu = struct.pack('>BHHB{}H'.format(len(regs)), 0x10, addr, len(regs), len(regs) * 2, *regs) 283 | pdu = struct.pack('>BHHB', 0x10, addr, len(regs), len(regs) * 2) 284 | pdu += convert.u16s_to_bytes(regs, len(regs)) 285 | return self.__standard_modbus_tcp_request(pdu)[0] 286 | 287 | def mask_write_holding_register(self, addr, and_mask, or_mask): 288 | """ 289 | func_code: 0x16 290 | """ 291 | pdu = struct.pack('>BHHH', 0x16, addr, and_mask, or_mask) 292 | return self.__standard_modbus_tcp_request(pdu)[0] 293 | 294 | def write_and_read_holding_registers(self, r_addr, r_quantity, w_addr, w_regs, is_signed=False): 295 | """ 296 | func_code: 0x17 297 | """ 298 | # pdu = struct.pack('>BHHHHB{}{}'.format(len(w_regs), 'h' if w_signed else 'H'), 0x17, r_addr, r_quantity, w_addr, len(w_regs), len(w_regs) * 2, *w_regs) 299 | pdu = struct.pack('>BHHHHB', 0x17, r_addr, r_quantity, w_addr, len(w_regs), len(w_regs) * 2) 300 | pdu += convert.u16s_to_bytes(w_regs, len(w_regs)) 301 | code, res_data = self.__standard_modbus_tcp_request(pdu) 302 | if code == 0 and len(res_data) == 9 + r_quantity * 2: 303 | return 0, struct.unpack('>{}{}'.format(r_quantity, 'h' if is_signed else 'H'), res_data[9:]) 304 | else: 305 | return code, res_data -------------------------------------------------------------------------------- /xarm/tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/__init__.py -------------------------------------------------------------------------------- /xarm/tools/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/__pycache__/threads.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/__pycache__/threads.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__init__.py: -------------------------------------------------------------------------------- 1 | from ._blockly_tool import BlocklyTool -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/_blockly_base.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/_blockly_base.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/_blockly_handler.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/_blockly_handler.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/_blockly_highlight.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/_blockly_highlight.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/_blockly_node.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/_blockly_node.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/__pycache__/_blockly_tool.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/tools/blockly/__pycache__/_blockly_tool.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/tools/blockly/_blockly_highlight.py: -------------------------------------------------------------------------------- 1 | 2 | HIGHLIGHT_BLOCKS = [ 3 | 'studio_play_recording', 4 | 'studio_run_python', 5 | 'studio_run_traj', 6 | 'studio_run_file', 7 | 'app_studio_traj', 8 | 9 | 'wait_until', 10 | 'wait', 11 | 'loop_break', 12 | 13 | 'set_acceleration', 14 | 'set_speed', 15 | 'set_angle_speed', 16 | 'set_angle_acceleration', 17 | 18 | 'set_gripper', 19 | 'gripper_mode', 20 | 'gripper_enable', 21 | 'gripper_position', 22 | 'gripper_speed', 23 | 'gripper_set', 24 | 'gripper_set_status', 25 | 'set_suction_cup', 26 | 'get_suction_cup', 27 | 'check_air_pump_state', 28 | 'set_lite6_gripper', 29 | 30 | 'gpio_get_analog', 31 | 'gpio_get_digital', 32 | 'gpio_set_digital', 33 | 'gpio_get_controller_analog', 34 | 'gpio_get_controller_digital', 35 | 'gpio_set_controller_digital', 36 | 'gpio_set_controller_analog', 37 | 'gpio_set_digital_with_xyz', 38 | 'gpio_set_controller_digital_with_xyz', 39 | 'gpio_set_controller_analog_with_xyz', 40 | 41 | 'move_7', 42 | 'move_to', 43 | 'move_arc_to', 44 | 'move_circle', 45 | 'move', 46 | 'reset', 47 | 'sleep', 48 | 'motion_stop', 49 | 'motion_set_state', 50 | 'move_joints', 51 | 'move_joints_var', 52 | 'move_cartesian', 53 | 'move_cartesian_var', 54 | 'move_tool_line', 55 | 56 | 'set_tcp_load', 57 | 'set_tcp_offset', 58 | 'set_world_offset', 59 | 'set_counter_reset', 60 | 'set_counter_increase', 61 | 'set_collision_sensitivity', 62 | 63 | 'set_bio_gripper_init', 64 | 'set_bio_gripper', 65 | 'check_bio_gripper_is_catch', 66 | 'set_robotiq_init', 67 | 'set_robotiq_gripper', 68 | 'check_robotiq_is_catch', 69 | 70 | 'tool_message', 71 | 'tool_console', 72 | 'tool_console_with_variable', 73 | 74 | 'event_gpio_digital', 75 | 'event_gpio_analog', 76 | 'event_gpio_controller_digital', 77 | 'event_gpio_controller_analog', 78 | 79 | 'procedures_callnoreturn', 80 | 'procedures_callreturn', 81 | 82 | 'variables_set', 83 | 'math_change', 84 | 'loop_run_forever', 85 | 'controls_whileUntil', 86 | 'controls_repeat_ext', 87 | 'loop_break', 88 | 89 | 'python_code', 90 | 'python_expression', 91 | 'gpio_get_controller_ci_li', 92 | 'gpio_get_controller_di_li', 93 | 'gpio_get_tgpio_li' 94 | ] -------------------------------------------------------------------------------- /xarm/tools/blockly/_blockly_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2022, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | try: 10 | import xml.etree.cElementTree as ET 11 | except ImportError: 12 | import xml.etree.ElementTree as ET 13 | import re 14 | 15 | 16 | class _BlocklyNode(object): 17 | def __init__(self, xml_path): 18 | self._root = ET.parse(xml_path).getroot() 19 | self._ns = self.__get_ns() 20 | 21 | def __get_ns(self): 22 | try: 23 | r = re.compile('({.+})') 24 | if r.search(self._root.tag) is not None: 25 | ns = r.search(self._root.tag).group(1) 26 | else: 27 | ns = '' 28 | except Exception as e: 29 | print('get namespace exception: {}'.format(e)) 30 | ns = '' 31 | return ns 32 | 33 | def _get_node(self, tag, root=None): 34 | root = self._root if root is None else root 35 | return root.find(self._ns + tag) 36 | 37 | def _get_nodes(self, tag, root=None, descendant=False, **kwargs): 38 | root = self._root if root is None else root 39 | nodes = [] 40 | func = root.iter if descendant else root.findall 41 | for node in func(self._ns + tag): 42 | flag = True 43 | for k, v in kwargs.items(): 44 | if node.attrib[k] != v: 45 | flag = False 46 | if flag: 47 | nodes.append(node) 48 | return nodes 49 | 50 | def get_node(self, tag, root=None): 51 | """ 52 | Only call in studio 53 | """ 54 | return self._get_node(tag, root=root) 55 | 56 | def get_nodes(self, tag, root=None, descendant=False, **kwargs): 57 | """ 58 | Only call in studio 59 | """ 60 | return self._get_nodes(tag, root=root, descendant=descendant, **kwargs) 61 | -------------------------------------------------------------------------------- /xarm/tools/gcode.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2024, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import os 10 | import re 11 | import sys 12 | import socket 13 | import logging 14 | import threading 15 | 16 | logger = logging.Logger('gcode') 17 | logger_fmt = '[%(levelname)s][%(asctime)s][%(filename)s:%(lineno)d] - - %(message)s' 18 | logger_date_fmt = '%Y-%m-%d %H:%M:%S' 19 | stream_handler = logging.StreamHandler(sys.stdout) 20 | stream_handler.setLevel(logging.DEBUG) 21 | stream_handler.setFormatter(logging.Formatter(logger_fmt, logger_date_fmt)) 22 | logger.addHandler(stream_handler) 23 | logger.setLevel(logging.INFO) 24 | 25 | GCODE_PATTERN = r'([A-Z])([-+]?[0-9.]+)' 26 | CLEAN_PATTERN = r'\s+|\(.*?\)|;.*' 27 | 28 | 29 | class GcodeClient(object): 30 | def __init__(self, robot_ip): 31 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 32 | self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 33 | self.sock.setblocking(True) 34 | self.sock.connect((robot_ip, 504)) 35 | self._lock = threading.Lock() 36 | 37 | def execute(self, cmd): 38 | data = re.sub(CLEAN_PATTERN, '', cmd.strip().upper()) 39 | if not data: 40 | # logger.warning('[E] null after clean {}'.format(cmd)) 41 | return -1, [] 42 | if data[0] == '%': 43 | # logger.warning('[E] starts with % ({})'.format(cmd)) 44 | return -2, [] 45 | if not re.findall(GCODE_PATTERN, data): 46 | # logger.warning('[E] not found {}'.format(cmd)) 47 | return -3, [] 48 | data = data.encode('utf-8', 'replace') 49 | with self._lock: 50 | self.sock.send(data + b'\n') 51 | ret = self.sock.recv(5) 52 | code, mode_state, err = ret[0:3] 53 | state, mode = mode_state & 0x0F, mode_state >> 4 54 | cmdnum = ret[3] << 8 | ret[4] 55 | if code != 0 or err != 0: 56 | logger.error('[{}], code={}, err={}, mode={}, state={}, cmdnum={}'.format(cmd, code, err, mode, state, cmdnum)) 57 | elif state >= 4: 58 | logger.warning('[{}], code={}, err={}, mode={}, state={}, cmdnum={}'.format(cmd, code, err, mode, state, cmdnum)) 59 | return code, [mode, state, err, cmdnum] 60 | 61 | def execute_file(self, filepath): 62 | if not os.path.exists(filepath) or os.path.isdir(filepath): 63 | return -99 64 | with open(filepath, 'r') as f: 65 | for line in f.readlines(): 66 | cmd = line.strip() 67 | if not cmd: 68 | continue 69 | code, info = self.execute(cmd) 70 | if code < 0: 71 | continue 72 | if code != 0 or info[2] != 0: 73 | if code != 1 and code != 2: 74 | return code 75 | if cmd in ['M2', 'M02', 'M30']: 76 | logger.info('[{}] Program End'.format(cmd)) 77 | break 78 | return 0 79 | -------------------------------------------------------------------------------- /xarm/tools/list_ports.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | from serial.tools import list_ports 10 | 11 | def _dump_port(d): 12 | print('-' * 80) 13 | print(' device : {}'.format(d.device)) 14 | print(' hwid : {}'.format(d.hwid)) 15 | print(' product : {}'.format(d.hwid)) 16 | print(' description: {}'.format(d.hwid)) 17 | print('-' * 80) 18 | 19 | def get_ports(is_dump=True): 20 | ports = [] 21 | for i in list_ports.comports(): 22 | # if i.pid is not None and '{:04x}:{:04x}'.format(i.vid, i.pid) == vidpid: 23 | if i.pid is not None: 24 | if is_dump: 25 | _dump_port(i) 26 | ports.append({ 27 | 'pid': '{:04x}'.format(i.pid), 28 | 'vid': '{:04x}'.format(i.vid), 29 | 'device': i.device, 30 | 'serial_number': i.serial_number, 31 | 'hwid': i.hwid, 32 | 'name': i.name, 33 | 'description': i.description, 34 | 'interface': i.interface, 35 | 'location': i.location, 36 | 'manufacturer': i.manufacturer, 37 | 'product': i.product 38 | }) 39 | return ports -------------------------------------------------------------------------------- /xarm/tools/modbus_tcp.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2024, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import sys 10 | import time 11 | import socket 12 | import struct 13 | import logging 14 | import threading 15 | 16 | logger = logging.Logger('modbus_tcp') 17 | logger_fmt = '[%(levelname)s][%(asctime)s][%(filename)s:%(lineno)d] - - %(message)s' 18 | logger_date_fmt = '%Y-%m-%d %H:%M:%S' 19 | stream_handler = logging.StreamHandler(sys.stdout) 20 | stream_handler.setLevel(logging.DEBUG) 21 | stream_handler.setFormatter(logging.Formatter(logger_fmt, logger_date_fmt)) 22 | logger.addHandler(stream_handler) 23 | logger.setLevel(logging.INFO) 24 | 25 | 26 | class ModbusTcpClient(object): 27 | def __init__(self, ip, port=502, unit_id=0x01): 28 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 29 | self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 30 | self.sock.setblocking(True) 31 | self.sock.connect((ip, port)) 32 | self._transaction_id = 0 33 | self._protocol_id = 0x00 34 | self._unit_id = unit_id 35 | self._func_code = 0x00 36 | self._lock = threading.Lock() 37 | 38 | def __wait_to_response(self, transaction_id=None, unit_id=None, func_code=None, timeout=3): 39 | expired = time.monotonic() + timeout 40 | recv_data = b'' 41 | length = 0 42 | code = -3 # TIMEOUT 43 | send_transaction_id = transaction_id if transaction_id is not None else self._transaction_id 44 | send_unit_id = unit_id if unit_id is not None else self._unit_id 45 | send_func_code = func_code if func_code is not None else self._func_code 46 | while time.monotonic() < expired: 47 | if len(recv_data) < 7: 48 | recv_data += self.sock.recv(7 - len(recv_data)) 49 | if len(recv_data) < 7: 50 | continue 51 | if length == 0: 52 | length = struct.unpack('>H', recv_data[4:6])[0] 53 | if len(recv_data) < length + 6: 54 | recv_data += self.sock.recv(length + 6 - len(recv_data)) 55 | if len(recv_data) < length + 6: 56 | continue 57 | transaction_id = struct.unpack('>H', recv_data[0:2])[0] 58 | protocol_id = struct.unpack('>H', recv_data[2:4])[0] 59 | unit_id = recv_data[6] 60 | func_code = recv_data[7] 61 | if transaction_id != send_transaction_id: 62 | logger.warning('Receive a reply with a mismatched transaction id (S: {}, R: {}), discard it and continue waiting.'.format(send_transaction_id, transaction_id)) 63 | length = 0 64 | recv_data = b'' 65 | continue 66 | elif protocol_id != self._protocol_id: 67 | logger.warning('Receive a reply with a mismatched protocol id (S: {}, R: {}), discard it and continue waiting.'.format(self._protocol_id, protocol_id)) 68 | length = 0 69 | recv_data = b'' 70 | continue 71 | elif unit_id != send_unit_id: 72 | logger.warning('Receive a reply with a mismatched unit id (S: {}, R: {}), discard it and continue waiting.'.format(send_unit_id, unit_id)) 73 | length = 0 74 | recv_data = b'' 75 | continue 76 | elif func_code != send_func_code and func_code != send_func_code + 0x80: 77 | logger.warning('Receive a reply with a mismatched func code (S: {}, R: {}), discard it and continue waiting.'.format(send_func_code, func_code)) 78 | length = 0 79 | recv_data = b'' 80 | continue 81 | else: 82 | code = 0 83 | break 84 | if code == 0 and len(recv_data) == 9: 85 | logger.error('modbus tcp data exception, exp={}, res={}'.format(recv_data[8], recv_data)) 86 | return recv_data[8], recv_data 87 | elif code != 0: 88 | logger.error('recv timeout, len={}, res={}'.format(len(recv_data), recv_data)) 89 | return code, recv_data 90 | 91 | def __pack_to_send(self, pdu_data, unit_id=None): 92 | self._transaction_id = self._transaction_id % 65535 + 1 93 | unit_id = unit_id if unit_id is not None else self._unit_id 94 | data = struct.pack('>HHHB', self._transaction_id, self._protocol_id, len(pdu_data) + 1, unit_id) 95 | data += pdu_data 96 | self.sock.send(data) 97 | 98 | def __request(self, pdu, unit_id=None): 99 | with self._lock: 100 | self._func_code = pdu[0] 101 | self.__pack_to_send(pdu) 102 | return self.__wait_to_response(unit_id=unit_id, func_code=pdu[0]) 103 | 104 | def __read_bits(self, addr, quantity, func_code=0x01): 105 | assert func_code == 0x01 or func_code == 0x02 106 | pdu = struct.pack('>BHH', func_code, addr, quantity) 107 | code, res_data = self.__request(pdu) 108 | if code == 0 and len(res_data) == 9 + (quantity + 7) // 8: 109 | return code, [(res_data[9 + i // 8] >> (i % 8) & 0x01) for i in range(quantity)] 110 | else: 111 | return code, res_data 112 | 113 | def __read_registers(self, addr, quantity, func_code=0x03, signed=False): 114 | assert func_code == 0x03 or func_code == 0x04 115 | pdu = struct.pack('>BHH', func_code, addr, quantity) 116 | code, res_data = self.__request(pdu) 117 | if code == 0 and len(res_data) == 9 + quantity * 2: 118 | return 0, list(struct.unpack('>{}{}'.format(quantity, 'h' if signed else 'H'), res_data[9:])) 119 | else: 120 | return code, res_data 121 | 122 | def read_coil_bits(self, addr, quantity): 123 | """ 124 | func_code: 0x01 125 | """ 126 | return self.__read_bits(addr, quantity, func_code=0x01) 127 | 128 | def read_input_bits(self, addr, quantity): 129 | """ 130 | func_code: 0x02 131 | """ 132 | return self.__read_bits(addr, quantity, func_code=0x02) 133 | 134 | def read_holding_registers(self, addr, quantity, signed=False): 135 | """ 136 | func_code: 0x03 137 | """ 138 | return self.__read_registers(addr, quantity, func_code=0x03, signed=signed) 139 | 140 | def read_input_registers(self, addr, quantity, signed=False): 141 | """ 142 | func_code: 0x04 143 | """ 144 | return self.__read_registers(addr, quantity, func_code=0x04, signed=signed) 145 | 146 | def write_single_coil_bit(self, addr, on): 147 | """ 148 | func_code: 0x05 149 | """ 150 | pdu = struct.pack('>BHH', 0x05, addr, 0xFF00 if on else 0x0000) 151 | return self.__request(pdu)[0] 152 | 153 | def write_single_holding_register(self, addr, reg_val): 154 | """ 155 | func_code: 0x06 156 | """ 157 | pdu = struct.pack('>BHH', 0x06, addr, reg_val) 158 | return self.__request(pdu)[0] 159 | 160 | def write_multiple_coil_bits(self, addr, bits): 161 | """ 162 | func_code: 0x0F 163 | """ 164 | datas = [0] * ((len(bits) + 7) // 8) 165 | for i in range(len(bits)): 166 | if bits[i]: 167 | datas[i // 8] |= (1 << (i % 8)) 168 | pdu = struct.pack('>BHHB{}B'.format(len(datas)), 0x0F, addr, len(bits), len(datas), *datas) 169 | return self.__request(pdu)[0] 170 | 171 | def write_multiple_holding_registers(self, addr, regs): 172 | """ 173 | func_code: 0x10 174 | """ 175 | pdu = struct.pack('>BHHB{}H'.format(len(regs)), 0x10, addr, len(regs), len(regs) * 2, *regs) 176 | return self.__request(pdu)[0] 177 | 178 | def mask_write_holding_register(self, addr, and_mask, or_mask): 179 | """ 180 | func_code: 0x16 181 | """ 182 | pdu = struct.pack('>BHHH', 0x16, addr, and_mask, or_mask) 183 | return self.__request(pdu)[0] 184 | 185 | def write_and_read_holding_registers(self, r_addr, r_quantity, w_addr, w_regs, r_signed=False, w_signed=False): 186 | """ 187 | func_code: 0x17 188 | """ 189 | pdu = struct.pack('>BHHHHB{}{}'.format(len(w_regs), 'h' if w_signed else 'H'), 0x17, r_addr, r_quantity, w_addr, len(w_regs), len(w_regs) * 2, *w_regs) 190 | code, res_data = self.__request(pdu) 191 | if code == 0 and len(res_data) == 9 + r_quantity * 2: 192 | return 0, struct.unpack('>{}{}'.format(r_quantity, 'h' if r_signed else 'H'), res_data[9:]) 193 | else: 194 | return code, res_data -------------------------------------------------------------------------------- /xarm/tools/threads.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFactory, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | class ThreadManage(object): 11 | def __init__(self): 12 | self.threads = [] 13 | 14 | def append(self, thread): 15 | self.threads.append(thread) 16 | 17 | def remove(self, thread): 18 | if thread in self.threads: 19 | self.threads.remove(thread) 20 | 21 | def join(self, timeout=None): 22 | for t in self.threads: 23 | try: 24 | t.join(timeout=timeout) 25 | except: 26 | pass 27 | self.threads.clear() 28 | 29 | def count(self): 30 | return len(self.threads) 31 | -------------------------------------------------------------------------------- /xarm/tools/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2020, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | import math 9 | import time 10 | import sys 11 | import traceback 12 | 13 | 14 | def pprint(*args, **kwargs): 15 | try: 16 | stack_tuple = traceback.extract_stack()[0] 17 | # filename = stack_tuple[0] 18 | linenumber = stack_tuple[1] 19 | # funcname = stack_tuple[2] 20 | print('[{}][line:{}]'.format( 21 | time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time())), 22 | linenumber 23 | ), end=' ') 24 | except: 25 | pass 26 | print(*args, **kwargs) 27 | 28 | 29 | def is_prime(n): 30 | def _is_prime(): 31 | for i in range(6, int(math.sqrt(n) + 1), 6): 32 | if n % (i - 1) == 0 or n % (i + 1) == 0: 33 | return False 34 | return True 35 | return n == 2 or n == 3 or (n > 1 and n % 2 != 0 and n % 3 != 0 and _is_prime()) 36 | 37 | -------------------------------------------------------------------------------- /xarm/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '1.14.7' 2 | -------------------------------------------------------------------------------- /xarm/wrapper/__init__.py: -------------------------------------------------------------------------------- 1 | from .xarm_api import XArmAPI 2 | -------------------------------------------------------------------------------- /xarm/wrapper/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/wrapper/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/wrapper/__pycache__/xarm_api.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/wrapper/__pycache__/xarm_api.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/wrapper/studio_api.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2021, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import functools 10 | from ..x3.studio import Studio 11 | from .xarm_api import XArmAPI 12 | 13 | 14 | class XArmStudioAPI(Studio): 15 | def __init__(self, ip, ignore_warnning=False): 16 | super(XArmStudioAPI, self).__init__(ip, ignore_warnning=ignore_warnning) 17 | self.arm = self.__RemoteXArmAPI(self.call_sdk_api) 18 | 19 | class __RemoteXArmAPI(XArmAPI): 20 | def __init__(self, call_sdk_func, **kwargs): 21 | XArmAPI.__init__(self, do_not_open=True) 22 | self._arm = self.__RemoteXArm(call_sdk_func, self._arm) 23 | 24 | class __RemoteXArm: 25 | def __init__(self, call_sdk_func, _arm): 26 | self.__call_sdk_func = call_sdk_func 27 | self.__arm = _arm 28 | 29 | def __getattr__(self, item): 30 | if item.startswith(('register', 'release', 'arm_cmd')): 31 | raise Exception('Cannot remotely call interfaces that cannot serialize parameters or results') 32 | attr = getattr(self.__arm, item) 33 | remote_api = functools.partial(self.__call_sdk_func, api_name=item) 34 | return remote_api if callable(attr) else remote_api() 35 | -------------------------------------------------------------------------------- /xarm/x3/__init__.py: -------------------------------------------------------------------------------- 1 | from .xarm import XArm 2 | from .studio import Studio 3 | -------------------------------------------------------------------------------- /xarm/x3/__pycache__/__init__.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/__init__.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/base.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/base.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/base_board.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/base_board.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/code.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/code.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/decorator.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/decorator.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/events.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/events.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/ft_sensor.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/ft_sensor.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/gpio.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/gpio.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/grammar_async.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/grammar_async.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/gripper.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/gripper.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/modbus_tcp.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/modbus_tcp.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/parse.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/parse.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/record.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/record.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/robotiq.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/robotiq.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/servo.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/servo.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/studio.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/studio.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/track.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/track.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/utils.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/utils.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/__pycache__/xarm.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm/x3/__pycache__/xarm.cpython-311.pyc -------------------------------------------------------------------------------- /xarm/x3/base_board.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # __author: rock 3 | # @time: 2021-04-02 4 | 5 | import math 6 | import time 7 | from ..core.utils.log import logger 8 | from .base import Base 9 | from .decorator import xarm_is_connected 10 | 11 | 12 | class BaseBoard(Base): 13 | 14 | def __init__(self): 15 | super(BaseBoard, self).__init__() 16 | 17 | @xarm_is_connected(_type='get') 18 | def get_base_board_version(self, board_id=10): 19 | versions = ['*', '*', '*'] 20 | 21 | ret1 = self.arm_cmd.tgpio_addr_r16(0x0801, bid=board_id) 22 | ret2 = self.arm_cmd.tgpio_addr_r16(0x0802, bid=board_id) 23 | ret3 = self.arm_cmd.tgpio_addr_r16(0x0803, bid=board_id) 24 | 25 | code = 0 26 | if ret1[0] == 0 and len(ret1) == 2: 27 | versions[0] = ret1[1] 28 | else: 29 | code = ret1[0] 30 | if ret2[0] == 0 and len(ret2) == 2: 31 | versions[1] = ret2[1] 32 | else: 33 | code = ret2[0] 34 | if ret3[0] == 0 and len(ret3) == 2: 35 | versions[2] = ret3[1] 36 | else: 37 | code = ret3[0] 38 | 39 | return code, '.'.join(map(str, versions)) 40 | 41 | @xarm_is_connected(_type='get') 42 | def get_current_angle(self, board_id=10): 43 | code, acc_xyz = self.get_imu_data(board_id) 44 | self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=1, bid=board_id) 45 | if code == 0 and acc_xyz[0] != 0 and acc_xyz[1] != 0 and acc_xyz[2] != 0: 46 | angle = self.__get_z_axios_offset_angle(acc_xyz[0], acc_xyz[1], acc_xyz[2]) 47 | return code, angle 48 | else: 49 | return code, 0 50 | 51 | @staticmethod 52 | def __get_z_axios_offset_angle(x=1, y=1, z=1): 53 | angle = math.degrees(math.atan(z / (math.sqrt(abs((x ** 2) + (y ** 2)))))) 54 | angle = 90 - angle 55 | return angle 56 | 57 | @xarm_is_connected(_type='set') 58 | def write_sn(self, sn='', servo_id=10): 59 | code = 0 60 | if len(sn) == 14: 61 | for i in range(0, 14, 2): 62 | ret = self.arm_cmd.tgpio_addr_w16(addr=0x1900 + (int(i / 2)), value=ord(sn[i]) | ord(sn[i + 1]) << 8, bid=servo_id) 63 | code = self._check_code(ret[0]) 64 | time.sleep(0.1) 65 | if code != 0: 66 | self.log_api_info('API -> write_sn -> code={}, sn={}'.format(code, sn), code=code) 67 | return code 68 | self.log_api_info('API -> write_sn -> code={}, sn={}'.format(code, sn), code=code) 69 | return code 70 | 71 | @xarm_is_connected(_type='get') 72 | def get_sn(self, servo_id=10): 73 | rd_sn = '' 74 | ret = [0, ''] 75 | for i in range(0, 14, 2): 76 | ret = self.arm_cmd.tgpio_addr_r16(addr=0x0900 + (int(i / 2)), bid=servo_id) 77 | time.sleep(0.1) 78 | rd_sn = ''.join([rd_sn, chr(ret[1] & 0x00FF)]) 79 | rd_sn = ''.join([rd_sn, chr((ret[1] >> 8) & 0x00FF)]) 80 | ret[0] = self._check_code(ret[0]) 81 | if ret[0] != 0: 82 | self.log_api_info('API -> get_sn -> code={}, sn={}'.format(ret[0], rd_sn), code=ret[0]) 83 | return ret[0], '' 84 | self.log_api_info('API -> get_sn -> code={}, sn={}'.format(ret[0], rd_sn), code=ret[0]) 85 | return ret[0], rd_sn 86 | 87 | @xarm_is_connected(_type='set') 88 | def write_iden_to_base(self, idens, servo_id=10): 89 | cmds = [0x0D00, 0x0D0C, 0x0D18, 0x0D24, 0x0D30, 0x0D3C, 0x0D48] 90 | code = 0 91 | if idens: 92 | for i, data in enumerate(idens): 93 | # print(i, data) 94 | for j, d in enumerate(data): 95 | ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds[i] + (2 * j)) | 0x1000, value=d, bid=servo_id) 96 | # print("%x, %f, ret:%d" % (cmds[i] + (2 * j), d, ret[0])) 97 | 98 | time.sleep(0.1) 99 | code = ret[0] 100 | if code != 0: 101 | return code 102 | return code 103 | else: 104 | return 1 105 | 106 | @xarm_is_connected(_type='get') 107 | def get_imu_data(self, board_id=10): 108 | code = 0 109 | if board_id == 9: 110 | self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=1, bid=board_id) 111 | 112 | ret1 = self.arm_cmd.tgpio_addr_r32(addr=0x0C00, bid=board_id, fmt='>f') 113 | ret2 = self.arm_cmd.tgpio_addr_r32(addr=0x0C02, bid=board_id, fmt='>f') 114 | ret3 = self.arm_cmd.tgpio_addr_r32(addr=0x0C04, bid=board_id, fmt='>f') 115 | code = 0 if ret1[0] == 0 else ret1[0] 116 | code = code if ret2[0] == 0 else ret2[0] 117 | code = code if ret3[0] == 0 else ret3[0] 118 | 119 | if board_id == 9: 120 | self.arm_cmd.tgpio_addr_w16(addr=0x0606, value=0, bid=board_id) 121 | if code != 0: 122 | return code, [1, 1, 1] 123 | else: 124 | return code, [ret1[1], ret2[1], ret3[1]] 125 | 126 | @xarm_is_connected(_type='get') 127 | def read_iden_from_base(self, servo_id=10): 128 | cmds = [0x0D00, 0x0D0C, 0x0D18, 0x0D24, 0x0D30, 0x0D3C, 0x0D48] 129 | code = 0 130 | conf = [] 131 | for i in range(7): 132 | vl = [] 133 | for j in range(6): 134 | ret = self.arm_cmd.tgpio_addr_r32((cmds[i] + (2 * j)), servo_id, fmt='>f') 135 | time.sleep(0.01) 136 | # print("%x, %f, ret:%d" % (cmds[i] + (2 * j), ret[1], ret[0])) 137 | vl.append(ret[1]) 138 | code = ret[0] 139 | if code != 0: 140 | return code, None 141 | conf.append(vl) 142 | return code, conf 143 | 144 | @xarm_is_connected(_type='set') 145 | def write_poe_to_end(self, datas, servo_id=9): 146 | cmds1 = [0x0E00, 0x0E0C, 0x0E18, 0x0E24, 0x0E30, 0x0E3C, 0x0E48] 147 | cmds2 = 0x0F00 148 | code = 0 149 | if datas: 150 | joints, homes = datas 151 | for i, data in enumerate(joints): 152 | for j, d in enumerate(data): 153 | ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds1[i] + (2 * j)) | 0x1000, value=d, bid=servo_id) 154 | time.sleep(0.1) 155 | code = ret[0] 156 | if code != 0: 157 | return code 158 | for i, data in enumerate(homes): 159 | for j, d in enumerate(data): 160 | ret = self.arm_cmd.tgpio_addr_w32(addr=(cmds2 + (2 * (i * len(data) + j))) | 0x1000, value=d, bid=servo_id) 161 | time.sleep(0.1) 162 | code = ret[0] 163 | if code != 0: 164 | return code 165 | return code 166 | else: 167 | return 1 168 | 169 | @xarm_is_connected(_type='get') 170 | def read_poe_from_end(self, servo_id=9): 171 | cmds = [0x0E00, 0x0E0C, 0x0E18, 0x0E24, 0x0E30, 0x0E3C, 0x0E48] 172 | cmds2 = 0x0F00 173 | code = 0 174 | conf = [] 175 | joints_conf = [] 176 | homes_conf = [] 177 | for i in range(7): 178 | vl = [] 179 | for j in range(6): 180 | ret = self.arm_cmd.tgpio_addr_r32(cmds[i] + (2 * j), servo_id, fmt='>f') 181 | time.sleep(0.01) 182 | vl.append(ret[1]) 183 | code = ret[0] 184 | if code != 0: 185 | return code, None 186 | joints_conf.append(vl) 187 | for i in range(4): 188 | vl = [] 189 | for j in range(4): 190 | ret = self.arm_cmd.tgpio_addr_r32(cmds2 + 2 * (i * 4 + j), servo_id, fmt='>f') 191 | time.sleep(0.01) 192 | vl.append(ret[1]) 193 | code = ret[0] 194 | if code != 0: 195 | return code, None 196 | homes_conf.append(vl) 197 | conf = [joints_conf, homes_conf] 198 | return code, conf 199 | -------------------------------------------------------------------------------- /xarm/x3/code.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | from ..core.config.x_config import XCONF 11 | 12 | 13 | class APIState(object): 14 | NOT_CONNECTED = -1 # 已断开或未连接 15 | NOT_READY = -2 # 未使能或者设置状态 16 | API_EXCEPTION = -3 # 接口异常，可能是参数错误 17 | CMD_NOT_EXIST = -4 # 命令不存在 18 | TCP_LIMIT = -6 # 笛卡尔限位 19 | JOINT_LIMIT = -7 # 关节角度限位 20 | OUT_OF_RANGE = -8 # 超出范围 21 | EMERGENCY_STOP = -9 # 紧急停止 22 | SERVO_NOT_EXIST = -10 # 不存在此ID的关节 23 | CONVERT_FAILED = -11 # 转换Blockly失败 24 | RUN_BLOCKLY_EXCEPTION = -12 # 运行blockly app异常 25 | NORMAL = 0 # 正常 26 | HAS_ERROR = XCONF.UxbusState.ERR_CODE # 有尚未清除的错误 27 | HAS_WARN = XCONF.UxbusState.WAR_CODE # 有尚未清除的警告 28 | RES_TIMEOUT = XCONF.UxbusState.ERR_TOUT # 命令回复超时 29 | RES_LENGTH_ERROR = XCONF.UxbusState.ERR_LENG # TCP长度错误 30 | CMD_NUM_ERROR = XCONF.UxbusState.ERR_NUM # TCP序号错误 31 | CMD_PROT_ERROR = XCONF.UxbusState.ERR_PROT # TCP协议标志错误 32 | FUN_ERROR = XCONF.UxbusState.ERR_FUN # TCP回复指令和发送指令不匹配 33 | NO_TCP = XCONF.UxbusState.ERR_NOTTCP # 写数据异常 34 | STATE_NOT_READY = XCONF.UxbusState.STATE_NOT_READY # 参数错误 35 | RET_IS_INVALID = XCONF.UxbusState.INVALID # 结果无效 36 | OTHER = XCONF.UxbusState.ERR_OTHER # 其它错误 37 | PARAM_ERROR = XCONF.UxbusState.ERR_PARAM # 参数错误 38 | 39 | HOST_ID_ERR = 20 # 主机ID错误, 看使用的接口，可能是末端IO也可能是滑轨 40 | MODBUS_BAUD_NOT_SUPPORT = 21 # modbus不支持此波特率 41 | MODBUS_BAUD_NOT_CORRECT = 22 # 末端modbus波特率不正确 42 | MODBUS_ERR_LENG = 23 # modbus回复数据长度错误 43 | 44 | TRAJ_RW_FAILED = 31 # 读写轨迹失败(加载轨迹或保存轨迹) 45 | TRAJ_RW_TOUT = 32 # 读写轨迹等待超时(加载轨迹或保存轨迹) 46 | TRAJ_PLAYBACK_TOUT = 33 # 回放轨迹超时(多种情况) 47 | TRAJ_PLAYBACK_FAILED = 34 # 回放轨迹失败(多种情况) 48 | SUCTION_CUP_TOUT = 41 # 等待吸泵设置超时 49 | 50 | MODE_IS_NOT_CORRECT = 51 # 模式不正确 51 | 52 | LINEAR_TRACK_HAS_FAULT = 80 # 滑轨有错误 53 | LINEAR_TRACK_SCI_IS_LOW = 81 # 滑轨的SCI被置低了 54 | LINEAR_TRACK_NOT_INIT = 82 # 直线滑轨未初始化 55 | 56 | WAIT_FINISH_TIMEOUT = 100 # 等待操作完成超时 57 | CHECK_FAILED = 101 # 等待操作完成过程检测状态连续失败次数过多 58 | END_EFFECTOR_HAS_FAULT = 102 # 末端配件有错误 59 | END_EFFECTOR_NOT_ENABLED = 103 # 末端配件未使能 60 | 61 | # 129 ~ 144: 标准modbus tcp的异常码，实际异常码(api_code - 0x80) 62 | 63 | -------------------------------------------------------------------------------- /xarm/x3/decorator.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2021, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | 10 | import time 11 | import math 12 | import functools 13 | from ..core.utils.log import logger 14 | from .code import APIState 15 | from ..core.config.x_config import XCONF 16 | 17 | 18 | def check_modbus_baud(baud=2000000, _type='set', default=None, host_id=XCONF.TGPIO_HOST_ID): 19 | def _check_modbus_baud(func): 20 | @functools.wraps(func) 21 | def decorator(self, *args, **kwargs): 22 | code = self.checkset_modbus_baud(baud, host_id=host_id) 23 | if code != 0: 24 | logger.error('check modbus baud is failed, code={}'.format(code)) 25 | return code if _type == 'set' else (code, default if default != -99 else []) 26 | else: 27 | return func(self, *args, **kwargs) 28 | return decorator 29 | return _check_modbus_baud 30 | 31 | 32 | def xarm_is_connected(_type='set'): 33 | def _xarm_is_connected(func): 34 | @functools.wraps(func) 35 | def decorator(self, *args, **kwargs): 36 | if self.connected: 37 | return func(self, *args, **kwargs) 38 | else: 39 | logger.error('xArm is not connected') 40 | return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect') 41 | return decorator 42 | return _xarm_is_connected 43 | 44 | 45 | def xarm_is_ready(_type='set'): 46 | def _xarm_is_ready(func): 47 | @functools.wraps(func) 48 | def decorator(self, *args, **kwargs): 49 | if self.connected and kwargs.get('auto_enable', False): 50 | if not self.ready: 51 | self.motion_enable(enable=True) 52 | self.set_mode(0) 53 | self.set_state(0) 54 | if self.connected: 55 | if self.check_xarm_is_ready: 56 | return func(self, *args, **kwargs) 57 | else: 58 | logger.error('xArm is not ready') 59 | logger.info('Please check the arm for errors. If so, please clear the error first. ' 60 | 'Then enable the motor, set the mode and set the state') 61 | return APIState.NOT_READY if _type == 'set' else (APIState.NOT_READY, 'xArm is not ready') 62 | else: 63 | logger.error('xArm is not connected') 64 | return APIState.NOT_CONNECTED if _type == 'set' else (APIState.NOT_CONNECTED, 'xArm is not connect') 65 | return decorator 66 | return _xarm_is_ready 67 | 68 | 69 | def xarm_wait_until_not_pause(func): 70 | @functools.wraps(func) 71 | def decorator(self, *args, **kwargs): 72 | self.wait_until_not_pause() 73 | return func(self, *args, **kwargs) 74 | return decorator 75 | 76 | 77 | def xarm_wait_until_cmdnum_lt_max(func): 78 | @functools.wraps(func) 79 | def decorator(self, *args, **kwargs): 80 | self.wait_until_cmdnum_lt_max() 81 | return func(self, *args, **kwargs) 82 | return decorator 83 | 84 | 85 | def xarm_is_not_simulation_mode(ret=0): 86 | def _xarm_is_not_simulation_mode(func): 87 | @functools.wraps(func) 88 | def decorator(self, *args, **kwargs): 89 | if not self.check_is_simulation_robot(): 90 | return func(self, *args, **kwargs) 91 | else: 92 | return ret 93 | return decorator 94 | return _xarm_is_not_simulation_mode 95 | 96 | 97 | def api_log(func): 98 | @functools.wraps(func) 99 | def decorator(self, *args, **kwargs): 100 | ret = func(self, *args, **kwargs) 101 | logger.info('{}, ret={}, args={}, kwargs={}'.format(func.__name__, ret, args[1:], kwargs)) 102 | return ret 103 | return decorator 104 | 105 | -------------------------------------------------------------------------------- /xarm/x3/events.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | REPORT_ID = 'REPORT' 10 | REPORT_LOCATION_ID = 'LOCATION' 11 | REPORT_CONNECT_CHANGED_ID = 'REPORT_CONNECT_CHANGED' 12 | REPORT_STATE_CHANGED_ID = 'REPORT_STATE_CHANGED' 13 | REPORT_MODE_CHANGED_ID = 'REPORT_MODE_CHANGED' 14 | REPORT_MTABLE_MTBRAKE_CHANGED_ID = 'REPORT_MTABLE_MTBRAKE_CHANGED' 15 | REPORT_ERROR_WARN_CHANGED_ID = 'REPORT_ERROR_WARN_CHANGED' 16 | REPORT_CMDNUM_CHANGED_ID = 'REPORT_CMDNUM_CHANGED' 17 | REPORT_TEMPERATURE_CHANGED_ID = 'REPORT_TEMPERATURE_CHANGED' 18 | REPORT_COUNT_CHANGED_ID = 'REPORT_COUNT_CHANGED' 19 | REPORT_IDEN_PROGRESS_CHANGED_ID = 'REPORT_IDEN_PROGRESS_CHANGED_ID' 20 | FEEDBACK_ID = 'FEEDBACK_ID' 21 | 22 | 23 | class Events(object): 24 | REPORT_ID = REPORT_ID 25 | REPORT_LOCATION_ID = REPORT_LOCATION_ID 26 | REPORT_CONNECT_CHANGED_ID = REPORT_CONNECT_CHANGED_ID 27 | REPORT_STATE_CHANGED_ID = REPORT_STATE_CHANGED_ID 28 | REPORT_MODE_CHANGED_ID = REPORT_MODE_CHANGED_ID 29 | REPORT_MTABLE_MTBRAKE_CHANGED_ID = REPORT_MTABLE_MTBRAKE_CHANGED_ID 30 | REPORT_ERROR_WARN_CHANGED_ID = REPORT_ERROR_WARN_CHANGED_ID 31 | REPORT_CMDNUM_CHANGED_ID = REPORT_CMDNUM_CHANGED_ID 32 | REPORT_TEMPERATURE_CHANGED_ID = REPORT_TEMPERATURE_CHANGED_ID 33 | REPORT_COUNT_CHANGED_ID = REPORT_COUNT_CHANGED_ID 34 | REPORT_IDEN_PROGRESS_CHANGED_ID = REPORT_IDEN_PROGRESS_CHANGED_ID 35 | FEEDBACK_ID = FEEDBACK_ID 36 | 37 | def __init__(self): 38 | self._report_callbacks = { 39 | REPORT_ID: [], 40 | REPORT_LOCATION_ID: [], 41 | REPORT_CONNECT_CHANGED_ID: [], 42 | REPORT_ERROR_WARN_CHANGED_ID: [], 43 | REPORT_STATE_CHANGED_ID: [], 44 | REPORT_MODE_CHANGED_ID: [], 45 | REPORT_MTABLE_MTBRAKE_CHANGED_ID: [], 46 | REPORT_CMDNUM_CHANGED_ID: [], 47 | REPORT_COUNT_CHANGED_ID: [], 48 | REPORT_IDEN_PROGRESS_CHANGED_ID: [], 49 | FEEDBACK_ID: [] 50 | } 51 | 52 | def _register_report_callback(self, report_id, callback): 53 | if report_id not in self._report_callbacks.keys(): 54 | self._report_callbacks[report_id] = [] 55 | if (callable(callback) or isinstance(callback, dict)) and callback not in self._report_callbacks[report_id]: 56 | self._report_callbacks[report_id].append(callback) 57 | return True 58 | elif not (callable(callback) or isinstance(callback, dict)): 59 | return False 60 | else: 61 | return True 62 | 63 | def _release_report_callback(self, report_id, callback): 64 | if report_id in self._report_callbacks.keys(): 65 | if callback is None: 66 | self._report_callbacks[report_id].clear() 67 | return True 68 | elif callback: 69 | for cb in self._report_callbacks[report_id]: 70 | if callback == cb: 71 | self._report_callbacks[report_id].remove(callback) 72 | return True 73 | elif isinstance(cb, dict): 74 | if cb['callback'] == callback: 75 | self._report_callbacks[report_id].remove(cb) 76 | return True 77 | return False 78 | 79 | def register_report_callback(self, callback=None, report_cartesian=True, report_joints=True, 80 | report_state=True, report_error_code=True, report_warn_code=True, 81 | report_mtable=True, report_mtbrake=True, report_cmd_num=True): 82 | return self._register_report_callback(REPORT_ID, { 83 | 'callback': callback, 84 | 'cartesian': report_cartesian, 85 | 'joints': report_joints, 86 | 'error_code': report_error_code, 87 | 'warn_code': report_warn_code, 88 | 'state': report_state, 89 | 'mtable': report_mtable, 90 | 'mtbrake': report_mtbrake, 91 | 'cmdnum': report_cmd_num 92 | }) 93 | 94 | def register_report_location_callback(self, callback=None, report_cartesian=True, report_joints=False): 95 | ret = self._register_report_callback(REPORT_LOCATION_ID, { 96 | 'callback': callback, 97 | 'cartesian': report_cartesian, 98 | 'joints': report_joints, 99 | }) 100 | return ret 101 | 102 | def register_connect_changed_callback(self, callback=None): 103 | return self._register_report_callback(REPORT_CONNECT_CHANGED_ID, callback) 104 | 105 | def register_state_changed_callback(self, callback=None): 106 | return self._register_report_callback(REPORT_STATE_CHANGED_ID, callback) 107 | 108 | def register_mode_changed_callback(self, callback=None): 109 | return self._register_report_callback(REPORT_MODE_CHANGED_ID, callback) 110 | 111 | def register_mtable_mtbrake_changed_callback(self, callback=None): 112 | return self._register_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback) 113 | 114 | def register_error_warn_changed_callback(self, callback=None): 115 | return self._register_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback) 116 | 117 | def register_cmdnum_changed_callback(self, callback=None): 118 | return self._register_report_callback(REPORT_CMDNUM_CHANGED_ID, callback) 119 | 120 | def register_temperature_changed_callback(self, callback=None): 121 | return self._register_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback) 122 | 123 | def register_count_changed_callback(self, callback=None): 124 | return self._register_report_callback(REPORT_COUNT_CHANGED_ID, callback) 125 | 126 | def register_iden_progress_changed_callback(self, callback=None): 127 | return self._register_report_callback(REPORT_IDEN_PROGRESS_CHANGED_ID, callback) 128 | 129 | def register_feedback_callback(self, callback=None): 130 | return self._register_report_callback(FEEDBACK_ID, callback) 131 | 132 | def release_report_callback(self, callback=None): 133 | return self._release_report_callback(REPORT_ID, callback) 134 | 135 | def release_report_location_callback(self, callback=None): 136 | return self._release_report_callback(REPORT_LOCATION_ID, callback) 137 | 138 | def release_connect_changed_callback(self, callback=None): 139 | return self._release_report_callback(REPORT_CONNECT_CHANGED_ID, callback) 140 | 141 | def release_state_changed_callback(self, callback=None): 142 | return self._release_report_callback(REPORT_STATE_CHANGED_ID, callback) 143 | 144 | def release_mode_changed_callback(self, callback=None): 145 | return self._release_report_callback(REPORT_MODE_CHANGED_ID, callback) 146 | 147 | def release_mtable_mtbrake_changed_callback(self, callback=None): 148 | return self._release_report_callback(REPORT_MTABLE_MTBRAKE_CHANGED_ID, callback) 149 | 150 | def release_error_warn_changed_callback(self, callback=None): 151 | return self._release_report_callback(REPORT_ERROR_WARN_CHANGED_ID, callback) 152 | 153 | def release_cmdnum_changed_callback(self, callback=None): 154 | return self._release_report_callback(REPORT_CMDNUM_CHANGED_ID, callback) 155 | 156 | def release_temperature_changed_callback(self, callback=None): 157 | return self._release_report_callback(REPORT_TEMPERATURE_CHANGED_ID, callback) 158 | 159 | def release_count_changed_callback(self, callback=None): 160 | return self._release_report_callback(REPORT_COUNT_CHANGED_ID, callback) 161 | 162 | def release_iden_progress_changed_callback(self, callback=None): 163 | return self._release_report_callback(REPORT_IDEN_PROGRESS_CHANGED_ID, callback) 164 | 165 | def release_feedback_callback(self, callback=None): 166 | return self._release_report_callback(FEEDBACK_ID, callback) 167 | -------------------------------------------------------------------------------- /xarm/x3/ft_sensor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2021, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | import time 9 | from ..core.utils.log import logger 10 | from ..core.utils import convert 11 | from .base import Base 12 | from .code import APIState 13 | from .decorator import xarm_is_connected 14 | 15 | 16 | class FtSensor(Base): 17 | def __init__(self): 18 | super(FtSensor, self).__init__() 19 | 20 | @xarm_is_connected(_type='set') 21 | def set_impedance(self, coord, c_axis, M, K, B, **kwargs): 22 | if len(c_axis) < 6 or len(M) < 6 or len(K) < 6 or len(B) < 6: 23 | logger.error('set_impedance: parameters error') 24 | return APIState.API_EXCEPTION 25 | params_limit = kwargs.get('params_limit', True) 26 | if params_limit: 27 | for i in range(6): 28 | if i < 3: 29 | if M[i] < 0.02 or M[i] > 1.0: 30 | logger.error('set_impedance, M[{}] over range, range=[0.02, 1.0]'.format(i)) 31 | return APIState.API_EXCEPTION 32 | if K[i] < 0 or K[i] > 2000: 33 | logger.error('set_impedance, K[{}] over range, range=[0, 2000]'.format(i)) 34 | return APIState.API_EXCEPTION 35 | else: 36 | if M[i] < 0.0001 or M[i] > 0.01: 37 | logger.error('set_impedance, M[{}] over range, range=[0.0001, 0.01]'.format(i)) 38 | return APIState.API_EXCEPTION 39 | if K[i] < 0 or K[i] > 20: 40 | logger.error('set_impedance, K[{}] over range, range=[0, 20]'.format(i)) 41 | return APIState.API_EXCEPTION 42 | if B[i] < 0: 43 | logger.error('set_impedance, the value of B[{}] must be greater than or equal to 0'.format(i)) 44 | return APIState.API_EXCEPTION 45 | ret = self.arm_cmd.set_impedance(coord, c_axis, M, K, B) 46 | self.log_api_info('API -> set_impedance -> code={}'.format(ret[0]), code=ret[0]) 47 | return self._check_code(ret[0]) 48 | 49 | @xarm_is_connected(_type='set') 50 | def set_impedance_mbk(self, M, K, B, **kwargs): 51 | if len(M) < 6 or len(K) < 6 or len(B) < 6: 52 | logger.error('set_impedance_mbk: parameters error') 53 | return APIState.API_EXCEPTION 54 | params_limit = kwargs.get('params_limit', True) 55 | if params_limit: 56 | for i in range(6): 57 | if i < 3: 58 | if M[i] < 0.02 or M[i] > 1.0: 59 | logger.error('set_impedance_mbk, M[{}] over range, range=[0.02, 1.0]'.format(i)) 60 | return APIState.API_EXCEPTION 61 | if K[i] < 0 or K[i] > 2000: 62 | logger.error('set_impedance_mbk, K[{}] over range, range=[0, 2000]'.format(i)) 63 | return APIState.API_EXCEPTION 64 | else: 65 | if M[i] < 0.0001 or M[i] > 0.01: 66 | logger.error('set_impedance_mbk, M[{}] over range, range=[0.0001, 0.01]'.format(i)) 67 | return APIState.API_EXCEPTION 68 | if K[i] < 0 or K[i] > 20: 69 | logger.error('set_impedance_mbk, K[{}] over range, range=[0, 20]'.format(i)) 70 | return APIState.API_EXCEPTION 71 | if B[i] < 0: 72 | logger.error('set_impedance_mbk, the value of B[{}] must be greater than or equal to 0'.format(i)) 73 | return APIState.API_EXCEPTION 74 | ret = self.arm_cmd.set_impedance_mbk(M, K, B) 75 | self.log_api_info('API -> set_impedance_mbk -> code={}'.format(ret[0]), code=ret[0]) 76 | return self._check_code(ret[0]) 77 | 78 | @xarm_is_connected(_type='set') 79 | def set_impedance_config(self, coord, c_axis): 80 | if len(c_axis) < 6: 81 | logger.error('set_impedance_config: parameters error') 82 | return APIState.API_EXCEPTION 83 | ret = self.arm_cmd.set_impedance_config(coord, c_axis) 84 | self.log_api_info('API -> set_impedance_config -> code={}'.format(ret[0]), code=ret[0]) 85 | return self._check_code(ret[0]) 86 | 87 | @xarm_is_connected(_type='set') 88 | def config_force_control(self, coord, c_axis, f_ref, limits, **kwargs): 89 | if len(c_axis) < 6 or len(f_ref) < 6 or len(limits) < 6: 90 | logger.error('config_force_control: parameters error') 91 | return APIState.API_EXCEPTION 92 | params_limit = kwargs.get('params_limit', True) 93 | if params_limit: 94 | max_f_ref = [150, 150, 200, 4, 4, 4] 95 | for i in range(6): 96 | if f_ref[i] < -max_f_ref[i] or f_ref[i] > max_f_ref[i]: 97 | logger.error('config_force_control, f_ref[{}] over range, range=[{}, {}]'.format(i, -max_f_ref[i], max_f_ref[i])) 98 | return APIState.API_EXCEPTION 99 | ret = self.arm_cmd.config_force_control(coord, c_axis, f_ref, limits) 100 | self.log_api_info('API -> config_force_control -> code={}'.format(ret[0]), code=ret[0]) 101 | return self._check_code(ret[0]) 102 | 103 | @xarm_is_connected(_type='set') 104 | def set_force_control_pid(self, kp, ki, kd, xe_limit, **kwargs): 105 | if len(kp) < 6 or len(ki) < 6 or len(kd) < 6 or len(xe_limit) < 6: 106 | logger.error('set_force_control_pid: parameters error') 107 | return APIState.API_EXCEPTION 108 | params_limit = kwargs.get('params_limit', True) 109 | if params_limit: 110 | for i in range(6): 111 | if kp[i] < 0 or kp[i] > 0.05: 112 | logger.error('set_force_control_pid, kp[{}] over range, range=[0, 0.05]'.format(i)) 113 | return APIState.API_EXCEPTION 114 | if ki[i] < 0 or ki[i] > 0.0005: 115 | logger.error('set_force_control_pid, ki[{}] over range, range=[0, 0.0005]'.format(i)) 116 | return APIState.API_EXCEPTION 117 | if kd[i] < 0 or kd[i] > 0.05: 118 | logger.error('set_force_control_pid, kd[{}] over range, range=[0, 0.05]'.format(i)) 119 | return APIState.API_EXCEPTION 120 | if xe_limit[i] < 0 or xe_limit[i] > 200: 121 | logger.error('set_force_control_pid, xe_limit[{}] over range, range=[0, 200]'.format(i)) 122 | return APIState.API_EXCEPTION 123 | ret = self.arm_cmd.set_force_control_pid(kp, ki, kd, xe_limit) 124 | self.log_api_info('API -> set_force_control_pid -> code={}'.format(ret[0]), code=ret[0]) 125 | return self._check_code(ret[0]) 126 | 127 | @xarm_is_connected(_type='set') 128 | def ft_sensor_set_zero(self): 129 | ret = self.arm_cmd.ft_sensor_set_zero() 130 | self.log_api_info('API -> ft_sensor_set_zero -> code={}'.format(ret[0]), code=ret[0]) 131 | return self._check_code(ret[0]) 132 | 133 | @xarm_is_connected(_type='get') 134 | def ft_sensor_iden_load(self): 135 | protocol_identifier = self.arm_cmd.get_protocol_identifier() 136 | self.arm_cmd.set_protocol_identifier(2) 137 | self._keep_heart = False 138 | ret = self.arm_cmd.ft_sensor_iden_load() 139 | self.arm_cmd.set_protocol_identifier(protocol_identifier) 140 | self._keep_heart = True 141 | self.log_api_info('API -> ft_sensor_iden_load -> code={}'.format(ret[0]), code=ret[0]) 142 | code = self._check_code(ret[0]) 143 | if code == 0 or len(ret) > 5: 144 | ret[2] = ret[2] * 1000 # x_centroid, 从m转成mm 145 | ret[3] = ret[3] * 1000 # y_centroid, 从m转成mm 146 | ret[4] = ret[4] * 1000 # z_centroid, 从m转成mm 147 | return self._check_code(ret[0]), ret[1:11] 148 | 149 | @xarm_is_connected(_type='set') 150 | def ft_sensor_cali_load(self, iden_result_list, association_setting_tcp_load=False, **kwargs): 151 | if len(iden_result_list) < 10: 152 | return APIState.PARAM_ERROR 153 | params = iden_result_list[:] 154 | params[1] = params[1] / 1000.0 # x_centroid, 从mm转成m 155 | params[2] = params[2] / 1000.0 # y_centroid, 从mm转成m 156 | params[3] = params[3] / 1000.0 # z_centroid, 从mm转成m 157 | ret = self.arm_cmd.ft_sensor_cali_load(params) 158 | self.log_api_info('API -> ft_sensor_cali_load -> code={}, iden_result_list={}'.format(ret[0], iden_result_list), code=ret[0]) 159 | ret[0] = self._check_code(ret[0]) 160 | if ret[0] == 0 and association_setting_tcp_load: 161 | m = kwargs.get('m', 0.270) # 0.325 162 | x = kwargs.get('x', -17) 163 | y = kwargs.get('y', 9) 164 | z = kwargs.get('z', 11.8) 165 | weight = params[0] + m 166 | center_of_gravity = [ 167 | (m * x + params[0] * params[1]) / weight, 168 | (m * y + params[0] * params[2]) / weight, 169 | (m * z + params[0] * (32 + params[3])) / weight 170 | ] 171 | self.set_state(0) 172 | return self.set_tcp_load(weight, center_of_gravity) 173 | return ret[0] 174 | 175 | @xarm_is_connected(_type='set') 176 | def ft_sensor_enable(self, on_off): 177 | ret = self.arm_cmd.ft_sensor_enable(on_off) 178 | self.log_api_info('API -> ft_sensor_enable -> code={}, on_off={}'.format(ret[0], on_off), code=ret[0]) 179 | return self._check_code(ret[0]) 180 | 181 | @xarm_is_connected(_type='get') 182 | def ft_sensor_app_set(self, app_code): 183 | ret = self.arm_cmd.ft_sensor_app_set(app_code) 184 | self.log_api_info('API -> ft_sensor_app_set -> code={}, app_code={}'.format(ret[0], app_code), code=ret[0]) 185 | return self._check_code(ret[0]) 186 | 187 | @xarm_is_connected(_type='get') 188 | def ft_sensor_app_get(self): 189 | ret = self.arm_cmd.ft_sensor_app_get() 190 | return self._check_code(ret[0]), ret[1] 191 | 192 | @xarm_is_connected(_type='get') 193 | def get_ft_sensor_data(self): 194 | ret = self.arm_cmd.ft_sensor_get_data(self.version_is_ge(1, 8, 3)) 195 | return self._check_code(ret[0]), ret[1:7] 196 | 197 | @xarm_is_connected(_type='get') 198 | def get_ft_sensor_config(self): 199 | ret = self.arm_cmd.ft_sensor_get_config() 200 | ret[0] = self._check_code(ret[0]) 201 | return ret[0], ret[1:] 202 | 203 | @xarm_is_connected(_type='get') 204 | def get_ft_sensor_error(self): 205 | ret = self.arm_cmd.ft_sensor_get_error() 206 | ret[0] = self._check_code(ret[0]) 207 | return ret[0], ret[1] 208 | 209 | def set_ft_sensor_sn(self, sn): 210 | assert len(sn) >= 14, 'The length of SN is wrong' 211 | ret = [0] 212 | if len(sn) == 14: 213 | for i in range(0, 14): 214 | value = ord(sn[i]) 215 | if i < 8: 216 | ret = self.arm_cmd.servo_addr_w16(8, 0x1300+i, value) 217 | ret[0] = self._check_code(ret[0]) 218 | else: 219 | ret = self.arm_cmd.servo_addr_w16(8, 0x1400+(i-8), value) 220 | ret[0] = self._check_code(ret[0]) 221 | if ret[0] != 0: 222 | break 223 | time.sleep(0.05) 224 | self.log_api_info('API -> set_ft_sensor_sn -> code={}, sn={}'.format(ret[0], sn), code=ret[0]) 225 | return ret[0] 226 | 227 | def get_ft_sensor_sn(self): 228 | rd_sn = '' 229 | ret = [0, ''] 230 | for i in range(0, 14): 231 | if i < 8: 232 | ret = self.arm_cmd.servo_addr_r16(8, 0x0300+i) 233 | ret[0] = self._check_code(ret[0]) 234 | else: 235 | ret = self.arm_cmd.servo_addr_r16(8, 0x0400+(i-8)) 236 | ret[0] = self._check_code(ret[0]) 237 | if i < 2 and ret[-1] not in [65, 73]: 238 | return 1, "********" 239 | 240 | if chr(ret[-1]).isalnum(): 241 | rd_sn = ''.join([rd_sn, chr(ret[-1])]) 242 | else: 243 | rd_sn = ''.join([rd_sn, '*']) 244 | time.sleep(0.05) 245 | self.log_api_info('API -> get_ft_sensor_sn -> code={}, sn={}'.format(ret[0], rd_sn), code=ret[0]) 246 | return ret[0], rd_sn 247 | 248 | def get_ft_sensor_version(self): 249 | versions = ['*', '*', '*'] 250 | ret1 = self.arm_cmd.servo_addr_r16(8, 0x0801) 251 | ret1[0] = self._check_code(ret1[0]) 252 | ret2 = self.arm_cmd.servo_addr_r16(8, 0x0802) 253 | ret2[0] = self._check_code(ret2[0]) 254 | ret3 = self.arm_cmd.servo_addr_r16(8, 0x0803) 255 | ret3[0] = self._check_code(ret3[0]) 256 | 257 | if ret1[0] == 0 and ret1[1] < 10: 258 | versions[0] = ret1[1] 259 | if ret2[0] == 0 and ret1[1] < 100: 260 | versions[1] = ret2[1] 261 | if ret3[0] == 0 and ret1[1] < 1000: 262 | versions[2] = ret3[1] 263 | 264 | return ret1[0] or ret2[0] or ret3[0], '.'.join(map(str, versions)) 265 | -------------------------------------------------------------------------------- /xarm/x3/grammar_async.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2022, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import asyncio 10 | from ..core.utils.log import logger 11 | 12 | class AsyncObject(object): 13 | async def _asyncio_loop_func(self): 14 | logger.debug('asyncio thread start ...') 15 | while self.connected: 16 | await asyncio.sleep(0.001) 17 | logger.debug('asyncio thread exit ...') 18 | 19 | @staticmethod 20 | async def _async_run_callback(callback, msg): 21 | await callback(msg) 22 | -------------------------------------------------------------------------------- /xarm/x3/grammar_coroutine.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2022, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import asyncio 10 | from ..core.utils.log import logger 11 | 12 | 13 | class CoroutineObject(object): 14 | @asyncio.coroutine 15 | def _asyncio_loop_func(self): 16 | logger.debug('asyncio thread start ...') 17 | while self.connected: 18 | yield from asyncio.sleep(0.001) 19 | logger.debug('asyncio thread exit ...') 20 | 21 | @staticmethod 22 | @asyncio.coroutine 23 | def _async_run_callback(callback, msg): 24 | yield from callback(msg) -------------------------------------------------------------------------------- /xarm/x3/modbus_tcp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2023, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | from .base import Base 9 | from .decorator import xarm_is_connected 10 | 11 | 12 | class ModbusTcp(Base): 13 | def __init__(self): 14 | super(ModbusTcp, self).__init__() 15 | 16 | @xarm_is_connected(_type='get') 17 | def read_coil_bits(self, addr, quantity): 18 | """ 19 | func_code: 0x01 20 | """ 21 | return self.arm_cmd.read_coil_bits(addr, quantity) 22 | 23 | @xarm_is_connected(_type='get') 24 | def read_input_bits(self, addr, quantity): 25 | """ 26 | func_code: 0x02 27 | """ 28 | return self.arm_cmd.read_input_bits(addr, quantity) 29 | 30 | @xarm_is_connected(_type='get') 31 | def read_holding_registers(self, addr, quantity, is_signed=False): 32 | """ 33 | func_code: 0x03 34 | """ 35 | return self.arm_cmd.read_holding_registers(addr, quantity, is_signed) 36 | 37 | @xarm_is_connected(_type='get') 38 | def read_input_registers(self, addr, quantity, is_signed=False): 39 | """ 40 | func_code: 0x04 41 | """ 42 | return self.arm_cmd.read_input_registers(addr, quantity, is_signed) 43 | 44 | @xarm_is_connected(_type='set') 45 | def write_single_coil_bit(self, addr, bit_val): 46 | """ 47 | func_code: 0x05 48 | """ 49 | return self.arm_cmd.write_single_coil_bit(addr, bit_val) 50 | 51 | @xarm_is_connected(_type='set') 52 | def write_single_holding_register(self, addr, reg_val): 53 | """ 54 | func_code: 0x06 55 | """ 56 | return self.arm_cmd.write_single_holding_register(addr, reg_val) 57 | 58 | @xarm_is_connected(_type='set') 59 | def write_multiple_coil_bits(self, addr, bits): 60 | """ 61 | func_code: 0x0F 62 | """ 63 | return self.arm_cmd.write_multiple_coil_bits(addr, bits) 64 | 65 | @xarm_is_connected(_type='set') 66 | def write_multiple_holding_registers(self, addr, regs): 67 | """ 68 | func_code: 0x10 69 | """ 70 | return self.arm_cmd.write_multiple_holding_registers(addr, regs) 71 | 72 | @xarm_is_connected(_type='set') 73 | def mask_write_holding_register(self, addr, and_mask, or_mask): 74 | """ 75 | func_code: 0x16 76 | """ 77 | return self.arm_cmd.mask_write_holding_register(addr, and_mask, or_mask) 78 | 79 | @xarm_is_connected(_type='get') 80 | def write_and_read_holding_registers(self, r_addr, r_quantity, w_addr, w_regs, is_signed=False): 81 | """ 82 | func_code: 0x17 83 | """ 84 | return self.arm_cmd.write_and_read_holding_registers(r_addr, r_quantity, w_addr, w_regs, is_signed) 85 | -------------------------------------------------------------------------------- /xarm/x3/parse.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (MIT License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import re 10 | 11 | GCODE_PARAM_X = 'X' # TCP-X 12 | GCODE_PARAM_Y = 'Y' # TCP-Y 13 | GCODE_PARAM_Z = 'Z' # TCP-Z 14 | GCODE_PARAM_A = 'A' # TCP-Roll 15 | GCODE_PARAM_B = 'B' # TCP-Pitch 16 | GCODE_PARAM_C = 'C' # TCP-Yaw 17 | GCODE_PARAM_R = 'R' # TCP-Radius 18 | GCODE_PARAM_I = 'I' # Joint-1 19 | GCODE_PARAM_J = 'J' # Joint-2 20 | GCODE_PARAM_K = 'K' # Joint-3 21 | GCODE_PARAM_L = 'L' # Joint-4 22 | GCODE_PARAM_M = 'M' # Joint-5 23 | GCODE_PARAM_N = 'N' # Joint-6 24 | GCODE_PARAM_O = 'O' # Joint-7 25 | GCODE_PARAM_F = 'F' # Move-Speed 26 | GCODE_PARAM_Q = 'Q' # Move-Acc 27 | GCODE_PARAM_T = 'T' # Move-Time 28 | GCODE_PARAM_V = 'V' # Value 29 | GCODE_PARAM_D = 'D' # Addr 30 | 31 | 32 | class GcodeParser: 33 | def __init__(self): 34 | self._int_val = 0 35 | self._float_val = 0.0 36 | 37 | @staticmethod 38 | def __get_value(string, ch, return_type, default=None): 39 | pattern = r'{}(\-?\d+\.?\d*)'.format(ch) 40 | data = re.findall(pattern, string) 41 | if len(data) > 0: 42 | return return_type(data[0]) 43 | return default 44 | 45 | @staticmethod 46 | def __get_hex_value(string, ch, default=None): 47 | pattern = r'{}(-?\w{{3,4}})'.format(ch) 48 | data = re.findall(pattern, string) 49 | if len(data) > 0: 50 | return int(data[0], base=16) 51 | return default 52 | 53 | def _get_int_value(self, string, ch, default=None): 54 | return self.__get_value(string, ch, int, default=default) 55 | 56 | def _get_float_value(self, string, ch, default=None): 57 | return self.__get_value(string, ch, float, default=default) 58 | 59 | def get_int_value(self, string, default=None): 60 | if default is None: 61 | default = self._int_val 62 | self._int_val = self._get_int_value(string, GCODE_PARAM_V, default=default) 63 | return self._int_val 64 | else: 65 | return self._get_int_value(string, GCODE_PARAM_V, default=default) 66 | 67 | def get_float_value(self, string, default=0): 68 | return self._get_float_value(string, GCODE_PARAM_V, default=default) 69 | 70 | def get_addr(self, string, default=0): 71 | return self.__get_hex_value(string, GCODE_PARAM_D, default=default) 72 | 73 | def get_gcode_cmd_num(self, string, ch): 74 | return self._get_int_value(string, ch, default=-1) 75 | 76 | def get_mvvelo(self, string, default=None): 77 | return self._get_float_value(string, GCODE_PARAM_F, default=default) 78 | 79 | def get_mvacc(self, string, default=None): 80 | return self._get_float_value(string, GCODE_PARAM_Q, default=default) 81 | 82 | def get_mvtime(self, string, default=None): 83 | return self._get_float_value(string, GCODE_PARAM_T, default=default) 84 | 85 | def get_mvradius(self, string, default=None): 86 | return self._get_float_value(string, GCODE_PARAM_R, default=default) 87 | 88 | def get_id_num(self, string, default=None): 89 | return self._get_int_value(string, GCODE_PARAM_I, default=default) 90 | 91 | def get_poses(self, string, default=None): 92 | pose = [None] * 6 93 | pose[0] = self._get_float_value(string[2:], GCODE_PARAM_X, default=default) 94 | pose[1] = self._get_float_value(string[2:], GCODE_PARAM_Y, default=default) 95 | pose[2] = self._get_float_value(string[2:], GCODE_PARAM_Z, default=default) 96 | pose[3] = self._get_float_value(string[2:], GCODE_PARAM_A, default=default) 97 | pose[4] = self._get_float_value(string[2:], GCODE_PARAM_B, default=default) 98 | pose[5] = self._get_float_value(string[2:], GCODE_PARAM_C, default=default) 99 | return pose 100 | 101 | def get_joints(self, string, default=None): 102 | joints = [None] * 7 103 | joints[0] = self._get_float_value(string[2:], GCODE_PARAM_I, default=default) 104 | joints[1] = self._get_float_value(string[2:], GCODE_PARAM_J, default=default) 105 | joints[2] = self._get_float_value(string[2:], GCODE_PARAM_K, default=default) 106 | joints[3] = self._get_float_value(string[2:], GCODE_PARAM_L, default=default) 107 | joints[4] = self._get_float_value(string[2:], GCODE_PARAM_M, default=default) 108 | joints[5] = self._get_float_value(string[2:], GCODE_PARAM_N, default=default) 109 | joints[6] = self._get_float_value(string[2:], GCODE_PARAM_O, default=default) 110 | return joints 111 | -------------------------------------------------------------------------------- /xarm/x3/record.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import json 10 | import time 11 | import uuid 12 | from urllib import request 13 | from .code import APIState 14 | from ..core.config.x_config import XCONF 15 | from ..core.utils.log import logger 16 | from .base import Base 17 | from .decorator import xarm_is_connected 18 | 19 | 20 | class Record(Base): 21 | def __init__(self): 22 | super(Record, self).__init__() 23 | 24 | @xarm_is_connected(_type='get') 25 | def get_trajectories(self, ip=None): 26 | if ip is None: 27 | url = 'http://{}:18333/cmd'.format(self._port) 28 | else: 29 | url = 'http://{}:18333/cmd'.format(ip) 30 | try: 31 | data = {'cmd': 'xarm_list_trajs'} 32 | req = request.Request(url, headers={'Content-Type': 'application/json'}, data=json.dumps(data).encode('utf-8')) 33 | res = request.urlopen(req) 34 | if res.code == 200: 35 | result = json.loads(res.read().decode('utf-8')) 36 | return result['res'][0], [{'name': item['name'], 'duration': item['count'] / 100} for item in result['res'][1]] 37 | else: 38 | return APIState.API_EXCEPTION, [] 39 | except Exception as e: 40 | return APIState.API_EXCEPTION, [] 41 | 42 | @xarm_is_connected(_type='set') 43 | def start_record_trajectory(self): 44 | ret = self.arm_cmd.set_record_traj(1) 45 | self.log_api_info('API -> start_record_trajectory -> code={}'.format(ret[0]), code=ret[0]) 46 | return ret[0] 47 | 48 | @xarm_is_connected(_type='set') 49 | def stop_record_trajectory(self, filename=None, **kwargs): 50 | ret = self.arm_cmd.set_record_traj(0) 51 | if isinstance(filename, str) and filename.strip(): 52 | ret2 = self.save_record_trajectory(filename, wait=True, timeout=10, **kwargs) 53 | if ret2 != 0: 54 | return ret2 55 | self.log_api_info('API -> stop_record_trajectory -> code={}'.format(ret[0]), code=ret[0]) 56 | return ret[0] 57 | 58 | @xarm_is_connected(_type='set') 59 | def save_record_trajectory(self, filename, wait=True, timeout=2, **kwargs): 60 | assert isinstance(filename, str) and filename.strip() 61 | filename = filename.strip() 62 | if not filename.endswith('.traj'): 63 | full_filename = '{}.traj'.format(filename) 64 | else: 65 | full_filename = filename 66 | self.get_trajectory_rw_status() 67 | feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs) 68 | ret = self.arm_cmd.save_traj(full_filename, wait_time=0, feedback_key=feedback_key) 69 | trans_id = self._get_feedback_transid(feedback_key, studio_wait) 70 | self.log_api_info('API -> save_record_trajectory -> code={}'.format(ret[0]), code=ret[0]) 71 | ret[0] = self._check_code(ret[0]) 72 | if ret[0] == 0 and wait: 73 | return self.__wait_save_traj(timeout, trans_id, filename) 74 | if ret[0] != 0: 75 | logger.error('Save {} failed, ret={}'.format(filename, ret)) 76 | return ret[0] 77 | 78 | def __check_traj_status(self, status, filename='unknown'): 79 | if status == XCONF.TrajState.LOAD_SUCCESS: 80 | logger.info('Load {} success'.format(filename)) 81 | return 0 82 | elif status == XCONF.TrajState.LOAD_FAIL: 83 | logger.error('Load {} failed'.format(filename)) 84 | return APIState.TRAJ_RW_FAILED 85 | elif status == XCONF.TrajState.SAVE_SUCCESS: 86 | logger.info('Save {} success'.format(filename)) 87 | return 0 88 | elif status == XCONF.TrajState.SAVE_FAIL: 89 | logger.error('Save {} failed'.format(filename)) 90 | return APIState.TRAJ_RW_FAILED 91 | return -1 92 | 93 | def __wait_traj_op(self, timeout, trans_id, filename='unknown', op='Load'): 94 | if self._support_feedback and trans_id > 0: 95 | code, feedback_code = self._wait_feedback(timeout, trans_id) 96 | _, status = self.get_trajectory_rw_status() 97 | if code == 0: 98 | success_status = XCONF.TrajState.LOAD_SUCCESS if op == 'Load' else XCONF.TrajState.SAVE_SUCCESS 99 | failure_status = XCONF.TrajState.LOAD_FAIL if op == 'Load' else XCONF.TrajState.SAVE_FAIL 100 | status = success_status if feedback_code == XCONF.FeedbackCode.SUCCESS else failure_status if feedback_code == XCONF.FeedbackCode.FAILURE else status 101 | return self.__check_traj_status(status, filename) 102 | else: 103 | expired = (time.monotonic() + timeout) if timeout is not None else 0 104 | idle_cnts = 0 105 | while timeout is None or time.monotonic() < expired: 106 | time.sleep(0.1) 107 | code, status = self.get_trajectory_rw_status() 108 | if self._check_code(code) == 0: 109 | if status == XCONF.TrajState.IDLE: 110 | idle_cnts += 1 111 | if idle_cnts >= 5: 112 | logger.info('{} {} failed, idle'.format(op, filename)) 113 | return APIState.TRAJ_RW_FAILED 114 | else: 115 | code = self.__check_traj_status(status, filename) 116 | if code >= 0: 117 | return code 118 | logger.warning('{} {} timeout'.format(op, filename)) 119 | return APIState.TRAJ_RW_TOUT 120 | 121 | def __wait_load_traj(self, timeout, trans_id, filename='unknown'): 122 | return self.__wait_traj_op(timeout, trans_id, filename, 'Load') 123 | 124 | def __wait_save_traj(self, timeout, trans_id, filename='unknown'): 125 | return self.__wait_traj_op(timeout, trans_id, filename, 'Save') 126 | 127 | def __wait_play_traj(self, timeout, trans_id, times=1): 128 | if self._support_feedback and trans_id > 0: 129 | code, feedback_code = self._wait_feedback(timeout, trans_id) 130 | if feedback_code == XCONF.FeedbackCode.FAILURE: 131 | code = APIState.TRAJ_PLAYBACK_FAILED 132 | else: 133 | start_time = time.monotonic() 134 | while self.state != 1: 135 | if self.state in [4]: 136 | return APIState.STATE_NOT_READY 137 | if time.monotonic() - start_time > 5: 138 | return APIState.TRAJ_PLAYBACK_TOUT 139 | time.sleep(0.1) 140 | max_count = int((time.monotonic() - start_time) / 0.1) 141 | max_count = max_count if max_count > 10 else 10 142 | start_time = time.monotonic() 143 | while self.mode != 11: 144 | if self.state == 1: 145 | start_time = time.monotonic() 146 | time.sleep(0.1) 147 | continue 148 | if self.state in [4]: 149 | return APIState.STATE_NOT_READY 150 | if time.monotonic() - start_time > 5: 151 | return APIState.TRAJ_PLAYBACK_TOUT 152 | time.sleep(0.1) 153 | time.sleep(0.1) 154 | count = 0 155 | while self.state not in [4]: 156 | if self.state == 2: 157 | if times == 1: 158 | break 159 | count += 1 160 | else: 161 | count = 0 162 | if count > max_count: 163 | break 164 | time.sleep(0.1) 165 | code = 0 if self.state != 4 else APIState.STATE_NOT_READY 166 | 167 | if self.state not in [4]: 168 | self.set_mode(0) 169 | self.set_state(0) 170 | self._sync() 171 | return code 172 | 173 | @xarm_is_connected(_type='set') 174 | def load_trajectory(self, filename, wait=True, timeout=None, **kwargs): 175 | assert isinstance(filename, str) and filename.strip() 176 | filename = filename.strip() 177 | if not filename.endswith('.traj'): 178 | full_filename = '{}.traj'.format(filename) 179 | else: 180 | full_filename = filename 181 | self.get_trajectory_rw_status() 182 | feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs) 183 | ret = self.arm_cmd.load_traj(full_filename, wait_time=0, feedback_key=feedback_key) 184 | trans_id = self._get_feedback_transid(feedback_key, studio_wait) 185 | self.log_api_info('API -> load_trajectory -> code={}'.format(ret[0]), code=ret[0]) 186 | if ret[0] == 0 and wait: 187 | return self.__wait_load_traj(timeout, trans_id, filename) 188 | if ret[0] != 0: 189 | logger.error('Load {} failed, ret={}'.format(filename, ret)) 190 | return ret[0] 191 | 192 | @xarm_is_connected(_type='set') 193 | def playback_trajectory(self, times=1, filename=None, wait=False, double_speed=1, **kwargs): 194 | assert isinstance(times, int) 195 | times = times if times > 0 else -1 196 | if isinstance(filename, str) and filename.strip(): 197 | ret = self.load_trajectory(filename, wait=True, timeout=None) 198 | if ret != 0: 199 | return ret 200 | if self.state in [4]: 201 | return APIState.NOT_READY 202 | feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs) 203 | if self.version_is_ge(1, 2, 11): 204 | ret = self.arm_cmd.playback_traj(times, double_speed, feedback_key=feedback_key) 205 | else: 206 | ret = self.arm_cmd.playback_traj_old(times) 207 | trans_id = self._get_feedback_transid(feedback_key, studio_wait) 208 | self.log_api_info('API -> playback_trajectory -> code={}'.format(ret[0]), code=ret[0]) 209 | if ret[0] == 0 and wait: 210 | return self.__wait_play_traj(None, trans_id, times) 211 | return ret[0] 212 | 213 | @xarm_is_connected(_type='get') 214 | def get_trajectory_rw_status(self): 215 | ret = self.arm_cmd.get_traj_rw_status() 216 | return ret[0], ret[1] 217 | -------------------------------------------------------------------------------- /xarm/x3/report.py: -------------------------------------------------------------------------------- 1 | from xarm.core.utils import convert 2 | 3 | 4 | class ReportHandler(object): 5 | def __init__(self, report_type): 6 | self.buffer = b'' 7 | self.report_size = 0 8 | self.report_type = report_type 9 | if self.report_type == 'devlop': 10 | self.parse_handler = self._parse_report_tcp_develop_data 11 | elif self.report_type == 'normal': 12 | self.parse_handler = self._parse_report_tcp_normal_data 13 | elif self.report_type == 'rich': 14 | self.parse_handler = self._parse_report_tcp_rich_data 15 | else: 16 | self.parse_handler = None 17 | self.source_data = b'' 18 | self.parse_dict = {} 19 | 20 | def reset(self): 21 | self.buffer = b'' 22 | self.report_size = 0 23 | 24 | def process_report_data(self, recv_data): 25 | if recv_data == -1: 26 | return 27 | self.buffer += recv_data 28 | if len(self.buffer) < 4: 29 | return 30 | if self.report_size == 0: 31 | self.report_size = convert.bytes_to_u32(self.buffer[:4]) 32 | if len(self.buffer) < self.report_size: 33 | return 34 | if self.report_type == 'rich' and self.report_size == 233 and len(self.buffer) >= 245: 35 | # 兼容某几版固件上报的数据的数据长度和实际数据的长度不一致 36 | try: 37 | if len(self.buffer) >= 249: 38 | if convert.bytes_to_u32(self.buffer[245:249]) != self.report_size: 39 | if convert.bytes_to_u32(self.buffer[233:237]) == self.report_size: 40 | data = self.buffer[:self.report_size] 41 | self.buffer = self.buffer[self.report_size:] 42 | else: 43 | self.reset() 44 | # TODO reconnect 45 | return -1 46 | else: 47 | data = self.buffer[:245] 48 | self.buffer = self.buffer[245:] 49 | else: 50 | if convert.bytes_to_u32(self.buffer[233:237]) != self.report_size: 51 | data = self.buffer[:245] 52 | self.buffer = self.buffer[245:] 53 | else: 54 | data = self.buffer[:self.report_size] 55 | self.buffer = self.buffer[self.report_size:] 56 | except: 57 | self.reset() 58 | # TODO reconnect 59 | return -1 60 | else: 61 | data = self.buffer[:self.report_size] 62 | self.buffer = self.buffer[self.report_size:] 63 | self.source_data = data 64 | if self.parse_handler: 65 | return self.parse_handler(data) 66 | 67 | def __parse_report_common_data(self, rx_data): 68 | # length = convert.bytes_to_u32(rx_data[0:4]) 69 | length = len(rx_data) 70 | state, mode = rx_data[4] & 0x0F, rx_data[4] >> 4 71 | cmd_num = convert.bytes_to_u16(rx_data[5:7]) 72 | angles = convert.bytes_to_fp32s(rx_data[7:7 * 4 + 7], 7) 73 | pose = convert.bytes_to_fp32s(rx_data[35:6 * 4 + 35], 6) 74 | torque = convert.bytes_to_fp32s(rx_data[59:7 * 4 + 59], 7) 75 | self.parse_dict['length'] = length 76 | self.parse_dict['state'] = state 77 | self.parse_dict['mode'] = mode 78 | self.parse_dict['cmd_num'] = cmd_num 79 | self.parse_dict['angles'] = angles 80 | self.parse_dict['pose'] = pose 81 | self.parse_dict['torque'] = torque 82 | return [length, state, mode, cmd_num, angles, pose, torque] 83 | 84 | def _parse_report_tcp_develop_data(self, rx_data): 85 | ret = self.__parse_report_common_data(rx_data) 86 | return ret 87 | 88 | def _parse_report_tcp_normal_data(self, rx_data): 89 | ret = self.__parse_report_common_data(rx_data) 90 | mtbrake, mtable, error_code, warn_code = rx_data[87:91] 91 | tcp_offset = convert.bytes_to_fp32s(rx_data[91:6 * 4 + 91], 6) 92 | tcp_load = convert.bytes_to_fp32s(rx_data[115:4 * 4 + 115], 4) 93 | collis_sens, teach_sens = rx_data[131:133] 94 | gravity_direction = convert.bytes_to_fp32s(rx_data[133:3 * 4 + 133], 3) 95 | mtbrake = [mtbrake & 0x01, mtbrake >> 1 & 0x01, mtbrake >> 2 & 0x01, mtbrake >> 3 & 0x01, 96 | mtbrake >> 4 & 0x01, mtbrake >> 5 & 0x01, mtbrake >> 6 & 0x01, mtbrake >> 7 & 0x01] 97 | mtable = [mtable & 0x01, mtable >> 1 & 0x01, mtable >> 2 & 0x01, mtable >> 3 & 0x01, 98 | mtable >> 4 & 0x01, mtable >> 5 & 0x01, mtable >> 6 & 0x01, mtable >> 7 & 0x01] 99 | self.parse_dict['mtbrake'] = mtbrake 100 | self.parse_dict['mtable'] = mtable 101 | self.parse_dict['tcp_offset'] = tcp_offset 102 | self.parse_dict['tcp_load'] = tcp_load 103 | self.parse_dict['error_code'] = error_code 104 | self.parse_dict['warn_code'] = warn_code 105 | self.parse_dict['collis_sens'] = collis_sens 106 | self.parse_dict['teach_sens'] = teach_sens 107 | self.parse_dict['gravity_direction'] = gravity_direction 108 | ret.extend([mtbrake, mtable, error_code, warn_code, tcp_offset, tcp_load, collis_sens, teach_sens, gravity_direction]) 109 | return ret 110 | 111 | def _parse_report_tcp_rich_data(self, rx_data): 112 | ret = self._parse_report_tcp_normal_data(rx_data) 113 | length = ret[0] 114 | if length >= 151: 115 | arm_type, arm_axis, arm_master_id, arm_slave_id, arm_motor_tid, arm_motor_fid = rx_data[145:151] 116 | self.parse_dict['arm_type'] = arm_type 117 | self.parse_dict['arm_axis'] = arm_axis 118 | self.parse_dict['arm_master_id'] = arm_master_id 119 | self.parse_dict['arm_slave_id'] = arm_slave_id 120 | self.parse_dict['arm_motor_tid'] = arm_motor_tid 121 | self.parse_dict['arm_motor_fid'] = arm_motor_fid 122 | ret.extend([arm_type, arm_axis, arm_master_id, arm_slave_id, arm_motor_tid, arm_motor_fid]) 123 | if length >= 181: 124 | version = str(rx_data[151:180], 'utf-8') 125 | self.parse_dict['version'] = version 126 | ret.append(version) 127 | if length >= 201: 128 | trs_msg = convert.bytes_to_fp32s(rx_data[181:201], 5) 129 | tcp_jerk, min_tcp_acc, max_tcp_acc, min_tcp_speed, max_tcp_speed = trs_msg 130 | self.parse_dict['tcp_jerk'] = tcp_jerk 131 | self.parse_dict['min_tcp_acc'] = min_tcp_acc 132 | self.parse_dict['max_tcp_acc'] = max_tcp_acc 133 | self.parse_dict['min_tcp_speed'] = min_tcp_speed 134 | self.parse_dict['max_tcp_speed'] = max_tcp_speed 135 | ret.extend([tcp_jerk, min_tcp_acc, max_tcp_acc, min_tcp_speed, max_tcp_speed]) 136 | if length >= 221: 137 | p2p_msg = convert.bytes_to_fp32s(rx_data[201:221], 5) 138 | joint_jerk, min_joint_acc, max_joint_acc, min_joint_speed, max_joint_speed = p2p_msg 139 | self.parse_dict['joint_jerk'] = joint_jerk 140 | self.parse_dict['min_joint_acc'] = min_joint_acc 141 | self.parse_dict['max_joint_acc'] = max_joint_acc 142 | self.parse_dict['min_joint_speed'] = min_joint_speed 143 | self.parse_dict['max_joint_speed'] = max_joint_speed 144 | ret.extend([joint_jerk, min_joint_acc, max_joint_acc, min_joint_speed, max_joint_speed]) 145 | if length >= 229: 146 | rot_msg = convert.bytes_to_fp32s(rx_data[221:229], 2) 147 | rot_jerk, max_rot_acc = rot_msg 148 | self.parse_dict['rot_jerk'] = rot_jerk 149 | self.parse_dict['max_rot_acc'] = max_rot_acc 150 | ret.extend([rot_jerk, max_rot_acc]) 151 | if length >= 245: 152 | servo_code = [val for val in rx_data[229:245]] 153 | self.parse_dict['servo_code'] = servo_code 154 | ret.extend([servo_code[:-2], servo_code[-2:]]) 155 | if length >= 252: 156 | temperatures = list(map(int, rx_data[245:252])) 157 | self.parse_dict['temperatures'] = temperatures 158 | ret.append(temperatures) 159 | if length >= 284: 160 | speeds = convert.bytes_to_fp32s(rx_data[252:8 * 4 + 252], 8) 161 | self.parse_dict['speeds'] = speeds 162 | ret.append(speeds) 163 | if length >= 288: 164 | count = convert.bytes_to_u32(rx_data[284:288]) 165 | self.parse_dict['count'] = count 166 | ret.append(count) 167 | if length >= 312: 168 | world_offset = convert.bytes_to_fp32s(rx_data[288:6 * 4 + 288], 6) 169 | self.parse_dict['world_offset'] = world_offset 170 | ret.append(world_offset) 171 | if length >= 314: 172 | cgpio_reset_enable, tgpio_reset_enable = rx_data[312:314] 173 | self.parse_dict['cgpio_reset_enable'] = cgpio_reset_enable 174 | self.parse_dict['tgpio_reset_enable'] = tgpio_reset_enable 175 | ret.extend([cgpio_reset_enable, tgpio_reset_enable]) 176 | if length >= 416: 177 | # if length >= 340: 178 | is_collision_check, collision_tool_type = rx_data[314:316] 179 | collision_tool_params = convert.bytes_to_fp32s(rx_data[316:340], 6) 180 | self.parse_dict['is_collision_check'] = is_collision_check 181 | self.parse_dict['collision_tool_type'] = collision_tool_type 182 | self.parse_dict['collision_tool_params'] = collision_tool_params 183 | ret.extend([is_collision_check, collision_tool_type, collision_tool_params]) 184 | # if length >= 354: 185 | voltages = convert.bytes_to_u16s(rx_data[340:354], 7) 186 | voltages = list(map(lambda x: x / 100, voltages)) 187 | self.parse_dict['voltages'] = voltages 188 | ret.append(voltages) 189 | # if length >= 382: 190 | currents = convert.bytes_to_fp32s(rx_data[354:382], 7) 191 | self._currents = currents 192 | self.parse_dict['currents'] = currents 193 | ret.append(currents) 194 | # if length >= 416: 195 | cgpio_states = [] 196 | cgpio_states.extend(rx_data[382:384]) 197 | cgpio_states.extend(convert.bytes_to_u16s(rx_data[384:400], 8)) 198 | cgpio_states[6:10] = list(map(lambda x: x / 4095.0 * 10.0, cgpio_states[6:10])) 199 | cgpio_states.append(list(map(int, rx_data[400:408]))) 200 | cgpio_states.append(list(map(int, rx_data[408:416]))) 201 | self.parse_dict['cgpio_states'] = cgpio_states 202 | ret.append(cgpio_states) 203 | return ret 204 | 205 | -------------------------------------------------------------------------------- /xarm/x3/robotiq.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2020, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import time 10 | from ..core.utils.log import logger 11 | from .code import APIState 12 | from .base import Base 13 | from .decorator import xarm_is_connected, xarm_is_not_simulation_mode 14 | 15 | 16 | class RobotIQ(Base): 17 | def __init__(self): 18 | super(RobotIQ, self).__init__() 19 | self.__robotiq_openmm = None 20 | self.__robotiq_closemm = None 21 | self.__robotiq_aCoef = None 22 | self.__robotiq_bCoef = None 23 | self._robotiq_status = { 24 | 'gOBJ': 0, # 1和2表示停止并抓取到物体，3表示停止但没有抓取到物体，0表示正在动也没有抓取到物体 25 | 'gSTA': 0, # 3表示激活完成 26 | 'gGTO': 0, 27 | 'gACT': 0, 28 | 'kFLT': 0, 29 | 'gFLT': 0, # 错误码 30 | 'gPR': 0, # 抓取器的目标位置 31 | 'gPO': 0, # 通过编码器获取抓取器的实际位置, 值介于0x00和0xFF之间, 0xFF为全闭合, 0x00为全张开 32 | 'gCU': 0, # 从电机驱动器上瞬时读电流, 值介于0x00和0xFF之间, 等效电流大约为=10 * value mA 33 | } 34 | 35 | @property 36 | def robotiq_error_code(self): 37 | return self.robotiq_status['gFLT'] 38 | 39 | @property 40 | def robotiq_status(self): 41 | return self._robotiq_status 42 | 43 | def __robotiq_send_modbus(self, data_frame, min_res_len=0): 44 | code = self.checkset_modbus_baud(self._default_robotiq_baud) 45 | if code != 0: 46 | return code, [] 47 | return self.getset_tgpio_modbus_data(data_frame, min_res_len=min_res_len, ignore_log=True) 48 | 49 | @xarm_is_connected(_type='get') 50 | def __robotiq_set(self, params): 51 | data_frame = [0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, len(params)] 52 | data_frame.extend(params) 53 | return self.__robotiq_send_modbus(data_frame, 6) 54 | 55 | @xarm_is_connected(_type='get') 56 | def __robotiq_get(self, params): 57 | data_frame = [0x09, 0x03] 58 | data_frame.extend(params) 59 | code, ret = self.__robotiq_send_modbus(data_frame, 3 + 2 * params[-1]) 60 | if code == 0 and len(ret) >= 5: 61 | gripper_status_reg = ret[3] 62 | self._robotiq_status['gOBJ'] = (gripper_status_reg & 0xC0) >> 6 63 | self._robotiq_status['gSTA'] = (gripper_status_reg & 0x30) >> 4 64 | self._robotiq_status['gGTO'] = (gripper_status_reg & 0x08) >> 3 65 | self._robotiq_status['gACT'] = gripper_status_reg & 0x01 66 | if len(ret) >= 7: 67 | fault_status_reg = ret[5] 68 | self._robotiq_status['kFLT'] = (fault_status_reg & 0xF0) >> 4 69 | self._robotiq_status['gFLT'] = fault_status_reg & 0x0F 70 | self._robotiq_status['gPR'] = ret[6] 71 | if len(ret) >= 9: 72 | self._robotiq_status['gPO'] = ret[7] 73 | self._robotiq_status['gCU'] = ret[8] 74 | if self._robotiq_status['gSTA'] == 3 and (self._robotiq_status['gFLT'] == 0 or self._robotiq_status['gFLT'] == 9): 75 | self.robotiq_is_activated = True 76 | else: 77 | self.robotiq_is_activated = False 78 | return code, ret 79 | 80 | @xarm_is_connected(_type='get') 81 | @xarm_is_not_simulation_mode(ret=(0, 0)) 82 | def robotiq_reset(self): 83 | params = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00] 84 | code, ret = self.__robotiq_set(params) 85 | self.log_api_info('API -> robotiq_reset -> code={}, response={}'.format(code, ret), code=code) 86 | return code, ret 87 | 88 | @xarm_is_connected(_type='get') 89 | @xarm_is_not_simulation_mode(ret=(0, 0)) 90 | def robotiq_set_activate(self, wait=True, timeout=3): 91 | params = [0x01, 0x00, 0x00, 0x00, 0x00, 0x00] 92 | code, ret = self.__robotiq_set(params) 93 | if wait and code == 0: 94 | code = self.robotiq_wait_activation_completed(timeout) 95 | self.log_api_info('API -> robotiq_set_activate ->code={}, response={}'.format(code, ret), code=code) 96 | if code == 0: 97 | self.robotiq_is_activated = True 98 | return code, ret 99 | 100 | @xarm_is_connected(_type='get') 101 | def robotiq_set_position(self, pos, speed=0xFF, force=0xFF, wait=True, timeout=5, **kwargs): 102 | if kwargs.get('wait_motion', True): 103 | has_error = self.error_code != 0 104 | is_stop = self.is_stop 105 | code = self.wait_move() 106 | if not (code == 0 or (is_stop and code == APIState.EMERGENCY_STOP) 107 | or (has_error and code == APIState.HAS_ERROR)): 108 | return code, 0 109 | if self.check_is_simulation_robot(): 110 | return 0, 0 111 | if kwargs.get('auto_enable') and not self.robotiq_is_activated: 112 | self.robotiq_reset() 113 | self.robotiq_set_activate(wait=True) 114 | params = [0x09, 0x00, 0x00, pos, speed, force] 115 | code, ret = self.__robotiq_set(params) 116 | if wait and code == 0: 117 | code = self.robotiq_wait_motion_completed(timeout, **kwargs) 118 | self.log_api_info('API -> robotiq_set_position ->code={}, response={}'.format(code, ret), code=code) 119 | return code, ret 120 | 121 | def robotiq_open(self, speed=0xFF, force=0xFF, wait=True, timeout=5, **kwargs): 122 | return self.robotiq_set_position(0x00, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) 123 | 124 | def robotiq_close(self, speed=0xFF, force=0xFF, wait=True, timeout=5, **kwargs): 125 | return self.robotiq_set_position(0xFF, speed=speed, force=force, wait=wait, timeout=timeout, **kwargs) 126 | 127 | @xarm_is_connected(_type='get') 128 | @xarm_is_not_simulation_mode(ret=(0, 0)) 129 | def robotiq_get_status(self, number_of_registers=3): 130 | number_of_registers = 3 if number_of_registers not in [1, 2, 3] else number_of_registers 131 | params = [0x07, 0xD0, 0x00, number_of_registers] 132 | # params = [0x07, 0xD0, 0x00, 0x01] 133 | # params = [0x07, 0xD0, 0x00, 0x03] 134 | return self.__robotiq_get(params) 135 | 136 | def robotiq_wait_activation_completed(self, timeout=3): 137 | failed_cnt = 0 138 | expired = time.monotonic() + timeout if timeout is not None and timeout > 0 else 0 139 | code = APIState.WAIT_FINISH_TIMEOUT 140 | while expired == 0 or time.monotonic() < expired: 141 | _, ret = self.robotiq_get_status(number_of_registers=3) 142 | failed_cnt = 0 if _ == 0 else failed_cnt + 1 143 | if _ == 0: 144 | gFLT = self._robotiq_status['gFLT'] 145 | gSTA = self._robotiq_status['gSTA'] 146 | code = APIState.END_EFFECTOR_HAS_FAULT if gFLT != 0 and not (gFLT == 5 and gSTA == 1) \ 147 | else 0 if gSTA == 3 else code 148 | else: 149 | code = APIState.NOT_CONNECTED if _ == APIState.NOT_CONNECTED else APIState.CHECK_FAILED if failed_cnt > 10 else code 150 | if code != APIState.WAIT_FINISH_TIMEOUT: 151 | break 152 | time.sleep(0.05) 153 | return code 154 | 155 | def robotiq_wait_motion_completed(self, timeout=5, **kwargs): 156 | failed_cnt = 0 157 | expired = time.monotonic() + timeout if timeout is not None and timeout > 0 else 0 158 | code = APIState.WAIT_FINISH_TIMEOUT 159 | check_detected = kwargs.get('check_detected', False) 160 | while expired == 0 or time.monotonic() < expired: 161 | _, ret = self.robotiq_get_status(number_of_registers=3) 162 | failed_cnt = 0 if _ == 0 else failed_cnt + 1 163 | if _ == 0: 164 | gFLT = self._robotiq_status['gFLT'] 165 | gSTA = self._robotiq_status['gSTA'] 166 | gOBJ = self._robotiq_status['gOBJ'] 167 | code = APIState.END_EFFECTOR_HAS_FAULT if gFLT != 0 and not (gFLT == 5 and gSTA == 1) \ 168 | else 0 if (check_detected and (gOBJ == 1 or gOBJ == 2)) or (gOBJ == 1 or gOBJ == 2 or gOBJ == 3) \ 169 | else code 170 | else: 171 | code = APIState.NOT_CONNECTED if _ == APIState.NOT_CONNECTED else APIState.CHECK_FAILED if failed_cnt > 10 else code 172 | if code != APIState.WAIT_FINISH_TIMEOUT: 173 | break 174 | time.sleep(0.05) 175 | if self.robotiq_error_code != 0: 176 | print('ROBOTIQ Gripper ErrorCode: {}'.format(self.robotiq_error_code)) 177 | if code == 0 and not self.robotiq_is_activated: 178 | code = APIState.END_EFFECTOR_NOT_ENABLED 179 | return code 180 | 181 | @xarm_is_connected(_type='set') 182 | @xarm_is_not_simulation_mode(ret=False) 183 | def check_robotiq_is_catch(self, timeout=5): 184 | return self.robotiq_wait_motion_completed(timeout=timeout, check_detected=True) == 0 185 | 186 | def robotiq_calibrate_mm(self, closemm, openmm): 187 | ret = self.robotiq_open(wait=True) 188 | if ret[0] == 0: 189 | open_bit = self._robotiq_status['gPO'] 190 | ret = self.robotiq_close(wait=True) 191 | if ret[0] == 0: 192 | close_bit = self._robotiq_status['gPO'] 193 | self.__robotiq_aCoef = (closemm - openmm) / (close_bit - open_bit) 194 | self.__robotiq_bCoef = (openmm * close_bit - closemm * open_bit) / (close_bit - open_bit) 195 | self.__robotiq_closemm = closemm 196 | self.__robotiq_openmm = openmm 197 | return 0 198 | return ret[0] 199 | 200 | def robotiq_set_position_mm(self, pos_mm, speed=0xFF, force=0xFF, wait=False, timeout=5, check_detected=False): 201 | if self.__robotiq_openmm is None or self.__robotiq_closemm is None: 202 | print('You have to calibrate the gripper before using the function robotiq_set_position_mm()') 203 | elif pos_mm > self.__robotiq_openmm: 204 | print("The maximum opening is {}".format(self.__robotiq_openmm)) 205 | else: 206 | pos = int(self.__robotiq_mm_to_bit(pos_mm)) 207 | return self.robotiq_set_position(pos, speed=speed, force=force, wait=wait, timeout=timeout, check_detected=check_detected) 208 | 209 | def __robotiq_mm_to_bit(self, mm): 210 | if self.__robotiq_aCoef is None or self.__robotiq_bCoef is None: 211 | print('You have to calibrate the gripper before using the function robotiq_mm_to_bit()') 212 | else: 213 | return (mm - self.__robotiq_bCoef) / self.__robotiq_aCoef 214 | 215 | def __robotiq_bit_to_mm(self, bit): 216 | if self.__robotiq_aCoef is None or self.__robotiq_bCoef is None: 217 | print('You have to calibrate the gripper before using the function robotiq_bit_to_mm()') 218 | else: 219 | return self.__robotiq_aCoef * bit + self.__robotiq_bCoef 220 | 221 | -------------------------------------------------------------------------------- /xarm/x3/servo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2019, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | from ..core.config.x_config import XCONF 10 | from ..core.config.x_code import ServoError 11 | from ..core.utils.log import logger, pretty_print 12 | from .base import Base 13 | from .decorator import xarm_is_connected 14 | 15 | 16 | class Servo(Base): 17 | def __init__(self): 18 | super(Servo, self).__init__() 19 | 20 | @xarm_is_connected(_type='get') 21 | def get_servo_debug_msg(self, show=False, lang='en'): 22 | ret = self.arm_cmd.servo_get_dbmsg() 23 | dbmsg = [] 24 | lang = lang if lang == 'cn' else 'en' 25 | if self._check_code(ret[0]) == 0: 26 | for i in range(1, 9): 27 | servo_error = ServoError(ret[i * 2], status=ret[i * 2 - 1]) 28 | name = ('伺服-{}'.format(i) if lang == 'cn' else 'Servo-{}'.format(i)) if i < 8 else ('机械爪' if lang == 'cn' else 'Gripper') 29 | dbmsg.append({ 30 | 'name': name, 31 | 'servo_id': i, 32 | 'status': servo_error.status, 33 | 'code': servo_error.code, 34 | 'title': servo_error.title[lang], 35 | 'desc': servo_error.description[lang] 36 | }) 37 | if show: 38 | pretty_print('************* {}, {}: {} **************'.format( 39 | '获取伺服信息' if lang == 'cn' else 'GetServoDebugMsg', 40 | '状态' if lang == 'cn' else 'Status', 41 | ret[0]), color='light_blue') 42 | for servo_info in dbmsg: 43 | color = 'red' if servo_info['code'] != 0 or servo_info['status'] != 0 else 'white' 44 | pretty_print('* {}, {}: {}, {}: {}, {}: {}'.format( 45 | servo_info['name'], 46 | '状态' if lang == 'cn' else 'Status', 47 | servo_info['status'], 48 | '错误码' if lang == 'cn' else 'Code', 49 | servo_info['code'], 50 | '信息' if lang == 'cn' else 'Info', 51 | servo_info['title']), color=color) 52 | pretty_print('*' * 50, color='light_blue') 53 | return ret[0], dbmsg 54 | 55 | @xarm_is_connected(_type='set') 56 | def set_servo_zero(self, servo_id=None): 57 | """ 58 | Danger, do not use, may cause the arm to be abnormal, just for debugging 59 | :param servo_id: 60 | :return: 61 | """ 62 | assert isinstance(servo_id, int) and 1 <= servo_id <= 8, 'The value of parameter servo_id can only be 1-8.' 63 | ret = self.arm_cmd.servo_set_zero(servo_id) 64 | self.log_api_info('API -> set_servo_zero(servo_id={}) -> code={}'.format(servo_id, ret[0]), code=ret[0]) 65 | return ret[0] 66 | 67 | @xarm_is_connected(_type='set') 68 | def set_servo_addr_16(self, servo_id=None, addr=None, value=None, id_check=True): 69 | """ 70 | Danger, do not use, may cause the arm to be abnormal, just for debugging 71 | :param servo_id: 72 | :param addr: 73 | :param value: 74 | :param id_check: 75 | :return: 76 | """ 77 | if id_check: 78 | assert isinstance(servo_id, int) and 1 <= servo_id <= 7, 'The value of parameter servo_id can only be 1-7.' 79 | assert addr is not None, 'The value of parameter addr cannot be None.' 80 | assert value is not None, 'The value of parameter value cannot be None.' 81 | ret = self.arm_cmd.servo_addr_w16(servo_id, addr, value) 82 | self.log_api_info('API -> set_servo_addr_16(servo_id={}, addr={}, value={}) -> code={}'.format(servo_id, addr, value, ret[0]), code=ret[0]) 83 | return ret[0] 84 | 85 | @xarm_is_connected(_type='get') 86 | def get_servo_addr_16(self, servo_id=None, addr=None, id_check=True): 87 | """ 88 | Danger, do not use, may cause the arm to be abnormal, just for debugging 89 | :param servo_id: 90 | :param addr: 91 | :return: 92 | """ 93 | if id_check: 94 | assert isinstance(servo_id, int) and 1 <= servo_id <= 7, 'The value of parameter servo_id can only be 1-7.' 95 | assert addr is not None, 'The value of parameter addr cannot be None.' 96 | ret = self.arm_cmd.servo_addr_r16(servo_id, addr) 97 | return ret[0], ret[1] 98 | 99 | @xarm_is_connected(_type='set') 100 | def set_servo_addr_32(self, servo_id=None, addr=None, value=None, id_check=True): 101 | """ 102 | Danger, do not use, may cause the arm to be abnormal, just for debugging 103 | :param servo_id: 104 | :param addr: 105 | :param value: 106 | :return: 107 | """ 108 | if id_check: 109 | assert isinstance(servo_id, int) and 1 <= servo_id <= 7, 'The value of parameter servo_id can only be 1-7.' 110 | assert addr is not None, 'The value of parameter addr cannot be None.' 111 | assert value is not None, 'The value of parameter value cannot be None.' 112 | ret = self.arm_cmd.servo_addr_w32(servo_id, addr, value) 113 | self.log_api_info('API -> set_servo_addr_32(servo_id={}, addr={}, value={}) -> code={}'.format(servo_id, addr, value, ret[0]), code=ret[0]) 114 | return ret[0] 115 | 116 | @xarm_is_connected(_type='get') 117 | def get_servo_addr_32(self, servo_id=None, addr=None, id_check=True): 118 | """ 119 | Danger, do not use, may cause the arm to be abnormal, just for debugging 120 | :param servo_id: 121 | :param addr: 122 | :return: 123 | """ 124 | if id_check: 125 | assert isinstance(servo_id, int) and 1 <= servo_id <= 7, 'The value of parameter servo_id can only be 1-7.' 126 | assert addr is not None, 'The value of parameter addr cannot be None.' 127 | ret = self.arm_cmd.servo_addr_r32(servo_id, addr) 128 | return ret[0], ret[1] 129 | 130 | @xarm_is_connected(_type='set') 131 | def clean_servo_error(self, servo_id=None): 132 | """ 133 | Danger, do not use, may cause the arm to be abnormal, just for debugging 134 | :param servo_id: 135 | :return: 136 | """ 137 | return self.set_servo_addr_16(servo_id, 0x0109, 1) 138 | 139 | @xarm_is_connected(_type='get') 140 | def get_servo_state(self, servo_id): 141 | """ 142 | 获取运行状态 143 | :param servo_id: 144 | :return: 145 | """ 146 | ret = self.get_servo_addr_16(servo_id, 0x0000) 147 | return ret 148 | 149 | @xarm_is_connected(_type='get') 150 | def get_servo_rotate_speed(self, servo_id): 151 | """ 152 | 获取转速 153 | :param servo_id: 154 | :return: 155 | """ 156 | ret = self.get_servo_addr_16(servo_id, 0x0001) 157 | return ret 158 | 159 | @xarm_is_connected(_type='get') 160 | def get_servo_current_percentage(self, servo_id): 161 | """ 162 | 获取电流百分比 163 | :param servo_id: 164 | :return: 165 | """ 166 | ret = self.get_servo_addr_16(servo_id, 0x0002) 167 | return ret 168 | 169 | @xarm_is_connected(_type='get') 170 | def get_servo_current(self, servo_id): 171 | """ 172 | 获取电流 173 | :param servo_id: 174 | :return: 175 | """ 176 | ret = self.get_servo_addr_16(servo_id, 0x0003) 177 | return ret[0], ret[1] / 100 178 | 179 | @xarm_is_connected(_type='get') 180 | def get_servo_command_position(self, servo_id): 181 | """ 182 | 获取指令位置 183 | :param servo_id: 184 | :return: 185 | """ 186 | ret = self.get_servo_addr_32(servo_id, 0x0004) 187 | return ret 188 | 189 | @xarm_is_connected(_type='get') 190 | def get_servo_position(self, servo_id): 191 | """ 192 | 获取电机位置 193 | :param servo_id: 194 | :return: 195 | """ 196 | ret = self.get_servo_addr_32(servo_id, 0x0006) 197 | return ret 198 | 199 | @xarm_is_connected(_type='get') 200 | def get_servo_position_deviation(self, servo_id): 201 | """ 202 | 获取位置误差 203 | :param servo_id: 204 | :return: 205 | """ 206 | ret = self.get_servo_addr_32(servo_id, 0x0008) 207 | return ret 208 | 209 | @xarm_is_connected(_type='get') 210 | def get_servo_electrical_angle(self, servo_id): 211 | """ 212 | 获取电角度 213 | :param servo_id: 214 | :return: 215 | """ 216 | ret = self.get_servo_addr_16(servo_id, 0x000B) 217 | return ret 218 | 219 | @xarm_is_connected(_type='get') 220 | def get_servo_drv8323_sr0_register(self, servo_id): 221 | """ 222 | 获取DRV8323_SR0状态寄存器 223 | :param servo_id: 224 | :return: 225 | """ 226 | ret = self.get_servo_addr_16(servo_id, 0x000C) 227 | return ret 228 | 229 | @xarm_is_connected(_type='get') 230 | def get_servo_drv8323_sr1_register(self, servo_id): 231 | """ 232 | 获取DRV8323_SR1状态寄存器 233 | :param servo_id: 234 | :return: 235 | """ 236 | ret = self.get_servo_addr_16(servo_id, 0x000D) 237 | return ret 238 | 239 | @xarm_is_connected(_type='get') 240 | def get_servo_temperature(self, servo_id): 241 | """ 242 | 获取当前温度 243 | :param servo_id: 244 | :return: 245 | """ 246 | ret = self.get_servo_addr_16(servo_id, 0x000E) 247 | return ret 248 | 249 | @xarm_is_connected(_type='get') 250 | def get_servo_alarm_code(self, servo_id): 251 | """ 252 | 获取当前报警代码 253 | :param servo_id: 254 | :return: 255 | """ 256 | ret = self.get_servo_addr_16(servo_id, 0x000F) 257 | return ret 258 | 259 | @xarm_is_connected(_type='get') 260 | def get_servo_alarm_current(self, servo_id): 261 | """ 262 | 获取报警发生时的电流值 263 | :param servo_id: 264 | :return: 265 | """ 266 | ret = self.get_servo_addr_16(servo_id, 0x0010) 267 | return ret 268 | 269 | @xarm_is_connected(_type='get') 270 | def get_servo_alarm_speed(self, servo_id): 271 | """ 272 | 获取报警发生时的速度值 273 | :param servo_id: 274 | :return: 275 | """ 276 | ret = self.get_servo_addr_16(servo_id, 0x0011) 277 | return ret 278 | 279 | @xarm_is_connected(_type='get') 280 | def get_servo_alarm_voltage(self, servo_id): 281 | """ 282 | 获取报警发生时的输入电压值 283 | :param servo_id: 284 | :return: 285 | """ 286 | ret = self.get_servo_addr_16(servo_id, 0x0012) 287 | return ret 288 | 289 | @xarm_is_connected(_type='get') 290 | def get_servo_bus_voltage(self, servo_id): 291 | """ 292 | 获取母线电压 293 | :param servo_id: 294 | :return: 295 | """ 296 | ret = self.get_servo_addr_16(servo_id, 0x0018) 297 | return ret[0], ret[1] / 100 298 | 299 | @xarm_is_connected(_type='get') 300 | def get_servo_mu_state(self, servo_id): 301 | """ 302 | 获取MU当前状态 303 | :param servo_id: 304 | :return: 305 | """ 306 | ret = self.get_servo_addr_16(servo_id, 0x001E) 307 | return ret 308 | 309 | @xarm_is_connected(_type='get') 310 | def get_servo_mu_alarm_count(self, servo_id): 311 | """ 312 | 获取MU上电后报警次数 313 | :param servo_id: 314 | :return: 315 | """ 316 | ret = self.get_servo_addr_16(servo_id, 0x001F) 317 | return ret 318 | 319 | @xarm_is_connected(_type='get') 320 | def get_servo_feedback_position(self, servo_id): 321 | """ 322 | 获取关节反馈位置 323 | :param servo_id: 324 | :return: 325 | """ 326 | ret = self.get_servo_addr_32(servo_id, 0x0040) 327 | return ret 328 | 329 | # @xarm_is_connected(_type='get') 330 | # def get_servo_current(self, servo_id): 331 | # """ 332 | # 获取电流 333 | # :param servo_id: 334 | # :return: 335 | # """ 336 | # ret = self.get_servo_addr_16(servo_id, 0x0042) 337 | # return ret 338 | 339 | @xarm_is_connected(_type='get') 340 | def get_servo_version(self, servo_id=1): 341 | """ 342 | 获取关节版本 343 | :param servo_id: 344 | :return: 345 | """ 346 | assert isinstance(servo_id, int) and 1 <= servo_id <= 8, 'The value of parameter servo_id can only be 1-8.' 347 | 348 | def _get_servo_version(id_num): 349 | versions = ['*', '*', '*'] 350 | ret1 = self.get_servo_addr_16(id_num, 0x0801) 351 | ret2 = self.get_servo_addr_16(id_num, 0x0802) 352 | ret3 = self.get_servo_addr_16(id_num, 0x0803) 353 | code = 0 354 | if ret1[0] == 0: 355 | versions[0] = ret1[1] 356 | else: 357 | code = ret1[0] 358 | if ret2[0] == 0: 359 | versions[1] = ret2[1] 360 | else: 361 | code = ret2[0] 362 | if ret3[0] == 0: 363 | versions[2] = ret3[1] 364 | else: 365 | code = ret3[0] 366 | # if code != 0: 367 | # _, err_warn = self.get_err_warn_code() 368 | # if _ in [0, 1, 2]: 369 | # if err_warn[0] not in [10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 28]: 370 | # versions = [ret1[1], ret2[1], ret3[1]] 371 | return code, '.'.join(map(str, versions)) 372 | 373 | if servo_id > self.axis: 374 | code = 0 375 | versions = [] 376 | for i in range(1, self.axis + 1): 377 | ret = _get_servo_version(i) 378 | if ret[0] != 0: 379 | code = ret[0] 380 | versions.append(ret[1]) 381 | return code, versions 382 | else: 383 | return _get_servo_version(servo_id) 384 | 385 | @xarm_is_connected(_type='get') 386 | def get_harmonic_type(self, servo_id=1): 387 | """ 388 | 获取关节版本 389 | :param servo_id: 390 | :return: 391 | """ 392 | assert isinstance(servo_id, int) and 1 <= servo_id <= 8, 'The value of parameter servo_id can only be 1-8.' 393 | 394 | if servo_id > self.axis: 395 | code = 0 396 | types = [] 397 | for i in range(1, self.axis + 1): 398 | ret = self.get_servo_addr_16(i, 0x081F) 399 | if ret[0] != 0: 400 | code = ret[0] 401 | types.append(ret[1]) 402 | return code, types 403 | else: 404 | return self.get_servo_addr_16(servo_id, 0x081F) 405 | 406 | @xarm_is_connected(_type='get') 407 | def get_servo_error_code(self, servo_id=None): 408 | assert servo_id is None or (isinstance(servo_id, int) and servo_id >= 1), \ 409 | 'The value of parameter servo_id must be greater than 1 or None.' 410 | code = 0 411 | if servo_id is None or servo_id > self.axis: 412 | count = 7 if servo_id == 8 else self.axis 413 | errcodes = [0] * count 414 | for i in range(count): 415 | ret = self.get_servo_addr_32(i + 1, XCONF.ServoConf.CURR_POS) 416 | if ret[0] == XCONF.UxbusState.ERR_CODE: 417 | _, err_warn = self.get_err_warn_code() 418 | if _ == 0: 419 | if i + 11 == err_warn[0]: 420 | errcodes[i] = ret[1] 421 | else: 422 | errcodes[i] = 0 423 | else: 424 | code = _ 425 | logger.error('Get controller errwarn: ret={}, errwarn={}'.format(code, err_warn)) 426 | errcodes[i] = ret[1] 427 | else: 428 | errcodes = 0 429 | ret = self.get_servo_addr_32(servo_id, XCONF.ServoConf.CURR_POS) 430 | if ret[0] == XCONF.UxbusState.ERR_CODE: 431 | _, err_warn = self.get_err_warn_code() 432 | if _ == 0: 433 | if servo_id + 10 == err_warn[0]: 434 | errcodes = ret[1] 435 | else: 436 | errcodes = 0 437 | else: 438 | code = _ 439 | logger.error('Get controller errwarn: ret={}, errwarn={}'.format(code, err_warn)) 440 | errcodes = ret[1] 441 | return code, errcodes 442 | 443 | @xarm_is_connected(_type='set') 444 | def clean_servo_pvl_err(self, servo_id=None): 445 | assert servo_id is None or (isinstance(servo_id, int) and servo_id >= 1), \ 446 | 'The value of parameter servo_id must be greater than 1 or None.' 447 | if servo_id is None or servo_id > self.axis: 448 | count = 7 if servo_id == 8 else self.axis 449 | ids = range(count) 450 | else: 451 | ids = [servo_id - 1] 452 | _, errcode = self.get_servo_error_code() 453 | for i in ids: 454 | if errcode[i] == 0x12: 455 | self.set_servo_addr_16(i + 1, XCONF.ServoConf.RESET_PVL, 0x0002) 456 | self.set_servo_addr_16(i + 1, XCONF.ServoConf.RESET_ERR, 1) 457 | return 0 458 | 459 | @xarm_is_connected(_type='get') 460 | def get_servo_all_pids(self, servo_id=None): 461 | assert servo_id is None or (isinstance(servo_id, int) and servo_id >= 1), \ 462 | 'The value of parameter servo_id must be greater than 1 or None.' 463 | self.clean_error() 464 | self.clean_warn() 465 | addrs = [ 466 | XCONF.ServoConf.POS_KP, XCONF.ServoConf.POS_FWDKP, XCONF.ServoConf.POS_PWDTC, 467 | XCONF.ServoConf.SPD_KP, XCONF.ServoConf.SPD_KI, XCONF.ServoConf.CURR_KP, 468 | XCONF.ServoConf.CURR_KI, XCONF.ServoConf.SPD_IFILT, XCONF.ServoConf.SPD_OFILT, 469 | XCONF.ServoConf.CURR_IFILT, XCONF.ServoConf.POS_KD, XCONF.ServoConf.POS_CMDILT, 470 | XCONF.ServoConf.GET_TEMP, XCONF.ServoConf.OVER_TEMP 471 | ] 472 | if servo_id is None or servo_id > self.axis: 473 | count = 7 if servo_id == 8 else self.axis 474 | pids = [[9999] * len(addrs) for _ in range(count)] 475 | for i in range(count): 476 | for j, addr in enumerate(addrs): 477 | _, data = self.get_servo_addr_16(i + 1, addr) 478 | if _ == 0: 479 | pids[i][j] = data 480 | else: 481 | pids = [9999] * len(addrs) 482 | for j, addr in enumerate(addrs): 483 | _, data = self.get_servo_addr_16(servo_id, addr) 484 | pids[j] = data 485 | return 0, pids 486 | -------------------------------------------------------------------------------- /xarm/x3/studio.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2021, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import json 10 | import warnings 11 | from ..core.utils.log import logger 12 | from .code import APIState 13 | 14 | try: 15 | from requests import Session 16 | except: 17 | import urllib.request 18 | 19 | class Session(object): 20 | class Request: 21 | def __init__(self, url, data, **kwargs): 22 | req = urllib.request.Request(url, data.encode('utf-8')) 23 | self.r = urllib.request.urlopen(req) 24 | self._data = self.r.read() 25 | 26 | @property 27 | def status_code(self): 28 | return self.r.code 29 | 30 | def json(self): 31 | return json.loads(self._data.decode('utf-8')) 32 | 33 | def post(self, url, data=None, **kwargs): 34 | return self.Request(url, data) 35 | 36 | def close(self): 37 | pass 38 | 39 | 40 | class Studio(object): 41 | def __init__(self, ip, ignore_warnning=False): 42 | super(Studio, self).__init__() 43 | if not ignore_warnning: 44 | warnings.warn("don't use it for now, just for debugging") 45 | self.__ip = ip 46 | self.__session = Session() 47 | 48 | def __del__(self): 49 | self.__session.close() 50 | 51 | def run_blockly_app(self, name, **kwargs): 52 | try: 53 | self.call_studio_api({}, api_name='Core.command.xarm_set_blockly_init', show_fail_log=False) 54 | except: 55 | pass 56 | kwargs['appName'] = name 57 | ret = self.call_studio_api(kwargs, api_name='Core.command.run_blockly') 58 | if ret: 59 | return ret['code'] 60 | return APIState.API_EXCEPTION 61 | 62 | def delete_blockly_app(self, name): 63 | ret = self.call_studio_api({ 64 | 'parentPath': name, 65 | 'selectNode': { 66 | 'type': 'file' 67 | } 68 | }, api_name='Core.command.app_delete_item') 69 | if ret: 70 | return ret['code'] 71 | return APIState.API_EXCEPTION 72 | 73 | def playback_trajectory(self, filename, times=1, wait=False, double_speed=1): 74 | ret = self.call_studio_api({ 75 | 'filename': filename, 76 | 'times': times, 77 | 'speed': double_speed, 78 | 'wait': wait, 79 | }, api_name='Core.command.xarm_playback_traj') 80 | if ret: 81 | return ret['code'] 82 | return APIState.API_EXCEPTION 83 | 84 | def delete_trajectory(self, filename): 85 | ret = self.call_studio_api({ 86 | 'filename': filename, 87 | }, api_name='Core.command.xarm_delete_traj') 88 | if ret: 89 | return ret['code'] 90 | return APIState.API_EXCEPTION 91 | 92 | def set_initial_point(self, point): 93 | ret = self.call_studio_api({'point': point}, api_name='Core.command.xarm_set_initial_point') 94 | if ret: 95 | return ret['code'] 96 | return APIState.API_EXCEPTION 97 | 98 | def get_initial_point(self): 99 | ret = self.call_studio_api(api_name='XArm.xarm_initial_point') 100 | if ret: 101 | return ret['code'], ret['data'] 102 | return APIState.API_EXCEPTION, [0] * 7 103 | 104 | def call_sdk_api(self, *args, **kwargs): 105 | kwargs['api_name'] = 'XArm.xarm.{}'.format(kwargs['api_name']) 106 | return self.call_studio_api(*args, **kwargs) 107 | 108 | def call_studio_api(self, *args, **kwargs): 109 | kwargs['path'] = 'v2/api' 110 | return self.__call_remote_api(*args, **kwargs) 111 | 112 | def __call_remote_api(self, *args, **kwargs): 113 | api_name = kwargs.pop('api_name', None) 114 | show_fail_log = kwargs.pop('show_fail_log', True) 115 | path = kwargs.pop('path') 116 | if self.__ip and api_name: 117 | r = self.__session.post('http://{}:18333/{}'.format(self.__ip, path), data=json.dumps({ 118 | 'cmd': api_name, 'args': args, 'kwargs': kwargs 119 | }), timeout=(5, None)) 120 | if r.status_code == 200: 121 | res = r.json() 122 | if res['code'] != 0: 123 | if show_fail_log: 124 | logger.error(res['data']) 125 | return res 126 | else: 127 | if show_fail_log: 128 | logger.error('request failed, http_status_code={}'.format(r.status_code)) 129 | else: 130 | if show_fail_log: 131 | logger.error('ip or api_name is empty, ip={}, api_name={}'.format(self.__ip, api_name)) 132 | 133 | def get_mount_direction(self): 134 | 135 | ret = self.call_studio_api(api_name='XArm.xarm_mount_degrees') 136 | if ret: 137 | return ret['code'], ret['data'] 138 | return APIState.API_EXCEPTION, [0, 0] 139 | -------------------------------------------------------------------------------- /xarm/x3/utils.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/env python3 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2018, UFACTORY, Inc. 5 | # All rights reserved. 6 | # 7 | # Author: Vinman 8 | 9 | import time 10 | import math 11 | import functools 12 | from ..core.utils.log import logger 13 | from .code import APIState 14 | from ..core.config.x_config import XCONF 15 | 16 | 17 | def compare_time(time1, time2): 18 | try: 19 | s_time = time.mktime(time.strptime(time1, '%Y-%m-%d')) 20 | e_time = time.mktime(time.strptime(time2, '%Y-%m-%d')) 21 | return int(s_time) - int(e_time) > 0 22 | except: 23 | return False 24 | 25 | 26 | def compare_version(v1, v2): 27 | for i in range(3): 28 | if v1[i] > v2[i]: 29 | return True 30 | elif v1[i] < v2[i]: 31 | return False 32 | return False 33 | 34 | 35 | def filter_invaild_number(num, ndigits=3, default=0.0): 36 | if math.isnan(num) or math.isinf(num): 37 | return round(default, 0) if ndigits < 0 else round(default, ndigits) 38 | return round(num, 0) if ndigits < 0 else round(num, ndigits) 39 | 40 | 41 | def to_radian(val, is_radian=False, default=0): 42 | return default if val is None else float(val) if is_radian else math.radians(val) 43 | 44 | -------------------------------------------------------------------------------- /xarm6-start.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/luigipacheco/animaquina-xarm/f67b9b0653e98dd4d7609510ec8a6a4d079a5c42/xarm6-start.blend --------------------------------------------------------------------------------