├── .gitignore ├── LICENSE ├── MecademicRobot ├── FirmwareUpdate.py ├── RobotController.py ├── RobotFeedback.py └── __init__.py ├── README.md ├── docs └── logo │ └── mecademic_logo.jpg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Mecademic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MecademicRobot/FirmwareUpdate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import sys 3 | import argparse 4 | import requests 5 | import time 6 | import json 7 | from MecademicRobot import RobotController as RC 8 | 9 | def get_args(): 10 | """Command line interface to get arguments 11 | 12 | Command line interface to collect the robot IP address and the path 13 | of the firmware file. 14 | 15 | Returns 16 | ------- 17 | string 18 | Path the the robot firmware file for the robot update. 19 | string 20 | HTTP address of the robot to update. 21 | 22 | """ 23 | parser = argparse.ArgumentParser( 24 | description=f'Run the firmware Update of the robot.', 25 | epilog=f'exemple: python FirmwareUpdate.py --robot_fw_path ../../fw/v8.3.2.update --robot_ip_address m500-test-1.') 26 | parser.add_argument( 27 | '--robot_fw_path', 28 | metavar='robot_fw_path', 29 | type=str, 30 | nargs='+', 31 | default='.', 32 | help='The path of the firmware to update the robot.') 33 | parser.add_argument( 34 | '--robot_ip_address', 35 | metavar='robot_ip_address', 36 | type=str, 37 | nargs='+', 38 | default=['192.168.0.100'], 39 | help='The IP of the robot that will be update.') 40 | args = parser.parse_args(sys.argv[1:]) 41 | return [args.robot_fw_path[0], args.robot_ip_address[0]] 42 | 43 | def update_robot(file_path, ip_address): 44 | """Send the update specified by file_path to the robot. 45 | 46 | Parameters 47 | ---------- 48 | Param file_path : string 49 | Path to the firware file 50 | Param ip_address: string 51 | IP Address of the robot to update 52 | 53 | """ 54 | REQUEST_GET_TIMEOUT = 10 55 | ip_address_long = f'http://{ip_address}/' 56 | robot_inst = RC.RobotController(ip_address) 57 | if robot_inst.connect() : 58 | robot_inst.DeactivateRobot() 59 | robot_inst.disconnect() 60 | print(f'Opening firmware file...') 61 | try: 62 | update_file = open(file_path, 'rb') 63 | except OSError: 64 | print(f'Could not open/read file: {file_path}.') 65 | sys.exit() 66 | with update_file: 67 | update_data = update_file.read() 68 | update_file_size_str = str(len(update_data)) 69 | print(f'Done, update size is: {update_file_size_str}B.') 70 | 71 | print(f'Uploading file...') 72 | headers = {'Connection': 'keep-alive', 73 | 'Content-type': 'application/x-gzip', 74 | 'Content-Length': update_file_size_str} 75 | r = requests.post(ip_address_long, data=update_data, headers=headers) 76 | try: 77 | r.raise_for_status() 78 | except requests.exceptions.HTTPError as errh: 79 | print(f'HTTP request error when posting Update request.') 80 | print(f'Error: {errh}') 81 | print(f'Firmware upload request failed.') 82 | sys.exit() 83 | except requests.exceptions.ConnectionError as errc: 84 | print(f'HTTP request connection error when posting Update request.') 85 | print(f'Error: {errc}') 86 | print(f'Firmware upload request failed.') 87 | sys.exit() 88 | except requests.exceptions.Timeout as errt: 89 | print(f'HTTP request timeout when posting Update request.') 90 | print(f'Error: {errt}') 91 | print(f'Firmware upload request failed.') 92 | sys.exit() 93 | except requests.exceptions.RequestException as err: 94 | print(f'HTTP request return a connection Error when posting Update request.') 95 | print(f'Error: {err}') 96 | print(f'Firmware upload request failed.') 97 | sys.exit() 98 | 99 | print('Upgrading the robot...') 100 | update_done = False 101 | progress = '' 102 | last_progress = '' 103 | while not update_done: 104 | r = requests.get(ip_address_long, 'update', timeout=REQUEST_GET_TIMEOUT) 105 | try: 106 | r.raise_for_status() 107 | except requests.exceptions.HTTPError as errh: 108 | print(f'HTTP request error when posting Update request.') 109 | print(f'Error: {errh}') 110 | print(f'Firmware upgrading failed.') 111 | sys.exit() 112 | except requests.exceptions.ConnectionError as errc: 113 | print(f'HTTP request connection error when posting Update request.') 114 | print(f'Error: {errc}') 115 | print(f'Firmware upgrading failed.') 116 | sys.exit() 117 | except requests.exceptions.Timeout as errt: 118 | print(f'HTTP request timeout when posting Update request.') 119 | print(f'Error: {errt}') 120 | print(f'Firmware upgrading failed.') 121 | sys.exit() 122 | except requests.exceptions.RequestException as err: 123 | print(f'HTTP request return a connection Error when posting Update request.') 124 | print(f'Error: {err}') 125 | print(f'Firmware upgrading failed.') 126 | sys.exit() 127 | 128 | if r.status_code == 200: 129 | resp = r.text 130 | else: 131 | resp = None 132 | # When the json file is not yet created, get() returns 0. 133 | if (resp is None) or (resp == '0'): 134 | continue 135 | 136 | try: 137 | request_answer = json.loads(resp) 138 | except Exception as e: 139 | print(f'Failed to parse the answer "{resp}". {e}') 140 | continue 141 | 142 | if request_answer.get('STATUS'): 143 | status_code = int(request_answer.get('STATUS').get('Code')) 144 | status_msg = request_answer.get('STATUS').get('MSG') 145 | 146 | if status_code in [0, 1]: 147 | keys = sorted(request_answer.get('LOG').keys()) 148 | if keys: 149 | last_progress = progress 150 | progress = request_answer.get('LOG').get(keys[-1]) 151 | new_progress = progress.replace(last_progress, '') 152 | if '#' in new_progress: 153 | print(f' ', end='') 154 | print(f'{new_progress}', end='', flush='True') 155 | if '100%' in new_progress: 156 | print(f'') 157 | if status_code == 0: 158 | update_done = True 159 | print(f'{status_msg}') 160 | else: 161 | print(f'{status_msg}') 162 | raise RuntimeError('Error while updating') 163 | 164 | time.sleep(2) 165 | 166 | print(f'Update done') 167 | time.sleep(5) 168 | print(f'Rebooting ...', end='', flush='True') 169 | while(not robot_inst.connect()): 170 | time.sleep(1) 171 | print(f'...', end='', flush='True') 172 | print(f'\nRebooting done.') 173 | robot_inst.disconnect() 174 | 175 | 176 | if __name__ == '__main__': 177 | """Update the robot firmware. 178 | 179 | """ 180 | [robot_fw_path, robot_ip_address] = get_args() 181 | update_robot(robot_fw_path, robot_ip_address) 182 | -------------------------------------------------------------------------------- /MecademicRobot/RobotController.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import socket 3 | import time 4 | 5 | 6 | class RobotController: 7 | """Class for the Mecademic Robot allowing for communication and control of the 8 | Mecademic Robot with all of its features available. 9 | 10 | Attributes 11 | ---------- 12 | address : string 13 | The IP address associated to the Mecademic Robot 14 | socket : socket object 15 | Socket connecting to physical Mecademic Robot. 16 | EOB : int 17 | Setting for EOB (End of Block) reply. 18 | EOM : int 19 | Setting for EOM (End of Movement) reply. 20 | error : boolean 21 | Error Status of the Mecademic Robot. 22 | queue : boolean 23 | Queuing option flag. 24 | 25 | """ 26 | 27 | def __init__(self, address): 28 | """Constructor for an instance of the Class Mecademic Robot. 29 | 30 | Parameters 31 | ---------- 32 | address : string 33 | The IP address associated to the Mecademic Robot. 34 | 35 | """ 36 | self.address = address 37 | self.socket = None 38 | self.EOB = 1 39 | self.EOM = 1 40 | self.error = False 41 | self.queue = False 42 | 43 | def is_in_error(self): 44 | """Status method that checks whether the Mecademic Robot is in error mode. 45 | 46 | Return the global variable error, which is updated by the other methods. 47 | 48 | Returns 49 | ------- 50 | error : boolean 51 | Returns the error flag, True for error and False otherwise. 52 | 53 | """ 54 | return self.error 55 | 56 | def ResetError(self): 57 | """Resets the error in the Mecademic Robot. 58 | 59 | Returns 60 | ------- 61 | response : string 62 | Message from the robot. 63 | 64 | """ 65 | self.error = False 66 | cmd = 'ResetError' 67 | response = self.exchange_msg(cmd) 68 | reset_success = self._response_contains(response, ['The error was reset', 'There was no error to reset']) 69 | if reset_success: 70 | self.error = False 71 | else: 72 | self.error = True 73 | return response 74 | 75 | def connect(self): 76 | """Connects Mecademic Robot object communication to the physical Mecademic Robot. 77 | 78 | Returns 79 | ------- 80 | status : boolean 81 | Returns the status of the connection, true for success, false for failure 82 | 83 | """ 84 | try: 85 | self.socket = socket.socket() 86 | self.socket.settimeout(0.1) # 100ms 87 | try: 88 | self.socket.connect((self.address, 10000)) 89 | except socket.timeout: 90 | raise TimeoutError 91 | 92 | # Receive confirmation of connection 93 | if self.socket is None: 94 | raise RuntimeError 95 | 96 | self.socket.settimeout(10) # 10 seconds 97 | try: 98 | response = self.socket.recv(1024).decode('ascii') 99 | except socket.timeout: 100 | raise RuntimeError 101 | 102 | if self._response_contains(response, ['[3001]']): 103 | print(f'Another user is already connected, closing connection.') 104 | elif self._response_contains(response, ['[3000]']): # search for key [3000] in the received packet 105 | return True 106 | else: 107 | print(f'Unexpected code returned.') 108 | print(f'response: {response}') 109 | raise RuntimeError 110 | 111 | except TimeoutError: 112 | return False 113 | except RuntimeError: 114 | return False 115 | 116 | def disconnect(self): 117 | """Disconnects Mecademic Robot object from physical Mecademic Robot. 118 | 119 | """ 120 | if(self.socket is not None): 121 | self.socket.close() 122 | self.socket = None 123 | 124 | @staticmethod 125 | def _response_contains(response, code_list): 126 | """Scans received response for code IDs. 127 | 128 | Parameters 129 | ---------- 130 | response : 131 | Message to scan for codes. 132 | code_list : 133 | List of codes to look for in the response. 134 | 135 | Returns 136 | ------- 137 | response_found : 138 | Returns whether the response contains a code ID of interest. 139 | 140 | """ 141 | response_found = False 142 | for code in code_list: 143 | if response.find(code) != -1: 144 | response_found = True 145 | break 146 | return response_found 147 | 148 | def _send(self, cmd): 149 | """Sends a command to the physical Mecademic Robot. 150 | 151 | Parameters 152 | ---------- 153 | cmd : string 154 | Command to be sent. 155 | 156 | Returns 157 | ------- 158 | status : boolean 159 | Returns whether the message is sent. 160 | 161 | """ 162 | if self.socket is None or self.error: #check that the connection is established or the robot is in error 163 | return False #if issues detected, no point in trying to send a cmd that won't reach the robot 164 | cmd = cmd + '\0' 165 | status = 0 166 | while status == 0: 167 | try: #while the message hasn't been sent 168 | status = self.socket.send(cmd.encode('ascii')) #send command in ascii encoding 169 | except: 170 | break 171 | if status != 0: 172 | return True #return true when the message has been sent 173 | #Message failed to be sent, return false 174 | return False 175 | 176 | def _receive(self, answer_list, delay=20): 177 | """Receives message from the Mecademic Robot and looks for 178 | expected answer in the reply. 179 | 180 | Parameters 181 | ---------- 182 | answer_list : list 183 | Codes to look for in the response. 184 | delay : int 185 | Time to set for timeout of the socket. 186 | 187 | Returns 188 | ------- 189 | response : string 190 | Response received from Mecademic Robot. 191 | 192 | """ 193 | if self.socket is None: #check that the connection is established 194 | return #if no connection, nothing to receive 195 | response_list = [] 196 | response_found = False 197 | for x in answer_list: #convert codes to search for in answer into comparable format 198 | response_list.append(f'[{str(x)}]') 199 | error_found = False 200 | error_list = [f'[{str(i)}]' for i in range(1000, 1039)]+[f'[{str(i)}]' for i in [3001,3003,3005,3009,3014,3026]] #Make error codes in a comparable format 201 | self.socket.settimeout(delay) #set read timeout to desired delay 202 | while not response_found and not error_found: #while no answers have been received, keep looking 203 | try: 204 | response = self.socket.recv(1024).decode('ascii') #read message from robot 205 | except socket.timeout: 206 | return #if timeout reached, either connection lost or nothing was sent from robot (damn disabled EOB and EOM) 207 | if(len(response_list)!=0): #if the message has a code to look for, find them 208 | response_found = self._response_contains(response, response_list) 209 | error_found = self._response_contains(response, error_list) #search message for errors 210 | if error_found: #if errors have been found, flag the script 211 | self.error = True 212 | return response #return the retrieved message 213 | 214 | def exchange_msg(self, cmd, delay=20, decode=True): 215 | """Sends and receives with the Mecademic Robot. 216 | 217 | Parameters 218 | ---------- 219 | cmd : string 220 | Command to send to the Mecademic Robot. 221 | delay : int 222 | Timeout to set for the socket. 223 | decode : string 224 | decrypt response based on right response code 225 | 226 | Returns 227 | ------- 228 | response : string 229 | Response with desired code ID. 230 | 231 | """ 232 | response_list = self._get_answer_list(cmd) 233 | if(not self.error): #if there is no error 234 | status = self._send(cmd) #send the command to the robot 235 | if status is True: #if the command was sent 236 | if self.queue: #if Queueing enabled skip receving responses 237 | return 238 | else: 239 | answer = self._receive(response_list, delay)#get response from robot 240 | if answer is not None: #if message was retrieved 241 | for response in response_list: #search for response codes 242 | if self._response_contains(answer, [str(response)]): 243 | if(decode): 244 | return self._decode_msg(answer, response) #decrypt response based on right response code 245 | else: 246 | return answer 247 | error_list = [str(i) for i in range(1000, 1039)]+[str(i) for i in [3001,3003,3005,3009,3014,3026]] #Make error codes in a comparable format 248 | for response in error_list: 249 | if self._response_contains(answer, [str(response)]): 250 | if(decode): 251 | return self._decode_msg(answer, response) #decrypt response based on right response code 252 | else: 253 | return answer 254 | else: 255 | if(len(response_list) == 0): #if we aren't expecting anything, don't bother looking 256 | return 257 | else: 258 | return 259 | #if message didn't send correctly, reboot communication 260 | self.disconnect() 261 | time.sleep(1) 262 | self.connect() 263 | return 264 | 265 | def _build_command(self, cmd, arg_list=[]): 266 | """Builds the command string to send to the Mecademic Robot 267 | from the function name and arguments the command needs. 268 | 269 | Parameters 270 | ---------- 271 | cmd : string 272 | Command name to send to the Mecademic Robot 273 | arg_list : list 274 | List of arguments the command requires 275 | 276 | Returns 277 | ------- 278 | command : string 279 | Final command for the Mecademic Robot 280 | 281 | """ 282 | command = cmd 283 | if(len(arg_list)!=0): 284 | command = command + '(' 285 | for index in range(0, (len(arg_list)-1)): 286 | command = command+str(arg_list[index])+',' 287 | command = command+str(arg_list[-1])+')' 288 | return command 289 | 290 | def _decode_msg(self, response, response_key): 291 | """Decrypt information from the Mecademic Robot response to useful information 292 | that can be manipulated. 293 | 294 | Parameters 295 | ---------- 296 | response : string 297 | Response from the Mecademic Robot 298 | response_key : int 299 | Code ID of response to decrypt 300 | 301 | Returns 302 | ------- 303 | code_list_int : list of int 304 | Decrypted information 305 | 306 | """ 307 | code = response.replace('['+str(response_key)+'][', '').replace(']', '').replace('\x00', '') #remove delimiters and \x00 bytes 308 | code_list = code.split(',') #split packets into their individual selves 309 | if(response_key == 2026 or response_key == 2027): #if expected packet is from GetJoints (2026) or GetPose (2027), rest of packet is position data 310 | code_list_float = tuple((float(x) for x in code_list)) #convert position data to floats 311 | return code_list_float 312 | elif(response_key == 2029 or response_key == 2007 or response_key == 2079): #if expected packet is from GetConf (2029), GetStatusRobot (2007) or GetStatusGripper (2079), rest of packet is data 313 | code_list_int = tuple((int(x) for x in code_list)) #convert status data into integers 314 | return code_list_int 315 | else: 316 | return code #nothing to decrypt or decryption not specified 317 | 318 | def _get_answer_list(self, command): 319 | """Retrieve the expected answer codes that the Mecademic Robot should 320 | send as feedback after a command. 321 | 322 | Parameters 323 | ---------- 324 | command : string 325 | Command that is to be sent to the Mecademic Robot. 326 | 327 | Returns 328 | ------- 329 | answer_list : list 330 | List of answer codes to search for in response. 331 | 332 | """ 333 | if(command.find('ActivateRobot') != -1): 334 | return [2000,2001] 335 | elif(command.find('ActivateSim')!= -1): 336 | return [2045] 337 | elif(command.find('ClearMotion')!= -1): 338 | return [2044] 339 | elif(command.find('DeactivateRobot')!= -1): 340 | return [2004] 341 | elif(command.find('BrakesOn')!= -1): 342 | return [2010] 343 | elif(command.find('BrakesOff')!= -1): 344 | return [2008] 345 | elif(command.find('GetConf')!= -1): 346 | return [2029] 347 | elif(command.find('GetJoints')!= -1): 348 | return [2026] 349 | elif(command.find('GetStatusRobot')!= -1): 350 | return [2007] 351 | elif(command.find('GetStatusGripper')!= -1): 352 | return [2079] 353 | elif(command.find('GetPose')!= -1): 354 | return [2027] 355 | elif(command.find('Home')!= -1): 356 | return [2002,2003] 357 | elif(command.find('PauseMotion')!= -1): 358 | answer_list = [2042] 359 | if(self.EOM == 1): 360 | answer_list.append(3004) 361 | return answer_list 362 | elif(command.find('ResetError')!= -1): 363 | return [2005,2006] 364 | elif(command.find('ResumeMotion')!= -1): 365 | return [2043] 366 | elif(command.find('SetEOB')!= -1): 367 | return [2054,2055] 368 | elif(command.find('SetEOM')!= -1): 369 | return [2052,2053] 370 | else: 371 | answer_list = [] 372 | if(self.EOB==1): 373 | answer_list.append(3012) 374 | if(self.EOM==1): 375 | for name in ['MoveJoints','MoveLin','MoveLinRelTRF','MoveLinRelWRF','MovePose','SetCartAcc','SetJointAcc','SetTRF','SetWRF']: 376 | if(command.find(name) != -1): 377 | answer_list.append(3004) 378 | break 379 | return answer_list 380 | 381 | def ActivateRobot(self): 382 | """Activates the Mecademic Robot. 383 | 384 | Returns 385 | ------- 386 | response : string 387 | Returns receive decrypted response. 388 | 389 | """ 390 | cmd = 'ActivateRobot' 391 | return self.exchange_msg(cmd) 392 | 393 | def DeactivateRobot(self): 394 | """Deactivates the Mecademic Robot. 395 | 396 | Returns 397 | ------- 398 | response : string 399 | Returns receive decrypted response. 400 | 401 | """ 402 | cmd = 'DeactivateRobot' 403 | return self.exchange_msg(cmd) 404 | 405 | def ActivateSim(self): 406 | """Activates the Mecademic Robot simulation mode. 407 | 408 | Returns 409 | ------- 410 | response : string 411 | Returns receive decrypted response. 412 | 413 | """ 414 | cmd = 'ActivateSim' 415 | return self.exchange_msg(cmd) 416 | 417 | def DeactivateSim(self): 418 | """Deactivate the Mecademic Robot simulation mode. 419 | 420 | Returns 421 | ------- 422 | response : string 423 | Returns receive decrypted response. 424 | 425 | """ 426 | cmd = 'DeactivateSim' 427 | return self.exchange_msg(cmd) 428 | 429 | def SwitchToEtherCAT(self): 430 | """Places the Mecademic Robot in EtherCat mode 431 | 432 | Returns 433 | ------- 434 | response : string 435 | Returns receive decrypted response. 436 | 437 | """ 438 | cmd = 'SwitchToEtherCAT' 439 | return self.exchange_msg(cmd) 440 | 441 | def SetEOB(self, e): 442 | """Sets End of Block answer active or inactive in the Mecademic Robot. 443 | 444 | Parameters 445 | ---------- 446 | e : int 447 | Enables (1) EOB or Disables (0) EOB. 448 | 449 | Returns 450 | ------- 451 | response : string 452 | Returns receive decrypted response. 453 | 454 | """ 455 | if(e == 1): 456 | self.EOB = 1 457 | else: 458 | self.EOB = 0 459 | raw_cmd = 'SetEOB' 460 | cmd = self._build_command(raw_cmd,[e]) 461 | return self.exchange_msg(cmd) 462 | 463 | def SetEOM(self, e): 464 | """Sets End of Movement answer active or inactive in the Mecademic Robot. 465 | 466 | Parameters 467 | ---------- 468 | e : int 469 | Enables (1) EOM or Disables (0) EOM. 470 | 471 | Returns 472 | ------- 473 | response : string 474 | Returns receive decrypted response. 475 | 476 | """ 477 | if(e == 1): 478 | self.EOM = 1 479 | else: 480 | self.EOM = 0 481 | raw_cmd = 'SetEOM' 482 | cmd = self._build_command(raw_cmd,[e]) 483 | return self.exchange_msg(cmd) 484 | 485 | def home(self): 486 | """Homes the Mecademic Robot. 487 | 488 | Returns 489 | ------- 490 | response : string 491 | Returns receive decrypted response. 492 | 493 | """ 494 | cmd = 'Home' 495 | return self.exchange_msg(cmd) 496 | 497 | def Delay(self, t): 498 | """Gives the Mecademic Robot a wait time before performing another action. 499 | 500 | Parameters 501 | ---------- 502 | t : int or float 503 | Delay time in seconds. 504 | 505 | Returns 506 | ------- 507 | response : string 508 | Returns receive decrypted response. 509 | 510 | """ 511 | if(not isinstance(t,float)): 512 | t = float(t) 513 | raw_cmd = 'Delay' 514 | cmd = self._build_command(raw_cmd,[t]) 515 | return self.exchange_msg(cmd, t*2) 516 | 517 | def GripperOpen(self): 518 | """Opens the gripper of the end-effector. 519 | 520 | Returns 521 | ------- 522 | response : string 523 | Returns the decrypted response from the Mecademic Robot. 524 | 525 | """ 526 | cmd = 'GripperOpen' 527 | return self.exchange_msg(cmd) 528 | 529 | def GripperClose(self): 530 | """Closes the gripper of the end-effector. 531 | 532 | Returns 533 | ------- 534 | response : string 535 | Returns the decrypted response from the Mecademic Robot. 536 | 537 | """ 538 | cmd = 'GripperClose' 539 | return self.exchange_msg(cmd) 540 | 541 | def MoveJoints(self, theta_1, theta_2, theta_3, theta_4, theta_5, theta_6): 542 | """Moves the joints of the Mecademic Robot to the desired angles. 543 | 544 | Parameters 545 | ---------- 546 | theta_1 : float 547 | Angle of joint 1 548 | theta_2 : float 549 | Angle of joint 2 550 | theta_3 : float 551 | Angle of joint 3 552 | theta_4 : float 553 | Angle of joint 4 554 | theta_5 : float 555 | Angle of joint 5 556 | theta_6 : float 557 | Angle of joint 6 558 | 559 | Returns 560 | ------- 561 | response : string 562 | Returns receive decrypted response 563 | 564 | """ 565 | raw_cmd = 'MoveJoints' 566 | cmd = self._build_command(raw_cmd,[theta_1,theta_2,theta_3,theta_4,theta_5,theta_6]) 567 | return self.exchange_msg(cmd) 568 | 569 | def MoveLin(self, x, y, z, alpha, beta, gamma): 570 | """Moves the Mecademic Robot tool reference in a straight line to final 571 | point with specified direction 572 | 573 | Parameters 574 | ---------- 575 | x : float 576 | Final x coordinate. 577 | y : float 578 | Final y coordinate. 579 | z : float 580 | Final z coordinate. 581 | alpha : float 582 | Final Alpha angle. 583 | beta : float 584 | Final Beta angle. 585 | gamma : float 586 | Final Gamma angle. 587 | 588 | Returns 589 | ------- 590 | response : string 591 | Returns receive decrypted response. 592 | 593 | """ 594 | raw_cmd = 'MoveLin' 595 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 596 | return self.exchange_msg(cmd) 597 | 598 | def MoveLinRelTRF(self, x, y, z, alpha, beta, gamma): 599 | """Moves the Mecademic Robot tool reference frame to specified coordinates and heading. 600 | 601 | Parameters 602 | ---------- 603 | x : float 604 | New Reference x coordinate. 605 | y : float 606 | New Reference y coordinate. 607 | z : float 608 | New Reference z coordinate. 609 | alpha : float 610 | New Reference Alpha angle. 611 | beta : float 612 | New Reference Beta angle. 613 | gamma : float 614 | New Reference Gamma angle. 615 | 616 | Returns 617 | ------- 618 | response : string 619 | Returns receive decrypted response. 620 | 621 | """ 622 | raw_cmd = 'MoveLinRelTRF' 623 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 624 | return self.exchange_msg(cmd) 625 | 626 | def MoveLinRelWRF(self, x, y, z, alpha, beta, gamma): 627 | """Moves the Mecademic Robot world reference frame to specified coordinates and heading. 628 | 629 | Parameters 630 | ---------- 631 | x : float 632 | New Reference x coordinate. 633 | y : float 634 | New Reference y coordinate. 635 | z : float 636 | New Reference z coordinate. 637 | alpha : float 638 | New Reference Alpha angle. 639 | beta : float 640 | New Reference Beta angle. 641 | gamma : float 642 | New Reference Gamma angle. 643 | 644 | Returns 645 | ------- 646 | response : string 647 | Returns receive decrypted response. 648 | 649 | """ 650 | raw_cmd = 'MoveLinRelWRF' 651 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 652 | return self.exchange_msg(cmd) 653 | 654 | def MovePose(self, x, y, z, alpha, beta, gamma): 655 | """Moves the Mecademic Robot joints to have the TRF at (x,y,z) 656 | with heading (alpha, beta, gamma). 657 | 658 | Parameters 659 | ---------- 660 | x : float 661 | Final x coordinate. 662 | y : float 663 | Final y coordinate. 664 | z : float 665 | Final z coordinate. 666 | alpha : float 667 | Final Alpha angle. 668 | beta : float 669 | Final Beta angle. 670 | gamma : float 671 | Final Gamma angle. 672 | 673 | Returns 674 | ------- 675 | response : string 676 | Returns receive decrypted response. 677 | 678 | """ 679 | raw_cmd = 'MovePose' 680 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 681 | return self.exchange_msg(cmd) 682 | 683 | def SetBlending(self, p): 684 | """Sets the blending of the Mecademic Robot. 685 | 686 | Parameters 687 | ---------- 688 | p : int 689 | Enable(1-100)/Disable(0) Mecademic Robot's blending. 690 | 691 | Returns 692 | ------- 693 | response : string 694 | Returns receive decrypted response. 695 | 696 | """ 697 | raw_cmd = 'SetBlending' 698 | cmd = self._build_command(raw_cmd,[p]) 699 | return self.exchange_msg(cmd) 700 | 701 | def SetAutoConf(self, e): 702 | """Enables or Disables the automatic robot configuration 703 | selection and has effect only on the MovePose command. 704 | 705 | Parameters 706 | ---------- 707 | e : boolean 708 | Enable(1)/Disable(0) Mecademic Robot's automatic configuration selection. 709 | 710 | Returns 711 | ------- 712 | response : string 713 | Returns receive decrypted response. 714 | 715 | """ 716 | raw_cmd = 'SetAutoConf' 717 | cmd = self._build_command(raw_cmd,[e]) 718 | return self.exchange_msg(cmd) 719 | 720 | def SetCartAcc(self, p): 721 | """Sets the cartesian accelerations of the linear and angular movements of the 722 | Mecademic Robot end effector. 723 | 724 | Parameters 725 | ---------- 726 | p : float 727 | Value between 1 and 100. 728 | 729 | Returns 730 | ------- 731 | response : string 732 | Returns receive decrypted response. 733 | 734 | """ 735 | raw_cmd = 'SetCartAcc' 736 | cmd = self._build_command(raw_cmd,[p]) 737 | return self.exchange_msg(cmd) 738 | 739 | def SetCartAngVel(self, w): 740 | """Sets the cartesian angular velocity of the Mecademic Robot TRF with respect to its WRF. 741 | 742 | Parameters 743 | ---------- 744 | w : float 745 | Value between 0.001 and 180. 746 | 747 | Returns 748 | ------- 749 | response : string 750 | Returns receive decrypted response. 751 | 752 | """ 753 | raw_cmd = 'SetCartAngVel' 754 | cmd = self._build_command(raw_cmd,[w]) 755 | return self.exchange_msg(cmd) 756 | 757 | def SetCartLinVel(self, v): 758 | """Sets the cartesian linear velocity of the Mecademic Robot's TRF relative to its WRF. 759 | 760 | Parameters 761 | ---------- 762 | v : float 763 | Between 0.001 and 500. 764 | 765 | Returns 766 | ------- 767 | response : string 768 | Returns receive decrypted response. 769 | 770 | """ 771 | raw_cmd = 'SetCartLinVel' 772 | cmd = self._build_command(raw_cmd,[v]) 773 | return self.exchange_msg(cmd) 774 | 775 | def SetConf(self, c1, c3, c5): 776 | """Sets the desired Mecademic Robot inverse kinematic configuration to be observed in the 777 | MovePose command. 778 | 779 | Parameters 780 | ---------- 781 | c1 : int 782 | -1 or 1. 783 | c3 : int 784 | -1 or 1. 785 | c5 : int 786 | -1 or 1. 787 | 788 | Returns 789 | ------- 790 | response : string 791 | Returns received decrypted response. 792 | 793 | """ 794 | raw_cmd = 'SetConf' 795 | cmd = self._build_command(raw_cmd,[c1,c3,c5]) 796 | return self.exchange_msg(cmd) 797 | 798 | def SetGripperForce(self, p): 799 | """Sets the Gripper's grip force. 800 | 801 | Parameters 802 | ---------- 803 | p : int 804 | Value between 1 to 100. 805 | 806 | Returns 807 | ------- 808 | response : string 809 | Returns the decrypted response from the Mecademic Robot. 810 | 811 | """ 812 | raw_cmd = 'SetGripperForce' 813 | cmd = self._build_command(raw_cmd,[p]) 814 | return self.exchange_msg(cmd) 815 | 816 | def SetGripperVel(self, p): 817 | """Sets the Gripper fingers' velocity with respect to the gripper. 818 | 819 | Parameters 820 | ---------- 821 | p : int 822 | value between 1 to 100. 823 | 824 | Returns 825 | ------- 826 | response : string 827 | Returns the decrypted response from the Mecademic Robot. 828 | 829 | """ 830 | raw_cmd = 'SetGripperVel' 831 | cmd = self._build_command(raw_cmd,[p]) 832 | return self.exchange_msg(cmd) 833 | 834 | def SetJointAcc(self, p): 835 | """Sets the acceleration of the joints. 836 | 837 | Parameters 838 | ---------- 839 | p : int 840 | value between 1 to 100. 841 | 842 | Returns 843 | ------- 844 | response : string 845 | Returns the decrypted response from the Mecademic Robot. 846 | 847 | """ 848 | raw_cmd = 'SetJointAcc' 849 | cmd = self._build_command(raw_cmd,[p]) 850 | return self.exchange_msg(cmd) 851 | 852 | def SetJointVel(self, velocity): 853 | """Sets the angular velocities of the Mecademic Robot's joints. 854 | 855 | Parameters 856 | ---------- 857 | velocity : int 858 | value between 1 to 100. 859 | 860 | Returns 861 | ------- 862 | response : string 863 | Returns the decrypted response from the Mecademic Robot. 864 | 865 | """ 866 | raw_cmd = 'SetJointVel' 867 | cmd = self._build_command(raw_cmd,[velocity]) 868 | return self.exchange_msg(cmd) 869 | 870 | def SetTRF(self, x, y, z, alpha, beta, gamma): 871 | """Sets the Mecademic Robot TRF at (x,y,z) and heading (alpha, beta, gamma) 872 | with respect to the FRF. 873 | 874 | Parameters 875 | ---------- 876 | x : float 877 | Final x coordinate. 878 | y : float 879 | Final y coordinate. 880 | z : float 881 | Final z coordinate. 882 | alpha : float 883 | Final Alpha angle. 884 | beta : float 885 | Final Beta angle. 886 | gamma : float 887 | Final Gamma angle. 888 | 889 | Returns 890 | ------- 891 | response : string 892 | Returns receive decrypted response. 893 | 894 | """ 895 | raw_cmd = 'SetTRF' 896 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 897 | return self.exchange_msg(cmd) 898 | 899 | def SetWRF(self, x, y, z, alpha, beta, gamma): 900 | """Sets the Mecademic Robot WRF at (x,y,z) and heading (alpha, beta, gamma) 901 | with respect to the BRF. 902 | 903 | Parameters 904 | ---------- 905 | x : float 906 | Final x coordinate. 907 | y : float 908 | Final y coordinate. 909 | z : float 910 | Final z coordinate. 911 | alpha : float 912 | Final Alpha angle. 913 | beta : float 914 | Final Beta angle. 915 | gamma : float 916 | Final Gamma angle. 917 | 918 | Returns 919 | ------- 920 | response : string 921 | Returns receive decrypted response. 922 | 923 | """ 924 | raw_cmd = 'SetWRF' 925 | cmd = self._build_command(raw_cmd,[x,y,z,alpha,beta,gamma]) 926 | return self.exchange_msg(cmd) 927 | 928 | def GetStatusRobot(self): 929 | """Retrieves the robot status of the Mecademic Robot. 930 | 931 | Returns 932 | ------- 933 | status : tuple 934 | Returns tuple with status of Activation, Homing, Simulation, 935 | Error, Paused, EOB and EOM. 936 | 937 | """ 938 | received = None 939 | while received is None: 940 | cmd = 'GetStatusRobot' 941 | received = self.exchange_msg(cmd) 942 | code_list_int = received 943 | return {'Activated': code_list_int[0], 944 | 'Homing': code_list_int[1], 945 | 'Simulation': code_list_int[2], 946 | 'Error': code_list_int[3], 947 | 'Paused': code_list_int[4], 948 | 'EOB': code_list_int[5], 949 | 'EOM': code_list_int[6]} 950 | 951 | def GetStatusGripper(self): 952 | """Retrieves the gripper status of the Mecademic Robot. 953 | 954 | Returns 955 | ------- 956 | status : tuple 957 | Returns tuple with status of Gripper enabled, Homing state, Holding part 958 | Limit reached, Error state and force overload 959 | 960 | """ 961 | received = None 962 | while received is None: 963 | cmd = 'GetStatusGripper' 964 | received = self.exchange_msg(cmd) 965 | code_list_int = received 966 | return {'Gripper enabled': code_list_int[0], 967 | 'Homing state': code_list_int[1], 968 | 'Holding part': code_list_int[2], 969 | 'Limit reached': code_list_int[3], 970 | 'Error state': code_list_int[4], 971 | 'force overload': code_list_int[5]} 972 | 973 | def GetConf(self): 974 | """Retrieves the current inverse kinematic configuration. 975 | 976 | Returns 977 | ------- 978 | response : string 979 | Returns the decrypted response from the Mecademic Robot. 980 | 981 | """ 982 | cmd = 'GetConf' 983 | return self.exchange_msg(cmd) 984 | 985 | def GetJoints(self): 986 | """Retrieves the Mecademic Robot joint angles in degrees. 987 | 988 | Returns 989 | ------- 990 | response : string 991 | Returns the decrypted response from the Mecademic Robot. 992 | 993 | """ 994 | cmd = 'GetJoints' 995 | return self.exchange_msg(cmd) 996 | 997 | def GetPose(self): 998 | """Retrieves the current pose of the Mecademic Robot TRF with 999 | respect to the WRF. 1000 | 1001 | Returns 1002 | ------- 1003 | response : string 1004 | Returns the decrypted response from the Mecademic Robot. 1005 | 1006 | """ 1007 | cmd = 'GetPose' 1008 | return self.exchange_msg(cmd) 1009 | 1010 | def PauseMotion(self): 1011 | """Stops the robot movement and holds until ResumeMotion. 1012 | 1013 | Returns 1014 | ------- 1015 | response : string 1016 | Returns the decrypted response from the Mecademic Robot. 1017 | 1018 | """ 1019 | cmd = 'PauseMotion' 1020 | return self.exchange_msg(cmd) 1021 | 1022 | def ResumeMotion(self): 1023 | """Resumes the robot movement after being Paused from PauseMotion 1024 | or ClearMotion. 1025 | 1026 | Returns 1027 | ------- 1028 | response : string 1029 | Returns the decrypted response from the Mecademic Robot. 1030 | 1031 | """ 1032 | cmd = 'ResumeMotion' 1033 | return self.exchange_msg(cmd) 1034 | 1035 | def ClearMotion(self): 1036 | """Stops the robot movement and deletes the rest of the robot's 1037 | trajectory. Holds until a ResumeMotion. 1038 | 1039 | Returns 1040 | ------- 1041 | response : string 1042 | Returns the decrypted response from the Mecademic Robot. 1043 | 1044 | """ 1045 | cmd = 'ClearMotion' 1046 | return self.exchange_msg(cmd) 1047 | 1048 | def BrakesOn(self): 1049 | """These commands enables the brakes of joints 1, 2 and 3, 1050 | if and only if the robotis powered but deactivated. 1051 | 1052 | Returns 1053 | ------- 1054 | response : string 1055 | Returns the decrypted response from the Mecademic Robot. 1056 | 1057 | """ 1058 | cmd = 'BrakesOn' 1059 | return self.exchange_msg(cmd) 1060 | 1061 | def BrakesOff(self): 1062 | """These commands disables the brakes of joints 1, 2 and 3, 1063 | if and only if the robotis powered but deactivated. 1064 | 1065 | Returns 1066 | ------- 1067 | response: string 1068 | Returns the decrypted response from the Mecademic Robot. 1069 | 1070 | """ 1071 | cmd = 'BrakesOff' 1072 | return self.exchange_msg(cmd) 1073 | 1074 | def set_queue(self, e): 1075 | """ Enables the queueing of move commands for blending. 1076 | 1077 | Parameters 1078 | ---------- 1079 | e : boolean 1080 | Enables (1) Queueing or Disables (0) Queueing. 1081 | 1082 | Returns 1083 | ------- 1084 | response : string 1085 | Returns receive decrypted response. 1086 | 1087 | """ 1088 | if (e == 1): 1089 | self.queue = True 1090 | self.UserEOM = self.EOM 1091 | self.SetEOM(0) 1092 | else: 1093 | self.queue = False 1094 | self.SetEOM(self.UserEOM) 1095 | return self.queue 1096 | -------------------------------------------------------------------------------- /MecademicRobot/RobotFeedback.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import socket 3 | import re 4 | 5 | 6 | class RobotFeedback: 7 | """Class for the Mecademic Robot allowing for live positional 8 | feedback of the Mecademic Robot. 9 | 10 | Attributes 11 | ---------- 12 | address : string 13 | The IP address associated to the Mecademic robot. 14 | socket : socket 15 | Socket connecting to physical Mecademic Robot. 16 | robot_status : tuple of boolean 17 | States status bit of the robot. 18 | gripper_status : tuple of boolean 19 | States status bit of the gripper. 20 | joints : tuple of floats 21 | Joint angle in degrees of each joint starting from 22 | joint 1 going all way to joint 6. 23 | cartesian : tuple of floats 24 | The cartesian values in mm and degrees of the TRF. 25 | joints_vel : floats 26 | Velocity of joints. 27 | torque : tuple of floats 28 | Torque of joints. 29 | accelerometer : tuple of floats 30 | Acceleration of joints. 31 | last_msg_chunk : string 32 | Buffer of received messages. 33 | version : string 34 | Firmware version of the Mecademic Robot. 35 | version_regex : list of int 36 | Version_regex. 37 | 38 | """ 39 | 40 | def __init__(self, address, firmware_version): 41 | """Constructor for an instance of the class Mecademic robot. 42 | 43 | Parameters 44 | ---------- 45 | address : string 46 | The IP address associated to the Mecademic robot. 47 | firmware_version : string 48 | Firmware version of the Mecademic Robot. 49 | 50 | """ 51 | self.address = address 52 | self.socket = None 53 | self.robot_status = () 54 | self.gripper_status = () 55 | self.joints = () #Joint Angles, angles in degrees | [theta_1, theta_2, ... theta_n] 56 | self.cartesian = () #Cartesian coordinates, distances in mm, angles in degrees | [x,y,z,alpha,beta,gamma] 57 | self.joints_vel =() 58 | self.torque =() 59 | self.accelerometer =() 60 | self.last_msg_chunk = '' 61 | a = re.search(r'(\d+)\.(\d+)\.(\d+)', firmware_version) 62 | self.version = a.group(0) 63 | self.version_regex = [int(a.group(1)), int(a.group(2)), int(a.group(3))] 64 | 65 | def connect(self): 66 | """Connects Mecademic Robot object communication to the physical Mecademic Robot. 67 | 68 | Returns 69 | ------- 70 | status : boolean 71 | Return whether the connection is established. 72 | 73 | """ 74 | try: 75 | self.socket = socket.socket() 76 | self.socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY,1) 77 | self.socket.settimeout(1) #1s 78 | try: 79 | self.socket.connect((self.address, 10001)) #connect to the robot's address 80 | except socket.timeout: #catch if the robot is not connected to in time 81 | raise TimeoutError 82 | # Receive confirmation of connection 83 | if self.socket is None: #check that socket is not connected to nothing 84 | raise RuntimeError 85 | self.socket.settimeout(1) #1s 86 | try: 87 | if(self.version_regex[0] <= 7): 88 | self.get_data() 89 | elif(self.version_regex[0] > 7): #RobotStatus and GripperStatus are sent on 10001 upon connecting from 8.x firmware 90 | msg = self.socket.recv(256).decode('ascii') #read message from robot 91 | self._get_robot_status(msg) 92 | self._get_gripper_status(msg) 93 | return True 94 | except socket.timeout: 95 | raise RuntimeError 96 | except TimeoutError: 97 | return False 98 | # OTHER USER !!! 99 | except RuntimeError: 100 | return False 101 | 102 | def disconnect(self): 103 | """Disconnects Mecademic Robot object from physical Mecademic Robot. 104 | 105 | """ 106 | if self.socket is not None: 107 | self.socket.close() 108 | self.socket = None 109 | 110 | def get_data(self, delay=0.1): 111 | """Receives message from the Mecademic Robot and 112 | saves the values in appropriate variables. 113 | 114 | Parameters 115 | ---------- 116 | delay: int or float 117 | Time to set for timeout of the socket. 118 | 119 | """ 120 | if self.socket is None: #check that the connection is established 121 | return #if no connection, nothing to receive 122 | self.socket.settimeout(delay) #set read timeout to desired delay 123 | try: 124 | raw_msg = self.socket.recv(256).decode('ascii') #read message from robot 125 | raw_response = raw_msg.split('\x00') # Split the data at \x00 to manage fragmented data 126 | raw_response[0] = self.last_msg_chunk + raw_response[0] # Merge the first data with last fragment from previous data stream 127 | self.last_msg_chunk = raw_response[-1] 128 | for response in raw_response[:-1]: 129 | if(self.version_regex[0] <= 7): 130 | self._get_joints(response) 131 | self._get_cartesian(response) 132 | elif(self.version_regex[0] > 7): 133 | self._get_joints(response) 134 | self._get_cartesian(response) 135 | self._get_joints_vel(response) 136 | self._get_torque_ratio(response) 137 | self._get_accelerometer(response) 138 | except TimeoutError: 139 | pass 140 | 141 | def _get_robot_status(self, response): 142 | """Gets the values of RobotStatus bits from the message sent by 143 | the Robot upon connecting. 144 | Values saved to attribute robotstatus of the object. 145 | 146 | Parameters 147 | ---------- 148 | response : string 149 | Message received from the Robot. 150 | 151 | """ 152 | code = None 153 | code = self._get_response_code('RobotStatus') 154 | for resp_code in code: 155 | if response.find(resp_code) != -1: 156 | self.robot_status = self._decode_msg(response, resp_code) 157 | 158 | def _get_gripper_status(self, response): 159 | """Gets the values of GripperStatus bits from the message sent by 160 | the Robot upon connecting. 161 | Values saved to attribute robotstatus of the object. 162 | 163 | Parameters 164 | ---------- 165 | response : string 166 | Message received from the robot. 167 | 168 | """ 169 | code = None 170 | code = self._get_response_code('GripperStatus') 171 | for resp_code in code: 172 | if response.find(resp_code) != -1: 173 | self.gripper_status = self._decode_msg(response,resp_code) 174 | 175 | def _get_joints(self, response): 176 | """Gets the joint values of the variables from the message sent by the Robot. 177 | Values saved to attribute joints of the object. 178 | 179 | Parameters 180 | ---------- 181 | response: string 182 | Message received from the Robot. 183 | 184 | """ 185 | code = None 186 | code = self._get_response_code('JointsPose') 187 | for resp_code in code: 188 | if response.find(resp_code) != -1: 189 | self.joints = self._decode_msg(response, resp_code) 190 | 191 | def _get_cartesian(self, response): 192 | """Gets the cartesian values of the variables from the message sent by the Robot. 193 | Values saved to attribute cartesian of the object. 194 | 195 | Parameters 196 | ---------- 197 | response : string 198 | Message received from the Robot. 199 | 200 | """ 201 | code = None 202 | code = self._get_response_code('CartesianPose') 203 | for resp_code in code: 204 | if response.find(resp_code) != -1: 205 | self.cartesian = self._decode_msg(response,resp_code) 206 | 207 | def _get_joints_vel(self, response): 208 | """Gets the velocity values of the Joints from the message sent by the Robot. 209 | Values saved to attribute jointsvel of the object. 210 | 211 | Parameters 212 | ---------- 213 | response : string 214 | Message received from the Robot. 215 | 216 | """ 217 | code = None 218 | code = self._get_response_code('JointsVel') 219 | for resp_code in code: 220 | if response.find(resp_code) != -1: 221 | self.joints_vel = self._decode_msg(response,resp_code) 222 | 223 | def _get_torque_ratio(self, response): 224 | """Gets the torque ratio values of the Joints from the message sent by the Robot. 225 | Values saved to attribute torque of the object. 226 | 227 | Parameters 228 | ---------- 229 | response : string 230 | Message received from the Robot. 231 | 232 | """ 233 | code = None 234 | code = self._get_response_code('TorqueRatio') 235 | for resp_code in code: 236 | if response.find(resp_code) != -1: 237 | self.torque = self._decode_msg(response,resp_code) 238 | 239 | def _get_accelerometer(self,response): 240 | """Gets the accelerometers values from the message sent by the Robot. 241 | Values saved to attribute accelerometer of the object. 242 | 243 | Parameters 244 | ---------- 245 | response : string 246 | Message received from the Robot. 247 | 248 | """ 249 | code = None 250 | code = self._get_response_code('AccelerometerData') 251 | for resp_code in code: 252 | if response.find(resp_code) != -1: 253 | self.accelerometer = self._decode_msg(response,resp_code) 254 | 255 | def _get_response_code(self, param): 256 | """Retreives the response code for the parameters being streamed on port 100001. 257 | 258 | Parameters 259 | ---------- 260 | param : string 261 | Parameter that needs to be extracted from raw data strem from Mecademic Robot. 262 | 1. Robot Status {sent only once upon connecting on 10001}. 263 | 2. Gripper Status {sent only once upon connecting on 10001}. 264 | 3. Joints Pose feedback. 265 | 4. Cartesian Pose feedback. 266 | 5. Joints Velocity feedback. 267 | 6. Torque Ratio. 268 | 7. Accelerometer data. 269 | 270 | Returns 271 | -------- 272 | answer_list : list of strings 273 | List of response codes to search for in the raw data stream. 274 | 275 | """ 276 | if param.find('RobotStatus') != -1: 277 | return ['[2007]'] 278 | elif param.find('GripperStatus')!= -1: 279 | return ['[2079]'] 280 | elif param.find('JointsPose') != -1: 281 | if(self.version_regex[0] <= 7): 282 | return ['[2102]'] 283 | elif(self.version_regex[0] > 7): 284 | return ['[2026]','[2210]'] 285 | elif param.find('CartesianPose') != -1: 286 | if(self.version_regex[0] <= 7): 287 | return ['[2103]'] 288 | elif(self.version_regex[0] > 7): 289 | return ['[2027]','[2211]'] 290 | elif param.find('JointsVel') != -1: 291 | return ['[2212]'] 292 | elif param.find('TorqueRatio') != -1: 293 | return ['[2213]'] 294 | elif param.find('AccelerometerData') != -1: 295 | return ['[2220]'] 296 | else: 297 | return ['Invalid'] 298 | 299 | def _decode_msg(self, response, resp_code): 300 | """ 301 | 302 | Parameters 303 | ---------- 304 | response : string 305 | Message received from the Robot. 306 | resp_code : string 307 | Message to decode 308 | 309 | Returns 310 | -------- 311 | params : tuplt of float 312 | Message decoded. 313 | 314 | """ 315 | response = response.replace(resp_code+'[','').replace(']','') 316 | params = () 317 | if response != '': 318 | param_str = response.split(',') 319 | if len(param_str) == 6: 320 | params = tuple((float(x) for x in param_str)) 321 | elif len(param_str) == 7: 322 | params = tuple((float(x) for x in param_str[1:])) # remove timestamp 323 | else: 324 | params =() 325 | return params 326 | -------------------------------------------------------------------------------- /MecademicRobot/__init__.py: -------------------------------------------------------------------------------- 1 | from .RobotController import RobotController 2 | from .RobotFeedback import RobotFeedback 3 | from .FirmwareUpdate import update_robot -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Mecademic](./docs/logo/mecademic_logo.jpg "Mecademic") 2 | 3 | # Mecademic Python API 4 | 5 | --- 6 | ## NOTE 7 | This package has been deprecated and replaced with the [mecademicpy](https://github.com/Mecademic/mecademicpy) package. The [mecademicpy](https://github.com/Mecademic/mecademicpy) package has all the features of this module with many additional features making it easier to use in larger applications and more user friendly. 8 | 9 | --- 10 | 11 | A python module designed for Robot products from Mecademic. The module offers tools that give access to all the features of the Mecademic Robots such as MoveLin and MoveJoints. The module can be started from a terminal or a python application and controls the Mecademic Products. 12 | 13 | #### Supported Robots 14 | 15 | * Meca500 R3 16 | 17 | ## Getting Started 18 | 19 | These instructions will allow you to control the Robots from Mecademic through a python package and use the package for development and deployment purposes. 20 | 21 | ## Prerequisites 22 | 23 | To be able to use the module without unexpected errors, the user must have a copy of python installed on their machine and it is recommended to use python version 3.6 or higher. [Python](https://www.python.org/) can be installed from its main website (a reboot will be require after the installation to complete the setup). 24 | 25 | ## Downloading the package 26 | 27 | There are two ways to download the python driver for the Mecademic Robots. The first option is to download the MecademicRobot.py module from the Mecademic Github. The module can then be used in projects just like any other python module. The other option is download and install it to your computer through pip. Pip will place the MecademicRobot package with the rest of your python packages in your path and can easily be imported to multiple projects. To install the package through pip, Mecadmic python_driver Github repository url must be given. Pip will download and install the package on your machine and place it in the python local packages directory. This is done by running the following command: 28 | 29 | ``` 30 | pip install git+https://github.com/Mecademic/python_driver 31 | ``` 32 | If the package is in your python package path, it can be imported into any of your python projects without having a copy in your directory. 33 | 34 | ## Running a Robot from Mecademic with the package 35 | 36 | To run a Mecademic Robot with the MecademicRobot package, two options present itself. The first is to use it in an interactive terminal. The other is to use it as a dependency of a runnable script. 37 | 38 | #### Interactive Terminal 39 | 40 | To use the MecademicRobot package in an interactive terminal, the user can either run the python file RobotController.py with the -i modifier in a terminal as follows: 41 | 42 | ``` 43 | python -i /MecademicRobot.py 44 | ``` 45 | or 46 | ``` 47 | C:> python -i MecademicRobot.py 48 | ``` 49 | or, if the package is in your python package path, you can also start a python shell and import it: 50 | ``` 51 | python 52 | >>> import MecademicRobot 53 | ``` 54 | 55 | This will open a python terminal with the module already imported. From there you need to connect to the Mecademic Robot before being able to perform actions. This is done by making an instance of the class RobotController by passing the IP Address of the Robot as an argument and using the function Connect(): 56 | ``` 57 | >>> robot = MecademicRobot.RobotController('192.168.0.100') 58 | >>> robot.Connect() 59 | ``` 60 | 61 | Once succesfully connected to the Robot, you are able to start performing actions. To get it ready for operations, it must be activated and homed or it will fall in error. To do so, the following functions are run: 62 | 63 | ``` 64 | >>> robot.Activate() 65 | >>> robot.Home() 66 | ``` 67 | 68 | The Robot is now ready to perform operations. [The user programming manual](https://mecademic.com/resources/documentation) or the documentation in the module is sufficiant to be able to make the Robot perform actions and control the robot. 69 | 70 | When done with the Robot and desire to power it off, it must first be deactivated and disconnected to avoid issues and problems. It is done by two functions: 71 | 72 | ``` 73 | >>> robot.Deactivate() 74 | >>> robot.Disconnect() 75 | ``` 76 | 77 | If during use the Robot encounters an error, it will go into error mode. In this mode, the module will block any command to the Robot unless its an error reset. To properly reset errors on the Robot, the following function must be run: 78 | 79 | ``` 80 | >>> robot.ResetError() 81 | ``` 82 | 83 | #### Runnable Script 84 | 85 | To make a runnable script, the above functions calls remain the same in the script. Actions must be placed between an activation and a deactivation to avoid errors. Writing the script is like regular programming in python. It is recommended to have an error catcher to get the Robot out of error mode and not have the Robot stop working in the middle of operation. This can be creatively done by using the isInError() and action functions to catch the Robot immediately as it falls in error and bringing it back to operating condition. A method can be made to it like the following: 86 | ```py 87 | def AutoRepair(robot): 88 | if(robot.isInError()): 89 | robot.ResetError() 90 | elif(robot.GetStatus()['Paused']==1): 91 | robot.ResumeMotion() 92 | ``` 93 | Note: Deactivating and reactivating the Robot is not necessary but can be helpful in power cycle recovery. 94 | 95 | An example of a script for a Mecademic Robot would be: 96 | ```py 97 | import MecademicRobot 98 | robot = MecademicRobot.RobotController('192.168.0.100') 99 | robot.Connect() 100 | robot.Activate() 101 | robot.Home() 102 | robot.SetBlending(0) 103 | robot.SetJointVel(100) 104 | while True: 105 | robot.MoveJoints(0,0,0,170,115,175) 106 | robot.MoveJoints(0,0,0,-170,-115,-175) 107 | robot.MoveJoints(0,-70,70,0,0,0) 108 | robot.MoveJoints(0,90,-135,0,0,0) 109 | robot.GripperClose() 110 | robot.MoveJoints(0,0,0,0,0,0) 111 | robot.MoveJoints(175,0,0,0,0,0) 112 | robot.MoveJoints(-175,0,0,0,0,0) 113 | robot.MoveJoints(175,0,0,0,0,0) 114 | robot.GripperOpen() 115 | robot.MoveJoints(0,0,0,0,0,0) 116 | ``` 117 | This will make the Robot perform a repetitive task until the program is terminated. 118 | 119 | A more viable way to make long programs for the Mecademic Products is by using a string. In python, there are various ways to write strings and the string variable type is useful to make a program script by using the triple quotes string. This format can be spread across multiple lines and includes newlines into the string without placing them implicitly. This makes it easy to read and write the program you are trying to develop. The script can be written easily with the string format as follow: 120 | ```py 121 | Program = """SetBlending(0) 122 | SetJointVel(100) 123 | MoveJoints(0,0,0,170,115,175) 124 | MoveJoints(0,0,0,-170,-115,-175) 125 | MoveJoints(0,0,0,170,115,175) 126 | MoveJoints(0,0,0,-170,-115,-175) 127 | MoveJoints(0,0,0,170,115,175) 128 | MoveJoints(0,0,0,-170,-115,-175) 129 | MoveJoints(0,0,0,170,115,175) 130 | MoveJoints(0,0,0,-170,-115,-175) 131 | MoveJoints(0,0,0,170,115,175) 132 | MoveJoints(0,0,0,-170,-115,-175) 133 | MoveJoints(0,-70,70,0,0,0) 134 | MoveJoints(0,90,-135,0,0,0) 135 | MoveJoints(0,-70,70,0,0,0) 136 | MoveJoints(0,90,-135,0,0,0) 137 | MoveJoints(0,-70,70,0,0,0) 138 | MoveJoints(0,90,-135,0,0,0) 139 | MoveJoints(0,-70,70,0,0,0) 140 | MoveJoints(0,90,-135,0,0,0) 141 | MoveJoints(0,-70,70,0,0,0) 142 | MoveJoints(0,0,0,0,0,0) 143 | MoveJoints(175,0,0,0,0,0) 144 | MoveJoints(-175,0,0,0,0,0) 145 | MoveJoints(175,0,0,0,0,0) 146 | MoveJoints(-175,0,0,0,0,0) 147 | MoveJoints(175,0,0,0,0,0) 148 | MoveJoints(-175,0,0,0,0,0) 149 | MoveJoints(175,0,0,0,0,0) 150 | MoveJoints(-175,0,0,0,0,0) 151 | MoveJoints(175,0,0,0,0,0) 152 | MoveJoints(0,0,0,0,0,0)""" 153 | ``` 154 | Each line has a command with the arguments it requires. The commands are written in the same way as the functions in the module. Once the script is complete, it must be broken down into a list of actions. Python makes it easy by using the available string functions. 155 | ```py 156 | Program = Program.replace(' ','') 157 | movements = Program.split("\n") 158 | ``` 159 | The variable movements contains the list of actions to perform after getting rid of all the empty spaces and seperating the command by distinguishing commands by line. To go through the actions one by one, it is only required to make a loop that iterates through the actions. Using the __exchangeMsg__ function, it is easy to send the command to the Robot. The __exchangeMsg__ function is responsable for interpreting the commands and return expected messages back to the user. It is the backbone of most of the functions of the module. 160 | ```py 161 | for action in movements: 162 | robot.exchangeMsg(action) 163 | if(robot.isInError()): 164 | AutoRepair(robot) 165 | ``` 166 | If the script you wrote is one you wish the Robot to repeat until stopped by a user for whatever reason, the previous loop can be placed inside an infinite loop. Using all the information, building blocks and functions provided, you are fully equipped to control and program your Mecademic Robot for your project's requirements. 167 | 168 | ## Get Live Positional Feedback from the Robot 169 | 170 | The robot is capable of giving it's position while in movement and the RobotFeedback module of the MecademicRobot package allows the user to have access to that data. If the module is run in interactive shell or in a script, the best way to get data as fast as possible to another file or to be printed to the user is by using the module in an infinite loop. 171 | 172 | The RobotFeedback constructor takes in two arguments, the IP address of the robot and the firmware version of the robot. Both are of the string variable type. The functions call looks as follows: 173 | ```py 174 | feedback = MecademicRobot.RobotFeedback(IP, firmware_version) 175 | ``` 176 | An example for how to use the RobotFeedback module is as follows: 177 | ```py 178 | import MecademicRobot 179 | feedback = MecademicRobot.RobotFeedback('192.168.0.100', '7.0.6') 180 | feedback.Connect() 181 | while(True): 182 | feedback.getData() 183 | print(feedback.joints) 184 | print(feedback.cartesian) 185 | ``` 186 | By calling __getData()__, the values of joints and cartesian get updated with the latest received data from the robot. The format of the data for joints is (joint_1, joint_2, ..., joint_n), where n is the number of joints on the Robot and the values are in degrees. For the format of the data in cartesian, the data is of the form (x, y, z, alpha, beta, gamma), where x, y and z are in mm and alpha, beta and gamma are in degrees. This module works at its best when it is run in parallel with RobotController, either as another runnable, threading, etc. When in parallel, the live data will be refreshed at a faster speed while controlling the robot. 187 | 188 | ## Getting Help 189 | 190 | To get support, you can start an issue on the Mecademic/python_driver issues section or send an email to support@mecademic.com. 191 | 192 | ## License 193 | 194 | All packages in this repository are licensed under the MIT license. 195 | 196 | ## Authors 197 | 198 | * **Mecademic** - *Continuous work* 199 | 200 | 201 | -------------------------------------------------------------------------------- /docs/logo/mecademic_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mecademic/python_driver/da81cc3058ecb81ac5ba1101a6d4dfff4283e192/docs/logo/mecademic_logo.jpg -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import setuptools 3 | 4 | with open('README.md', 'r') as fh: 5 | long_description = fh.read() 6 | 7 | setuptools.setup( 8 | name='MecademicRobot', 9 | version='1.0.2', 10 | author='Mecademic', 11 | author_email='support@mecademic.com', 12 | license='MIT', 13 | description='A package to control the Mecademic Robots through python', 14 | long_description=long_description, 15 | long_description_content_type='markdown', 16 | url='https://github.com/Mecademic/python_driver', 17 | packages=setuptools.find_packages(), 18 | data_files=[('',['LICENSE', 'README.md'])], 19 | include_package_data=True, 20 | classifiers=[ 21 | 'Programming Language :: Python :: 3', 22 | 'License :: OSI Approved :: MIT License', 23 | 'Operating System :: OS Independent', 24 | ], 25 | entry_points={ 26 | 'console_scripts': [ 27 | 'FirmwareUpdate = FirmwareUpdate:main', 28 | ], 29 | } 30 | ) 31 | --------------------------------------------------------------------------------