From bc68523d38c990fc434f6ed66c38e7cb1acb854e Mon Sep 17 00:00:00 2001 From: marcsello Date: Thu, 1 Apr 2021 17:45:59 +0200 Subject: [PATCH] Added files from the original repo --- .gitignore | 133 +++++ dual_ursim_control/config.ini | 42 ++ dual_ursim_control/log_config.ini | 25 + dual_ursim_control/main.py | 876 ++++++++++++++++++++++++++++++ dual_ursim_control/path.xlsx | Bin 0 -> 13345 bytes requirements.txt | 5 + 6 files changed, 1081 insertions(+) create mode 100644 .gitignore create mode 100644 dual_ursim_control/config.ini create mode 100644 dual_ursim_control/log_config.ini create mode 100644 dual_ursim_control/main.py create mode 100644 dual_ursim_control/path.xlsx create mode 100644 requirements.txt diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..cf3c132 --- /dev/null +++ b/.gitignore @@ -0,0 +1,133 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +#Pycharm +.idea/ +*.iml diff --git a/dual_ursim_control/config.ini b/dual_ursim_control/config.ini new file mode 100644 index 0000000..f38c945 --- /dev/null +++ b/dual_ursim_control/config.ini @@ -0,0 +1,42 @@ +[DEFAULT] + +; Used with docker sim +;MASTER_IP = 192.168.0.10 +;SLAVE_IP = 192.168.0.20 + +; BME lab Configuration + +; Fred +MASTER_IP = 10.6.6.12 + +; Erik +SLAVE_IP = 10.6.6.10 + +; Master: RG2 Gripper +MASTER_GRIPPER_WIDTH_OPENED = 50 +MASTER_GRIPPER_WIDTH_CLOSED = 28 + +; Slave: RG2FT Gripper +SLAVE_GRIPPER_IP = 10.6.6.11 +SLAVE_GRIPPER_WIDTH_OPENED = 90 +SLAVE_GRIPPER_WIDTH_CLOSED = 28 + +MASTER_ENABLE = True +SLAVE_ENABLE = True + +JOGGING_ENABLE = False + +EXECUTION = SLOW +;EXECUTION = FAST + +;Used with moveL +L_SPEED_SLOW = 0.05 +L_SPEED_FAST = 0.1 +L_ACC_SLOW = 0.75 +L_ACC_FAST = 0.75 + +;Used with moveJ +J_SPEED_SLOW = 1 +J_SPEED_FAST = 1 +J_ACC_SLOW = 4 +J_ACC_FAST = 4 \ No newline at end of file diff --git a/dual_ursim_control/log_config.ini b/dual_ursim_control/log_config.ini new file mode 100644 index 0000000..da14e29 --- /dev/null +++ b/dual_ursim_control/log_config.ini @@ -0,0 +1,25 @@ +[loggers] +keys=root + +[handlers] +keys=fileHandler, consoleHandler + +[formatters] +keys=simpleFormatter + +[logger_root] +level=DEBUG +handlers=fileHandler, consoleHandler + +[handler_fileHandler] +class=FileHandler +formatter=simpleFormatter +args=("control.log",) + +[handler_consoleHandler] +class=StreamHandler +formatter=simpleFormatter +args=(sys.stdout,) + +[formatter_simpleFormatter] +format='%(asctime)s [%(levelname)s] %(threadName)-10s %(message)s' \ No newline at end of file diff --git a/dual_ursim_control/main.py b/dual_ursim_control/main.py new file mode 100644 index 0000000..867896d --- /dev/null +++ b/dual_ursim_control/main.py @@ -0,0 +1,876 @@ +#!/usr/bin/env python + +import threading +from os import wait, read + +import requests +import socket +import rtde_control +import time +import math +import openpyxl +import json +import logging +import logging.config +from configparser import ConfigParser +from pynput.keyboard import Listener, Key + +config_object = ConfigParser() +config_object.read('config.ini') +config = config_object['DEFAULT'] + +# TODO: export these to json? +# Config file name +CONFIG_FILE = 'path.xlsx' +CONFIG_FILE_SHEET_NAME = 'Configurations' +CONFIG_FILE_SHEET_MASTER = 'M' +CONFIG_FILE_SHEET_SLAVE = 'S' + +j_speed = 0 +j_acc = 0 + +l_speed = 0 +l_acc = 0 + +jogging_wait = {"Master": False, "Slave": False} + +path_master = {} +path_slave = {} + + +def write_paths_to_json(path_master, path_slave): + with open('waypoints_master.json', 'w') as json_file: + json.dump(path_master, json_file, indent=4) + + with open('waypoints_slave.json', 'w') as json_file: + json.dump(path_slave, json_file, indent=4) + + +def read_paths_from_json(): + with open('waypoints_master.json', 'r') as json_file: + path_master = json.load(json_file) + + with open('waypoints_slave.json', 'r') as json_file: + path_slave = json.load(json_file) + + return path_master, path_slave + + +def read_path_waypoints_from_file(): + # This reads the master and slave sheets + # Data only, because otherwise you get references, or sums + wb = openpyxl.load_workbook(CONFIG_FILE, data_only=True) + sheet = wb[CONFIG_FILE_SHEET_MASTER] + + # X (mm) Y (mm) Z (mm) RX (rad) Ry (rad) Rz (rad) + # Must divide x,y,z with magic number= 1000 + + # J1 (deg) J2 (deg) J3 (deg) J4 (deg) J5 (deg) J6 (deg) + # Must convert to radian for some reason + + path_master['CM0_P'] = [sheet['B2'].value, sheet['C2'].value, sheet['D2'].value, + sheet['E2'].value, sheet['F2'].value, sheet['G2'].value] + path_master['CM0_P'][0:3] = [x / 1000.0 for x in path_master['CM0_P'][0:3]] + logging.debug(f"CM0_P: {path_master['CM0_P']}") + + path_master['CM0_J'] = [sheet['H2'].value, sheet['I2'].value, sheet['J2'].value, + sheet['K2'].value, sheet['L2'].value, sheet['M2'].value] + path_master['CM0_J'] = [math.radians(x) for x in path_master['CM0_J']] + logging.debug(f"CM0_J: {path_master['CM0_J']}") + + path_master['CM1_P'] = [sheet['B3'].value, sheet['C3'].value, sheet['D3'].value, + sheet['E3'].value, sheet['F3'].value, sheet['G3'].value] + path_master['CM1_P'][0:3] = [x / 1000.0 for x in path_master['CM1_P'][0:3]] + logging.debug(f"CM1_P: {path_master['CM1_P']}") + + path_master['CM1_J'] = [sheet['H3'].value, sheet['I3'].value, sheet['J3'].value, + sheet['K3'].value, sheet['L3'].value, sheet['M3'].value] + path_master['CM1_J'] = [math.radians(x) for x in path_master['CM1_J']] + logging.debug(f"CM1_J: {path_master['CM1_J']}") + + path_master['CM2_P'] = [sheet['B4'].value, sheet['C4'].value, sheet['D4'].value, + sheet['E4'].value, sheet['F4'].value, sheet['G4'].value] + path_master['CM2_P'][0:3] = [x / 1000.0 for x in path_master['CM2_P'][0:3]] + logging.debug(f"CM2_P: {path_master['CM2_P']}") + + path_master['CM2_J'] = [sheet['H4'].value, sheet['I4'].value, sheet['J4'].value, + sheet['K4'].value, sheet['L4'].value, sheet['M4'].value] + path_master['CM2_J'] = [math.radians(x) for x in path_master['CM2_J']] + logging.debug(f"CM2_J: {path_master['CM2_J']}") + + path_master['CM3_P'] = [sheet['B5'].value, sheet['C5'].value, sheet['D5'].value, + sheet['E5'].value, sheet['F5'].value, sheet['G5'].value] + path_master['CM3_P'][0:3] = [x / 1000.0 for x in path_master['CM3_P'][0:3]] + logging.debug(f"CM3_P: {path_master['CM3_P']}") + + path_master['CM3_J'] = [sheet['H5'].value, sheet['I5'].value, sheet['J5'].value, + sheet['K5'].value, sheet['L5'].value, sheet['M5'].value] + path_master['CM3_J'] = [math.radians(x) for x in path_master['CM3_J']] + logging.debug(f"CM3_J: {path_master['CM3_J']}") + + path_master['CM4_P'] = [sheet['B6'].value, sheet['C6'].value, sheet['D6'].value, + sheet['E6'].value, sheet['F6'].value, sheet['G6'].value] + path_master['CM4_P'][0:3] = [x / 1000.0 for x in path_master['CM4_P'][0:3]] + logging.debug(f"CM4_P: {path_master['CM4_P']}") + + path_master['CM4_J'] = [sheet['H6'].value, sheet['I6'].value, sheet['J6'].value, + sheet['K6'].value, sheet['L6'].value, sheet['M6'].value] + path_master['CM4_J'] = [math.radians(x) for x in path_master['CM4_J']] + logging.debug(f"CM4_J: {path_master['CM4_J']}") + + path_master['CM5_P'] = [sheet['B7'].value, sheet['C7'].value, sheet['D7'].value, + sheet['E7'].value, sheet['F7'].value, sheet['G7'].value] + path_master['CM5_P'][0:3] = [x / 1000.0 for x in path_master['CM5_P'][0:3]] + logging.debug(f"CM5_P: {path_master['CM5_P']}") + + path_master['CM5_J'] = [sheet['H7'].value, sheet['I7'].value, sheet['J7'].value, + sheet['K7'].value, sheet['L7'].value, sheet['M7'].value] + path_master['CM5_J'] = [math.radians(x) for x in path_master['CM5_J']] + logging.debug(f"CM5_J: {path_master['CM5_J']}") + + path_master['CM6_P'] = [sheet['B8'].value, sheet['C8'].value, sheet['D8'].value, + sheet['E8'].value, sheet['F8'].value, sheet['G8'].value] + path_master['CM6_P'][0:3] = [x / 1000.0 for x in path_master['CM6_P'][0:3]] + logging.debug(f"CM6_P: {path_master['CM6_P']}") + + path_master['CM6_J'] = [sheet['H8'].value, sheet['I8'].value, sheet['J8'].value, + sheet['K8'].value, sheet['L8'].value, sheet['M8'].value] + path_master['CM6_J'] = [math.radians(x) for x in path_master['CM6_J']] + logging.debug(f"CM6_J: {path_master['CM6_J']}") + + path_master['CM7_P'] = [sheet['B9'].value, sheet['C9'].value, sheet['D9'].value, + sheet['E9'].value, sheet['F9'].value, sheet['G9'].value] + path_master['CM7_P'][0:3] = [x / 1000.0 for x in path_master['CM7_P'][0:3]] + logging.debug(f"CM7_P: {path_master['CM7_P']}") + + path_master['CM7_J'] = [sheet['H9'].value, sheet['I9'].value, sheet['J9'].value, + sheet['K9'].value, sheet['L9'].value, sheet['M9'].value] + path_master['CM7_J'] = [math.radians(x) for x in path_master['CM7_J']] + logging.debug(f"CM7_J: {path_master['CM7_J']}") + + # TODO: add new master waypoints here + # path_master['CM8_P'] = [sheet['B10'].value, sheet['C10'].value, sheet['D10'].value, + # sheet['E10'].value, sheet['F10'].value, sheet['G10'].value] + # path_master['CM8_P'][0:3] = [x / 1000.0 for x in path_master['CM8_P'][0:3]] + # logging.debug(f"CM8_P: {path_master['CM8_P']}") + # + # path_master['CM8_J'] = [sheet['H10'].value, sheet['I10'].value, sheet['J10'].value, + # sheet['K10'].value, sheet['L10'].value, sheet['M10'].value] + # path_master['CM8_J'] = [math.radians(x) for x in path_master['CM8_J']] + # logging.debug(f"CM8_J: {path_master['CM8_J']}") + + # Change the sheet + sheet = wb[CONFIG_FILE_SHEET_SLAVE] + + path_slave['CS0_P'] = [sheet['B2'].value, sheet['C2'].value, sheet['D2'].value, + sheet['E2'].value, sheet['F2'].value, sheet['G2'].value] + path_slave['CS0_P'][0:3] = [x / 1000.0 for x in path_slave['CS0_P'][0:3]] + logging.debug(f"CS0_P: {path_slave['CS0_P']}") + + path_slave['CS0_J'] = [sheet['H2'].value, sheet['I2'].value, sheet['J2'].value, + sheet['K2'].value, sheet['L2'].value, sheet['M2'].value] + path_slave['CS0_J'] = [math.radians(x) for x in path_slave['CS0_J']] + logging.debug(f"CS0_J: {path_slave['CS0_J']}") + + path_slave['CS1_P'] = [sheet['B3'].value, sheet['C3'].value, sheet['D3'].value, + sheet['E3'].value, sheet['F3'].value, sheet['G3'].value] + path_slave['CS1_P'][0:3] = [x / 1000.0 for x in path_slave['CS1_P'][0:3]] + logging.debug(f"CS1_P: {path_slave['CS1_P']}") + + path_slave['CS1_J'] = [sheet['H3'].value, sheet['I3'].value, sheet['J3'].value, + sheet['K3'].value, sheet['L3'].value, sheet['M3'].value] + path_slave['CS1_J'] = [math.radians(x) for x in path_slave['CS1_J']] + logging.debug(f"CS1_J: {path_slave['CS1_J']}") + + path_slave['CS2_P'] = [sheet['B4'].value, sheet['C4'].value, sheet['D4'].value, + sheet['E4'].value, sheet['F4'].value, sheet['G4'].value] + path_slave['CS2_P'][0:3] = [x / 1000.0 for x in path_slave['CS2_P'][0:3]] + logging.debug(f"CS2_P: {path_slave['CS2_P']}") + + path_slave['CS2_J'] = [sheet['H4'].value, sheet['I4'].value, sheet['J4'].value, + sheet['K4'].value, sheet['L4'].value, sheet['M4'].value] + path_slave['CS2_J'] = [math.radians(x) for x in path_slave['CS2_J']] + logging.debug(f"CS2_J: {path_slave['CS2_J']}") + + path_slave['CS3_P'] = [sheet['B5'].value, sheet['C5'].value, sheet['D5'].value, + sheet['E5'].value, sheet['F5'].value, sheet['G5'].value] + path_slave['CS3_P'][0:3] = [x / 1000.0 for x in path_slave['CS3_P'][0:3]] + logging.debug(f"CS3_P: {path_slave['CS3_P']}") + + path_slave['CS3_J'] = [sheet['H5'].value, sheet['I5'].value, sheet['J5'].value, + sheet['K5'].value, sheet['L5'].value, sheet['M5'].value] + path_slave['CS3_J'] = [math.radians(x) for x in path_slave['CS3_J']] + logging.debug(f"CS3_J: {path_slave['CS3_J']}") + + path_slave['CS4_P'] = [sheet['B6'].value, sheet['C6'].value, sheet['D6'].value, + sheet['E6'].value, sheet['F6'].value, sheet['G6'].value] + path_slave['CS4_P'][0:3] = [x / 1000.0 for x in path_slave['CS4_P'][0:3]] + logging.debug(f"CS4_P: {path_slave['CS4_P']}") + + path_slave['CS4_J'] = [sheet['H6'].value, sheet['I6'].value, sheet['J6'].value, + sheet['K6'].value, sheet['L6'].value, sheet['M6'].value] + path_slave['CS4_J'] = [math.radians(x) for x in path_slave['CS4_J']] + logging.debug(f"CS4_J: {path_slave['CS4_J']}") + + path_slave['CS5_P'] = [sheet['B7'].value, sheet['C7'].value, sheet['D7'].value, + sheet['E7'].value, sheet['F7'].value, sheet['G7'].value] + path_slave['CS5_P'][0:3] = [x / 1000.0 for x in path_slave['CS5_P'][0:3]] + logging.debug(f"CS5_P: {path_slave['CS5_P']}") + + path_slave['CS5_J'] = [sheet['H7'].value, sheet['I7'].value, sheet['J7'].value, + sheet['K7'].value, sheet['L7'].value, sheet['M7'].value] + path_slave['CS5_J'] = [math.radians(x) for x in path_slave['CS5_J']] + logging.debug(f"CS5_J: {path_slave['CS5_J']}") + + # TODO: add new slave waypoints here + path_slave['CS6_P'] = [sheet['B8'].value, sheet['C8'].value, sheet['D8'].value, + sheet['E8'].value, sheet['F8'].value, sheet['G8'].value] + path_slave['CS6_P'][0:3] = [x / 1000.0 for x in path_slave['CS6_P'][0:3]] + logging.debug(f"CS6_P: {path_slave['CS6_P']}") + + path_slave['CS6_J'] = [sheet['H8'].value, sheet['I8'].value, sheet['J8'].value, + sheet['K8'].value, sheet['L8'].value, sheet['M8'].value] + path_slave['CS6_J'] = [math.radians(x) for x in path_slave['CS6_J']] + logging.debug(f"CS6_J: {path_slave['CS6_J']}") + + path_slave['CS7_P'] = [sheet['B9'].value, sheet['C9'].value, sheet['D9'].value, + sheet['E9'].value, sheet['F9'].value, sheet['G9'].value] + path_slave['CS7_P'][0:3] = [x / 1000.0 for x in path_slave['CS7_P'][0:3]] + logging.debug(f"CS7_P: {path_slave['CS7_P']}") + + path_slave['CS7_J'] = [sheet['H9'].value, sheet['I9'].value, sheet['J9'].value, + sheet['K9'].value, sheet['L9'].value, sheet['M9'].value] + path_slave['CS7_J'] = [math.radians(x) for x in path_slave['CS7_J']] + logging.debug(f"CS7_J: {path_slave['CS7_J']}") + + return path_master, path_slave + + +def on_press(key): + try: + # logging.debug("Key pressed: {0}".format(key)) + + if hasattr(key, 'char'): + if key.char == 'f': + jogging_wait['Master'] = True + + elif key.char == 'e': + jogging_wait['Slave'] = True + + except AttributeError: + logging.debug("Special key pressed: {0}".format(key)) + + +def on_release(key): + try: + # logging.debug("Key released: {0}".format(key)) + + if hasattr(key, 'char'): + if key.char == 'f': + jogging_wait['Master'] = False + + elif key.char == 'e': + jogging_wait['Slave'] = False + + except AttributeError: + logging.debug("Special key pressed: {0}".format(key)) + + +def master_jogging_wait(): + if config.getboolean('JOGGING_ENABLE'): + logging.debug("Master-Fred: Press 'f' to continue") + + while not jogging_wait['Master']: + pass + + +def slave_jogging_wait(): + if config.getboolean('JOGGING_ENABLE'): + logging.debug("Slave-Erik: Press 'e' to continue") + + while not jogging_wait['Slave']: + pass + + +def master_operate_gripper(target_width): + # https://github.com/gouxiangchen/UR5-control-with-RG2/blob/master/test_main.py + + if target_width < 10 or target_width > 110: + logging.debug('Gripper width out of bounds') + return + + port = 30001 + host = config['MASTER_IP'] + tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + tcp_socket.connect((host, port)) + + tcp_command = "def rg2ProgOpen():\n" + tcp_command += "\ttextmsg(\"inside RG2 function called\")\n" + + tcp_command += '\ttarget_width={}\n'.format(target_width) + tcp_command += "\ttarget_force=40\n" + tcp_command += "\tpayload=1.0\n" + tcp_command += "\tset_payload1=False\n" + tcp_command += "\tdepth_compensation=False\n" + tcp_command += "\tslave=False\n" + + tcp_command += "\ttimeout = 0\n" + tcp_command += "\twhile get_digital_in(9) == False:\n" + tcp_command += "\t\ttextmsg(\"inside while\")\n" + tcp_command += "\t\tif timeout > 400:\n" + tcp_command += "\t\t\tbreak\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\ttimeout = timeout+1\n" + tcp_command += "\t\tsync()\n" + tcp_command += "\tend\n" + tcp_command += "\ttextmsg(\"outside while\")\n" + + tcp_command += "\tdef bit(input):\n" + tcp_command += "\t\tmsb=65536\n" + tcp_command += "\t\tlocal i=0\n" + tcp_command += "\t\tlocal output=0\n" + tcp_command += "\t\twhile i<17:\n" + tcp_command += "\t\t\tset_digital_out(8,True)\n" + tcp_command += "\t\t\tif input>=msb:\n" + tcp_command += "\t\t\t\tinput=input-msb\n" + tcp_command += "\t\t\t\tset_digital_out(9,False)\n" + tcp_command += "\t\t\telse:\n" + tcp_command += "\t\t\t\tset_digital_out(9,True)\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\t\tif get_digital_in(8):\n" + tcp_command += "\t\t\t\tout=1\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\t\tsync()\n" + tcp_command += "\t\t\tset_digital_out(8,False)\n" + tcp_command += "\t\t\tsync()\n" + tcp_command += "\t\t\tinput=input*2\n" + tcp_command += "\t\t\toutput=output*2\n" + tcp_command += "\t\t\ti=i+1\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\treturn output\n" + tcp_command += "\tend\n" + tcp_command += "\ttextmsg(\"outside bit definition\")\n" + + tcp_command += "\ttarget_width=target_width+0.0\n" + tcp_command += "\tif target_force>40:\n" + tcp_command += "\t\ttarget_force=40\n" + tcp_command += "\tend\n" + + tcp_command += "\tif target_force<4:\n" + tcp_command += "\t\ttarget_force=4\n" + tcp_command += "\tend\n" + tcp_command += "\tif target_width>110:\n" + tcp_command += "\t\ttarget_width=110\n" + tcp_command += "\tend\n" + tcp_command += "\tif target_width<0:\n" + tcp_command += "\t\ttarget_width=0\n" + tcp_command += "\tend\n" + tcp_command += "\trg_data=floor(target_width)*4\n" + tcp_command += "\trg_data=rg_data+floor(target_force/2)*4*111\n" + tcp_command += "\tif slave:\n" + tcp_command += "\t\trg_data=rg_data+16384\n" + tcp_command += "\tend\n" + + tcp_command += "\ttextmsg(\"about to call bit\")\n" + tcp_command += "\tbit(rg_data)\n" + tcp_command += "\ttextmsg(\"called bit\")\n" + + tcp_command += "\tif depth_compensation:\n" + tcp_command += "\t\tfinger_length = 55.0/1000\n" + tcp_command += "\t\tfinger_heigth_disp = 5.0/1000\n" + tcp_command += "\t\tcenter_displacement = 7.5/1000\n" + + tcp_command += "\t\tstart_pose = get_forward_kin()\n" + tcp_command += "\t\tset_analog_inputrange(2, 1)\n" + tcp_command += "\t\tzscale = (get_analog_in(2)-0.026)/2.976\n" + tcp_command += "\t\tzangle = zscale*1.57079633-0.087266462\n" + tcp_command += "\t\tzwidth = 5+110*sin(zangle)\n" + + tcp_command += "\t\tstart_depth = cos(zangle)*finger_length\n" + + tcp_command += "\t\tsync()\n" + tcp_command += "\t\tsync()\n" + tcp_command += "\t\ttimeout = 0\n" + + tcp_command += "\t\twhile get_digital_in(9) == True:\n" + tcp_command += "\t\t\ttimeout=timeout+1\n" + tcp_command += "\t\t\tsync()\n" + tcp_command += "\t\t\tif timeout > 20:\n" + tcp_command += "\t\t\t\tbreak\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\ttimeout = 0\n" + tcp_command += "\t\twhile get_digital_in(9) == False:\n" + tcp_command += "\t\t\tzscale = (get_analog_in(2)-0.026)/2.976\n" + tcp_command += "\t\t\tzangle = zscale*1.57079633-0.087266462\n" + tcp_command += "\t\t\tzwidth = 5+110*sin(zangle)\n" + tcp_command += "\t\t\tmeasure_depth = cos(zangle)*finger_length\n" + tcp_command += "\t\t\tcompensation_depth = (measure_depth - start_depth)\n" + tcp_command += "\t\t\ttarget_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0])\n" + tcp_command += "\t\t\tif timeout > 400:\n" + tcp_command += "\t\t\t\tbreak\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\t\ttimeout=timeout+1\n" + tcp_command += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\tnspeed = norm(get_actual_tcp_speed())\n" + tcp_command += "\t\twhile nspeed > 0.001:\n" + tcp_command += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n" + tcp_command += "\t\t\tnspeed = norm(get_actual_tcp_speed())\n" + tcp_command += "\t\tend\n" + tcp_command += "\tend\n" + tcp_command += "\tif depth_compensation==False:\n" + tcp_command += "\t\ttimeout = 0\n" + tcp_command += "\t\twhile get_digital_in(9) == True:\n" + tcp_command += "\t\t\ttimeout = timeout+1\n" + tcp_command += "\t\t\tsync()\n" + tcp_command += "\t\t\tif timeout > 20:\n" + tcp_command += "\t\t\t\tbreak\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\ttimeout = 0\n" + tcp_command += "\t\twhile get_digital_in(9) == False:\n" + tcp_command += "\t\t\ttimeout = timeout+1\n" + tcp_command += "\t\t\tsync()\n" + tcp_command += "\t\t\tif timeout > 400:\n" + tcp_command += "\t\t\t\tbreak\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\tend\n" + tcp_command += "\tend\n" + tcp_command += "\tif set_payload1:\n" + tcp_command += "\t\tif slave:\n" + tcp_command += "\t\t\tif get_analog_in(3) < 2:\n" + tcp_command += "\t\t\t\tzslam=0\n" + tcp_command += "\t\t\telse:\n" + tcp_command += "\t\t\t\tzslam=payload\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\telse:\n" + tcp_command += "\t\t\tif get_digital_in(8) == False:\n" + tcp_command += "\t\t\t\tzmasm=0\n" + tcp_command += "\t\t\telse:\n" + tcp_command += "\t\t\t\tzmasm=payload\n" + tcp_command += "\t\t\tend\n" + tcp_command += "\t\tend\n" + tcp_command += "\t\tzsysm=0.0\n" + tcp_command += "\t\tzload=zmasm+zslam+zsysm\n" + tcp_command += "\t\tset_payload(zload)\n" + tcp_command += "\tend\n" + + tcp_command += "end\n" + + tcp_socket.send(str.encode(tcp_command)) + tcp_socket.close() + time.sleep(1) + + +def master_gripper_toggle(state='Open'): + logging.info(f'Master RG2 gripper state: {state} ') + # This function accepts either 'Open' or 'Close' + + if state == 'Open': + master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_OPENED')) + + elif state == 'Close': + master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_CLOSED')) + + else: + logging.info('Invalid gripper option given') + return + + +def master_connect(): + try: + robot = rtde_control.RTDEControlInterface(config['MASTER_IP']) + + except: + text = f"Cannot connect to robot at: {config['MASTER_IP']}" + + logging.info(text) + exit(text) + + logging.info(f"Connected to robot at: {config['MASTER_IP']}") + + # robot.setPayload(1, (0, 0, 0.01)) + return robot + + +def slave_connect(): + try: + robot = rtde_control.RTDEControlInterface(config['SLAVE_IP']) + + except: + text = f"Cannot connect to robot at: {config['SLAVE_IP']}" + + logging.info(text) + exit(text) + + logging.info(f"Connected to robot at: {config['SLAVE_IP']}") + + # robot.setPayload(1, (0, 0, 0.01)) + return robot + + +def master_disconnect(robot): + robot.disconnect() + logging.info('Disconnected master robot') + # time.sleep(1) + + +def slave_disconnect(robot): + robot.disconnect() + logging.info('Disconnected master robot') + # time.sleep(1) + + +def slave_gripper_toggle(state='Open'): + logging.info(f'Slave RG2-FT gripper state: {state} ') + # This function accepts either 'Open' or 'Close' + + ip = config['SLAVE_GRIPPER_IP'] + + if state == 'Open': + width = config.getint('SLAVE_GRIPPER_WIDTH_OPENED') + + elif state == 'Close': + width = config.getint('SLAVE_GRIPPER_WIDTH_CLOSED') + else: + # log error + logging.info('Invalid gripper option given') + return + + curl_text = f'http://{ip}/api/dc/rg2ft/set_width/{width}/40' + logging.debug(f"Sending command to gripper: {curl_text}") + requests.get(curl_text) + + time.sleep(1) + + +def master_movel_time_based(pose, a=1, v=0.06, t=2): + message = f"movel(p{pose}, a={a}, v={v}, t={t}, r=0)\n" + logging.info(f'Time-based movel: {message}') + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.connect((config['MASTER_IP'], 30001)) + + s.send(str.encode(message)) + s.close() + time.sleep(t + 0.05) + return + + +def slave_movel_time_based(pose, a=1, v=0.06, t=2): + message = f"movel(p{pose}, a={a}, v={v}, t={t}, r=0)\n" + logging.info(f'Time-based movel: {message}') + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.connect((config['SlAVE_IP'], 30001)) + + s.send(str.encode(message)) + s.close() + time.sleep(t + 0.05) + return + + +# def move_marcell_sync_begin(): +# +# message = "movel(p[0.116, -0.394, 0.0, 0.608, -1.48, 0.621], a=1, v=0.06, t=5, r=0)\n" +# logging.debug(message) +# s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +# s.connect((config['MASTER_IP'], 30001)) +# +# s.send(str.encode(message)) +# s.close() +# time.sleep(5) +# return +# +# def move_marcell_snync_end(): +# message = "movel(p[0.066, -0.444, 0.0, 0.572, -1.49, 0.585], a=1, v=0.06, t=5, r=0)\n" +# +# logging.debug(message) +# s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +# s.connect((config['MASTER_IP'], 30001)) +# +# s.send(str.encode(message)) +# s.close() +# time.sleep(5) +# +# return + + +def master_thread(sync_event, path): + logging.info(f'Starting master') + + robot = master_connect() + + # master_movel_time_based(pose=path['CM5_P'], t=5) + + sync_event['Master'].clear() + + master_disconnect(robot) + master_gripper_toggle('Open') + robot = master_connect() + + master_jogging_wait() + robot.moveJ(path['CM1_J'], j_speed, j_acc) + logging.info(f'CM1: M Start Up') + + # master_jogging_wait() + # robot.moveL(path['CM8_P'], j_speed, j_acc) + # logging.info(f'CM8') + + master_disconnect(robot) + master_gripper_toggle('Open') + robot = master_connect() + + master_jogging_wait() + robot.moveL(path['CM2_P'], l_speed, l_acc) + logging.info(f'CM2: M Start Down') + + master_disconnect(robot) + master_gripper_toggle('Close') + robot = master_connect() + + master_jogging_wait() + robot.moveL(path['CM1_P'], l_speed, l_acc) + logging.info(f'CM1: M Start Up') + + master_jogging_wait() + robot.moveJ(path['CM3_J'], j_speed, j_acc) + logging.info(f'CM3: M Start Oriented') + + master_jogging_wait() + robot.moveJ(path['CM4_J'], j_speed, j_acc) + logging.info(f'CM4: M Sync Begin') + + if config.getboolean('SLAVE_ENABLE'): + sync_event['Master'].set() + logging.info('Signaling to slave') + + logging.debug('Waiting for slave') + sync_event['Slave'].wait() + sync_event['Slave'].clear() + + # time.sleep(0.1) + + master_jogging_wait() + robot.moveL(path['CM5_P'], l_speed * 0.64, l_acc) + # master_disconnect(robot) + # master_movel_time_based(pose=path['CM5_P'], t=3) + # robot = master_connect() + logging.info(f'CM5: M Sync End') + + # ez csak tesztre + # master_jogging_wait() + # # robot.moveL(path['CM4_P'], l_speed, l_acc) + # master_disconnect(robot) + # master_movel_time_based(pose=path['CM4_P'], t=5) + # logging.info(f'CM4: M Sync Begin') + # robot = master_connect() + # return + + if config.getboolean('SLAVE_ENABLE'): + logging.debug('Waiting for slave') + + sync_event['Slave'].wait() + sync_event['Slave'].clear() + + master_jogging_wait() + robot.moveJ(path['CM6_J'], j_speed, j_acc) + logging.info(f'CM6: M Goal Up') + + master_jogging_wait() + robot.moveL(path['CM7_P'], l_speed, l_acc) + logging.info(f'CM7: M Goal Down') + + master_disconnect(robot) + master_gripper_toggle('Open') + robot = master_connect() + + master_jogging_wait() + robot.moveL(path['CM6_P'], l_speed, l_acc) + logging.info(f'CM6: M Goal Up') + + master_disconnect(robot) + master_gripper_toggle('Close') + robot = master_connect() + + master_jogging_wait() + robot.moveL(path['CM1_P'], l_speed, l_acc) + logging.info(f'CM1: M Start Up') + + # Stop the control script + robot.stopScript() + + logging.info('Done') + + +def slave_thread(sync_event, path): + logging.info(f'Starting slave') + + robot = slave_connect() + + sync_event['Slave'].clear() + + slave_gripper_toggle('Open') + + slave_jogging_wait() + robot.moveJ(path['CS1_J'], j_speed, j_acc) + logging.info(f'CS1: S Start Up') + + slave_jogging_wait() + robot.moveL(path['CS2_P'], l_speed, l_acc) + logging.info(f'CS2: S Start Down') + + slave_gripper_toggle('Close') + + slave_jogging_wait() + robot.moveL(path['CS1_P'], l_speed, l_acc) + logging.info(f'CS1: S Start Up') + + slave_jogging_wait() + robot.moveJ(path['CS3_J'], j_speed, j_acc) + logging.info(f'CS3: S Start Oriented') + + slave_jogging_wait() + robot.moveJ(path['CS4_J'], j_speed, j_acc) + logging.info(f'CS4: S Sync Begin') + + time.sleep(0.5) + + if config.getboolean('MASTER_ENABLE'): + logging.debug('Waiting for master') + + sync_event['Master'].wait() + sync_event['Master'].clear() + + slave_jogging_wait() + robot.moveL(path['CS5_P'], l_speed, l_acc) + # robot.disconnect() + # slave_movel_time_based(pose=path['CS5_P'], t=3) + # robot = slave_connect() + logging.info(f'CS5: S Sync End') + + if config.getboolean('MASTER_ENABLE'): + sync_event['Slave'].set() + logging.debug('Signaling to master') + + # time.sleep(0.09) + + slave_jogging_wait() + robot.moveL(path['CS6_P'], l_speed, l_acc) + logging.info(f'CS6: S Sync Begin Rotated') + + # slave_jogging_wait() + # robot.moveL(path['CS4_P'], l_speed, l_acc) + # # robot.disconnect() + # # slave_movel_time_based(pose=path['CS4_P'], t=3) + # # robot = slave_connect() + # logging.info(f'CS4: S Sync Begin') + + # slave_jogging_wait() + # robot.moveJ(path['CS6_J'], j_speed, j_acc) + # # # # robot.disconnect() + # # # # slave_movel_time_based(pose=path['CS6_P'], t=8) + # # # # robot = slave_connect() + # logging.info(f'CS6: S Sync Begin Rotated') + + slave_gripper_toggle('Open') + + slave_jogging_wait() + robot.moveL(path['CS7_P'], l_speed, l_acc) + logging.info(f'CS7: S Sync Retreat') + + slave_jogging_wait() + robot.moveJ(path['CS3_J'], j_speed, j_acc) + logging.info(f'CS3: S Start Oriented') + + if config.getboolean('MASTER_ENABLE'): + sync_event['Slave'].set() + logging.debug('Signaling to master') + + # slave_jogging_wait() + # # robot.moveL(path['CS6_P'], l_speed, l_acc) + # robot.disconnect() + # slave_movel_time_based(pose=path['CS6_P'], t=8) + # robot = slave_connect() + # logging.info(f'CS6: S Sync Begin Rotated') + + # csak mikor nemkell forgas + + # ez csak teszt + # slave_jogging_wait() + # robot.moveL(path['CS4_P'], l_speed, l_acc) + # logging.info(f'CS4: S Sync Begin') + # return + + slave_jogging_wait() + robot.moveJ(path['CS1_J'], j_speed, j_acc) + logging.info(f'CS1: S Start Up') + + # Stop the control script + robot.stopScript() + + logging.info('Done') + + +if __name__ == '__main__': + + logging.config.fileConfig('log_config.ini') + + if config['EXECUTION'] == 'SLOW': + j_speed = config.getfloat('J_SPEED_SLOW') + l_speed = config.getfloat('L_SPEED_SLOW') + + j_acc = config.getfloat('J_ACC_SLOW') + l_acc = config.getfloat('L_ACC_SLOW') + + logging.info("Execution speed: slow") + + elif config['EXECUTION'] == 'FAST': + j_speed = config.getfloat('J_SPEED_FAST') + l_speed = config.getfloat('L_SPEED_FAST') + + j_acc = config.getfloat('J_ACC_FAST') + l_acc = config.getfloat('L_ACC_FAST') + + logging.info("Execution speed: fast") + + if config.getboolean('JOGGING_ENABLE'): + logging.info('Jogging enabled') + + # TODO: eliminate excel + # Reads from m and s sheets + path_master, path_slave = read_path_waypoints_from_file() + # write_paths_to_json(path_master, path_slave) + + # TODO: Prefer this + # path_master, path_slave = read_paths_from_json() + + sync_event = { + 'Master': threading.Event(), + 'Slave': threading.Event() + } + + logging.info('Starting keyboard listener') + + listener = Listener(on_press=on_press, on_release=on_release) + listener.start() + + if config.getboolean('MASTER_ENABLE'): + master_thread = threading.Thread(name='MasterThread', target=master_thread, args=(sync_event, path_master)) + master_thread.start() + + if config.getboolean('SLAVE_ENABLE'): + slave_thread = threading.Thread(name='SlaveThread', target=slave_thread, args=(sync_event, path_slave)) + slave_thread.start() + + if config.getboolean('MASTER_ENABLE'): + master_thread.join() + + if config.getboolean('SLAVE_ENABLE'): + slave_thread.join() + + listener.stop() + listener.join() + logging.info("Done") + +# TODO: export from ursimulator: root@xu:/var/snap/docker/common/var-lib-docker/volumes/dockursim/_data/programs.UR3# + diff --git a/dual_ursim_control/path.xlsx b/dual_ursim_control/path.xlsx new file mode 100644 index 0000000000000000000000000000000000000000..a1a10422aacd914c218a7f3ef32e86f30d6c470e GIT binary patch literal 13345 zcmbVzWl&sOvo`J)Tm~n&>)@8)76|Shd~mnmL4rF355aYCcMt9s+}(nGaNb+@$HG6i|ba$;@y}O^a*Rzylp<%Eg5D*X`X7uEhA^s5Lm*4shrq+%u%&*_2u^kFs ztSG@Jo?qy1xRyl_vJ0Bzq?*V%N!qDwiE6`gy93Uz-(g{*L{@c0+57lD8Rf6Y*)p!M zQp@UvscMEnwNM*-fA1c8vUg|U9?9|PeFKzKwiBm!_^K##IlL}9nCIyfErYasAul@) zxnq220wE|8;-IVQ%Kl^$x*e!_7=dYTu9h?B3)37ZZ)3$}D@_XU5aRfyP7z1$(k~;I z>-8PjjEQO*jO4yz>fJE3dJ>ODfWj&z|0aoi`?w!>vM!4JF@9(-IT@?5VcP*@bO|@o zj2Xyk?&K*?=`AI9{0)bgLtiFbEAwG!8%s$R9w8`|Gxhi-2%}IC5K8}Nn9yJT;bzU^ zYUf~OWM^l^>}F#fsj?WA#){#6qE6OEqvK$K8C0T-S;OGGXj#4N&CLqK(<=cUFfD%a zgvSD3=BScFEOIz{jSd^SJ2E#o!sMr5lb?l<*o)+`8yk_vmVZ4nkO0^e_R}Uoi8(;Z z+p3);()>URi-(6l2tt!Kl&b~lX5cC*3&!IhL~JT2Os%OM>FUyDr7%Ot4J@}`6xd3q zkTG?NJ)#E>J0^BY6gVSx){r3qx%&0+N+wKdhY3Qyt3`_!?_)8=xV&?hS(;`zVi}bF zdw$7#2lqqteZ1o@lPN#pn_k^PJB$efYQ2Dvkq)eYxF;MtFz%Te z{WV2UwpSAUfV)&gQF$Hthpq|V_b&3eBo;YMT^Af!DFQ(-l1_W2iY|A<>?&&z-*e~d z`I}(DEOJt2PXc67*#qK}h0$^Xj6*3Cy?RC;Xz6*OR%Q>Cq8C+6%xAob$GJa#Nk6ih z2DfHMR5qayXtd$W@v@I#Mh#DYHWXPl6%AODY&gMzv%`*hVqCgHl6?75ny3K5&DS%) z0o_kTp^1!r!n3Rty*h+KKGXk9=v&+4+y-e&3pU?%&s<&WocEgQ^(m*`#+>yfNU44& zYNdN%^lR!%(B}QU1#6_)qya|B<9Pc`U0g@VxEp(DoqYsHIQO`~lYbPr5ZNmyv5gza zcMQJY70+p!muk@w$;*bfA$g{iiL1_WW1r?Vr+8-zIPqTcX&?GO~)k-P+Xg zHKUHSSHI?RWB8ot`@5a|3JgS5jh#$nP}MJ*T+aOXV?O{Z9VVkP=4|Ed&FpNi2tuW! zX4RglC-oOUg0bP9u?~7aIhk(Ja?yzb>nR1IjkaxpW4>q2Mh&6_jK`m%LVuRf4w2#2Ss&r>inG$? zeF$zLhwxA8h_An(b07Q_fj1f}^hU_>QDU=jk~FLxH$q3UA^a0X)%29cnJu%^+6(#K)Ht^ zbP`rJ9|T*CeG>1cL1;(D0FyGPPoUSc`)D!p7Tj^%Ma=uw6{u zT0=%iZ~|yE^|_$gf8C(*AuyJ3lUhE|qByTk*Et!=GZW6+y$IwIW*NRHH~r(nqaB3_ zA3Ax(d+2ke0V6(g@8>;ev(42BIbOmg?9_4jY=RZc)KmU|vxOno+GM-(jeypJ+oOGa z52BwzbNY)pPnZjOWj*>_R+K{vrX{G0C8(oj^3TJMZPch56M9iK7xz0~#TITC15!t7 zNI3l89=$7wWU?5e8L3gO&U2PVX0+g$7eqag&|@*DQ6r)5Bz>rvrir+Lt^&|T#+ ze?C*!wQ%uGh{6~D4jJhBunMuB zM#7D*u6^sJ1+pU5NGL{-S1oaI-@tR{s&Ki$e4tkQls54XYG_kc%KxvWXJDHmgd( zx5?8fpSQc4?HOMT{PlB|6UAE?K`uff23OSN7ZU;mt2>8b9Bu)Sed3~?yAI4NuCU|( zLtfD|^hd$A9Ov}ZaP8Etdz>V`rFPR@)aD#DihQZTAaVYrg^9wRjh8;SNC+#pe=1-k zm6g+C-%caSkrUyv@P^cuxbEwZq+1{SeXa?KuOm0|g!4orgji5v9eHO%?t=#rt9pWE zp7-dQJI_$BWo{kXd7=ac0s@u)Kb1NBzgwr{CsR`=N0z@XY_AosMMvB9mjtHIs(!~h zU0O-t?*iGVqjXVxVB0!#ir`!*8Zp zdpG+;lkXkrYv zA@@-Ll}-k_@aeY`n?zBEZr_|!Q~i5^IpJOwmD06IzB%>t^AL6EnsmuUC1Lrv@8|1w zXW^vf<_nmqT34h`M3!-?=&`#)xoY+#WfKAYoVO1cWnJ$HnGcSHgz~MH2fFG;cHF!% zDp1rrCizA_{#@>nzIxEK@)BAyyVkLaIb%NPM4z=1RNw_LvLDta+8wa0Zn#}kAj~Yt z$Z6G{+_*d5a#rsM+~NvvnT@w;{hmFvxdrs;S4*boFr?*sVW)Vzie7KWYoWmV`}_vr zE-X2!`YP2BA3hI%r@B95HBH(5y<_Q6z-w`7odo{ra!aXI@O4Lg%tV>Kql$Tu^r~jK zb9h(F?(woIGh*M<;;FvkLcj}cmN27O#kZgbuZxJwqYXI8y}a{0R?>v#a&P@4lO6uu zo6t_C_6vbWMB)%g)+Ozx0r6>qZ{Ii9=IKP}F>x#!>`X3^vNTX~0Tw*3l2o|t5Bza= zzO7qTq$qm1mwdu1MNQwNtlLQKx|*%1c}4eJZkRZxaG;{P)XR6HP&QNlBBaDmULu0( zOpVrtf*R(dA!{#lg@M7p&h(>A)hIGIqAMXt9=C`5;Ea&0+~zmOYa-iXi50;L#G?*? z=>jfd6~G%zLT%T$x&^&i+61r~1b={oY+^RB^>pRIH*|w=^!zyZ07K_0nZM*pPH0l? zNu3rs0H^oJ1_^o6sxQ*qb*jdfh=!(%IL6_XuivhE3@!N6V`6y$Pp3}J)JnYX&hJ}= zJDdYR`s(hZ0ndJ_6Y%7oDHAk=to>vLO(DIrwG>W}lyI*#GQE6QGRhzU4mt5q= zN>kNW38`GjZc-@O*Me#Fb zf1c$)7L;RK6YhPPoX#5zJN%kG7-N)m|5j!;1k9hgg{Io=`Fd|y<(UvU$g~K*$O;Y< z9#aVW1Q`Bi;xH_83^$BoXEON0D}g8vv5OSOoMM&xeytTiYXwqD1!q|#Yv!qwm=TL5 zcEE&nwxj&$t|{$Zuw%6an&H&ZAKCgz(@4Id^rRzH?-f5(%LMUssd)$jAS>1hS1!)kSg}yXJ2X`k?UarN9z#-GWo@tnoA(uBMmc}aUahY#%|V&MuG>I z5&QeFM3Ub2^Bo$?p(`$E^!b{UaSg^gsNpo>*E#u63vSixl&iBk^HH3C!KS-Z57$md zwDCa4)&Clr{(<6+>iV1=ecTqm!3f>4`|NfsjN}BeQ4-~9y7Y9-x z3N^#Ul#`fjqIIp|4qQ*#$$Di@qV8%p7Av}d0otP5R@`r)%unI1d_!S&!kJaOrnArt zz)Z}Isp*h#0)PJ|cWgM@Iwk!2T*>`_C1_X&GGV%CWJDRipW7sepBFu;5NuCl$gpo! z$dU(Rv$z+W)vi`DU6>8s(9feFq>(YpA*OkDfwPs!nB^eos^etDP}Z(ita0MZrol$W;w#^{FD} zy%H`Xr74&1x(xXO5Wea4?-teSgnP?;xS1 zE&8NFMs)s-7TSUQY=sg z!FN!77)iEt@tnH#0pjf_X1R_8u2js6l7Uls^r1|U@Fz`P{k$E#@R)H>4bjQ<+wfK@ zSb-LebbCLVBN9D}_GRNr-qA#t6qY5~6d0Jxwns=j7U$6iZ4~&)y&be*CTU0YAId?f znU`7{~9VMhE9!QMWPATb%9p!lB^(LE5NuxyJfjawMsB!ddhidX! z3|aD7LY6Ij1Y5(mAR=Hqqm;ZfEG#!8n(l{s0a}sFL?~rZe}VC7Bfya*n*s-xM6#m{ zV9yd!XWPLlPq=S_q-A`z?@V%ok_d^*9rk7-O*$7F`&0xo%{m8XRY zc>te+sdG-!TTo3>zF%HaPsq3hi!8vKriHOLMlTUV(nAwU(o+-V@UlvfwDg-p6L?E> zb1QvLc4MN(`bQidbuvwMQ{(F+jQN9D)Fo=pF$JX4vF_hI|0ab)*-VYsMP~q@h{)8o zrC8-Q2H(}!TzE5kQ(!>Q&casmZvAj%!nRdmTvw99AC-wNnU1w#5RtbYg*;)i_*G$lNd|A2W#w!XFM+ial-^TvW z9x5FGJrJi~uf77OHqIzurnBf?(0?~3K{ zsJ&361b%&6-q{sTdc{u}Z`v0R?7n5Sd9%BF2;#8eqN$Nk`*uPO@=%7Le-m1|Agu{t z=Nx%TcqUjqIL^rLf`99zWQ4rVs|?;G%bAsT*RNqJ$#FFF_B_}y#lb~<#Ox|n5okMf zKCoeADiIzYmyr|qEnB!e(eYT-^KCxu)TL?b=Dfr!{e9yx)~WH!c{bQECGdcGspJMl zUrm$Qiddr!xvCqPy=3|plevGYy~q(0){RUN{t_% zUb=pP%UmMvurI{-`FamtuM~6M(6|6gjyy=2#ECpEKw!7bv@JZpJ3Jqnl4-t0Na%SCQ$BbkIZ@x5=q5b!u!aq_m6tB^!k;!6oo#sfS_3b zd@o~>=g1uqi>PCspj}o3j>{}aeUgQZxUY{I$-_y>5?peK={Fm9&y;)8pdhRUWOFod z$}_;pz!w}|ZVwYS(-gMXBqlJavN^xk-NDw|S<}zH-`^}zaNgl5xVZkEM5ka3p%Ct} zEIZ)&NxcmF3aM-|mS|ylsGI+jxpv@DGwxBNy8q43G983{Ux{C0E0LD!?MJbn(tMV- zT)}80U1UwxF21|BLkHeDrvu{2BxSwfal4+)@&UD^wv((sbS-ZH)D|`E!VzBDRNlPM%Rvq`R{}E(O==ukgx02M6jx; zB^m_8jmH1SdX@dJ^=hiNV=gy#-|9Ub?MY07zRRH#;i}D{Q>qRBB#+T%vStMBT3*w) zR!n?97KN&gDv41$~KqQhjsR%pz>l zWC_DgWj^J$NwLSRr|;`TLv1+lGO|I?6GnL?gY5V0_}ER*g2Awc1oC%rfo2kb7A zL}Ru%BZ_?asG-Ft(>vHARBEZc2825e?!B9HvB3Pq7FsjW>C}b;%PeU2YMeE`x#(v( z0$v;0+)qwA=~IeSigg~%#?3>T9oKK?D>vqAed3?m+d?nu??a9ISf5=6kAc9A4#7TY z9iMWqk0&8G4|O{8ebUkwsNrt9*M`bUPmIX=yI4%m{Y5)l>eobepGeS;Ljo>@SJ88m zzbnP-4($2lJr()R@1}0emr)+lR|^~eR&y##_|?8*wlt)IiHllenZ9F>-A%VuYU;zs z^EA5B%cyzkbb2=Jeaf@tZbf*0yVd@PvfCQp&+g@ugSec)*|@uX_K~_d>HA5ad7_lq zyPI`7x*+ss;Y6OJ@};eq)GsB}=>zQJeaX~ep=zowoh}3XAV$@@j=3`nJZb^;#(d}F zy6LtqwU}wLhfxJpy<3$?VI0R_DV26P(jcx6#q=mKTa4HjJBL91$OyndSrTs(U$E}VK=7K zd&dIJj4i*`i9l`j6{kgDE1E56{C++~S|j+TE4_uT8G~ZvdyYxpf|SenrB7&XXhBs2 z*t;)d5Kph3YWRzb^%A;k4#$U&LbP&v2QgZwGqaw?K({TQ<567n+Mwym3TCEWZ{+(4 zqlr2YGgE;#^381<1y1MxbL?W4=$@%xUTk*3e+28sEDq7b3 zJRPsRJfdS=N$U5j6L55}iGT_(=Mp|QaOreHaC2#~-*mq?Ig{3{CO4M-~ z`SUDe_Gt|Srtv|Jmx2nN1I|&QHa#OLVNnNu3jFfVm-aKZG4ab(6sT~RThq|qOUi%K z)yRxILo@VwIt~^Zr5qla2KiWCv)!Ni*haHGrXbb?$b9C1C~sep;wg>`N^bDf#s%f| zpoZY$D&Q{Nm_YVv)#gtk;=?ikESIwc6rl7G0%DH`jMt_E8sO#%do-wcYd`m38SH@F@2MDiUqLR*Zn4>0x+8~xLg4xuS+@Z zVb~jv?f|F6wtt*VclHhhM2uaBA#Pv346?SNO7u((IMI0ZrTyyy7a9&H?Ie2`WUu+l z5Eq&kx$WC;7U19)2g5d2ZsS2hnndZX+6 zReoK=RKY+(4Yy%XVZ*9|8M5v7b*1nq^_FEAfF+%*xm!#A^o!fO(h{tC`MS=^8PuWL zFu34O!Vo0F_yHO$9qu)yG;na{r1q^1sG^i5sDmQPU6*J{aR#og92HbO-eVCkcU9V1 zb0>?E=Nn-xPjxU_MLZFQ0I;FAP@)8-YYvfv;on2`rPjScO7I(k2Uw3gqZIh~8#BEP zw-T^#zjqD`C|#fpQf}vhK?5hUGUK2bl-iewRLPCd5IztjK0}BbCbCWz?#}{HvVPMt{UF??UlH(unAConlfnWNXX3epb}M@vr{~IfCZDZ&b~J- z7EtY{nJy2)g9b9FLY^q+mKVAZCaTtk*i-VY)6t{})TK7*mB)9-jpq}upS3(5O8zUDrzUvYuHeV1e-}-gRdK_lJOd)8q&Xt%+twox_A})8+$V{3-KIZo?SE$xs;cNEn0KoJW#bisFq-^b{?J9B<`Flzck-?Yyo~yNf0#Nx{`!;XFiFi(@Y+J zpnDQ6R)1$NK$v6PisP7-N*L^q!)v^p)L9P=$zi0 zF&C+uZ0OB|UnseZ-BUvB2Lhx!m+SN=cgw|f295n?;zKgJT=`R}j9swd8IkS1n73@c ze;&1CvCl?F|Hw+gxb5$9G_!}&T(x%)rsyk6^=gC?TKtJ=U`vb}frggBHRnz{|Ez-OD z55;fww0`EUKJjiZFqG~`;21Ac1(%H*^-Ywx5ghn>hY8&^s_{t2Q1deEYCKC=J=P7d z))UJ;d7F z!8U;OD0i##h!SvgGanUf?aDWi!z)goN`va!CS+pjo&Om3gw0YK%Of;ZEG&rEb%E2t zk-@*hwy0WSb{~8=clK-Hkv2dS#4);YY!2Y3o}N{?3k#4Hs^4K8OSQ}R`rBlxTh3TOH2)#}Jt zT2}2Y%2V8UU88HsH2gKL{j-9*vcA3UM)TkVkb&0Drf4$%O!1oA^zJkK=kk)Lpl4;0 zN-Ob%7C?On+cFQ-Jk^*Y=l7=I0-j}o%+&Oo^UI^#VFyvEJt5vL`2*;aud1`x>c{6A zEDLVi=>vn{uXH09F6oV6E1RTd=&|wIlL+E(F4Cj9CVUbnUW<>!VDMQ)oZaD6Pmn&> z?ew*+o7th&;*>+AOJbv8%^6eN^=N}zwiTkCcfPqDch}S1UAfj{o2|qx7n>ndy{}hB zTA?<(R~bvp+J{?y*|aW=o3H~7{~dJQH`GE5$P2yco+H|A7Y+59`V=FhAENdcMjD)V zw36e%Q{PSyMIEU;Fe=#N+1NS>Hpu^6B4pZ%Jzo z)v^&BEsQQmZ-sYW-kN;9O66%&C>dW%;BRb77SNFk7R1__F)3oTx04qO3Ujq$S4p00 zr&gXvv9sBxCwiVS%dTLH)K1GE>}{^q{9!EuF-uQ$KSQlH??-F*k(+%Oy49zfqROWx z)cV(RTT)Cuf;+^__HKz(iKDCgiIq2 z!~~M07Bc2qiGBL5_g5o;#f6YX0HqC%anm^5eL!u8nt*WofQ7t?Z3SRRj7lkc56H@5ivq`R3X=&*g>eJt)Y_NG&sL>L)6o+VB+1{9 ziF<%Rn1%!=vM^{kNK-BE)}%Q0;!&$d2+(j4<7Fs7B@G6mVeS9zFj5vqt|19HC)2y+ z6bA3mhJyyiWnF#C{MxPRPw?AH{eN`>7bGUMA|VYGQf&X15Aujf1>ltpjfYaO|YRedj@k0gY*z*7O1uqo+Rm2(P#v)o(yQv!S>f! zGth204mmjlHlrsZDU#zD(31+}ebks0?NomQRH7A8Si__>AV=el(}PTCx2Nb}>(^!W z7axKB5*`TeP|q7x8_gIFF$EQ(H7Ip=W2pb)a|It2O8=%5R8i{(JCpG;nDeArYF0s9V=hau>& z%Wj2W0-@lMVEfOoi7;M3fPoS%C|wHlQ4=#m4L?!DTp zAGrk8HH}KA*URgmR9`f}IGmk3{0YWr{O)E&U5jbLa2Utk|ag zRp(vRIHiU1--{RPna(hjrdqrD8iJVLEmBR(_VSN5A+ZFYuyE-oZ3HJQ#}r$Qm8^?7 z3-UV!86TxsrM9kr`BrvBU^SHlmvejc(b80TVxR6_fq%S&WlPuQ$Ba<%egV_3h0Lfi zVrgN#LEZg=;h`Ue@e{SU_ZrS5`G=*q4p}{vq`c}A^c&D8W~L=cC6+~0R<{P{CO20% z6p#1OTshYYvc-AoJK$V*A=5tamr7fPX`~gGA`6(7Ci>>&fQsSX#^tC@La+HMW78Rh zf_b}IA2;gEv>y954j=j= zahiW^S)V)JA{W0je8d0P@CpC5W&JN++@GAUzc_L&asSPc>vv5B6TNce$ntdi-27;r z(=2T;=OMv$`WOg&7DYfgeogSk^D~-n(Ul>f8y@p!g^sog0K>PDw&MIULKfGr6=itS zxbVc2SYU617+U~C1zRu;-Mj{X%Umc+Hf?6I(Ae@voM>9hu~MS@6PZO3Si`MUij)pwqXaf?WZET%G3(?$ao52)FGz_=<34FdC5Y>S7hWsr@^@X5(eS_ zNMXYMFACEqLkCk66(v+THQbFz7eI8&NRSDBK`pdRswn$g zZ_lY!D)#3z7nOF)_2Y26CoO~fsCLo`JYy9Vo?W!*!H@@e;{k=v^AY&FOW4XR@Z&~8 zE%jl^7h-r~1gHU@;F%kVefIfxG%GB5Le*B(TYxTiNHXip^00gY?%ZH(A)N)&Lwe*K z5CepMQT?*DUSEDRpHYLR73rW$;~QMGBXmq!A30Z>J{ih7pw+cJfFtTAsjk1VOMU{_wl*Z-~v=4`^ zO3LH8)zeX@Wjtk#hx5(&A#sjxxs@hKqAJ>wM-iKMKC$M^Ml0j$1ibkQ6(tTyljXOe z)E-8=2E+!jBLUH19~iMg03;v*EFc6*;T>dO_F9d8A|cBu(dXPwNq=lF;JCX^&GVjW$isXD_Bo@S+LQr-`=*lEJLK^JbTypJiEWT%ksAM}_X28U|8ByQ*(M!@3DjOxF$X*ih5QTD57Vq_y1%gRre8>#|2L!a%}eT< z*cmH1*nu2bjO`puU(vj`vaVtPE4KHIdUUHeUA+aNL0GC_L$?Vd;i_eWu_~?nqW7aG zG77iz+0lxl!q}rFJO5Nd9kk6mN|kq?G2F@ARS+}_J|&DjdTtU7nRFCMh<%AN_{rEL zU7yoDewsrByMV-+$q^gukdPBu`^Cum7S7TP>U(sQ4o{=mOnzhcmopYMsHjfR7psaA zgT(cjpX#}ke44mkH6OKgo#9~nltfV{2<4zYk7*${a;uX%4F^@>UKg$vIgv?Tb%cJX zUp)!%UXs>*hG}EUOj<5%jXo*@W-;<|i8a8PWuK-)9u1)mTRq&ykA1)mGWivvBAmXt znHkGZ7<7X8bk)jrxFKQAm^w_f}{4au1}=ioOSin*ee+EHRQ83;9Icn(p$}iU7whmV7i)0W*seB3k&%# z`V_L+SK@#_laO=@3$x$Dy;hmZ$3uWMBm~6reHE~$iX{e-=Q6K3l&Zx>G zfhHJ^&;;FyGKWy10pRwud*_Uda5Hprd?wg!p3Aeizv8JNZ#v;w5+O(uI4){e2IhiOSJy)q{jL)QpV1XPIfl0!K$5B09Xjyu(NCc!aB zhNHuH4HW}KOG9D)fDy3la-`s&i@MEQ2J@ZmQz6{NCY^_>n6fGUbd>xWA6!1016(%s z+tNysIa6D8q-;y;LnY!YS|o{2)cutOOCqG%Y*dmq?~Vp#0lVjBG#9h|I?>lo9&I*x zBeFS;ZX4LOhVCe>Rb(So5U^!!NGh}9D-)<8>)r2F0w40fm38&(69KE151~Ug7FN8- zw^|S4A_Y=3-wM}!2;(8iA{Tqptnj`9@7V&~b5Rc@KJbBU(g{fPme?Y^$v!vJ6CA06 zvvI=5#3uZ+S$s9HaP-j5od>wPo;0&GP}*OSjV{qA*hpTV^CQxO71-eSR_!RQ<*4I; zN7m=BcLlsPKZrzMqF?+UJL2mMXp7s~I+@x!>3?(wnL6se&cKMOwqh3Qz1X;h4qgCV#=J1*QDhY3jt@INKG|sd#(~OdKzHHSf3Aa#8 zB{|3JL4e?}-Gn1&21?akub-?~xdU%eGY?)MP|ZIN%?g_SBa+SAtM$pmZY6Nm+vqcp zn?$l2NxY)>rbyf&5>jJ{w?hlsqV_U`J44Z7C4B_5h*``+1Go z+c6lrYR^7_^?1_?Hg^ymO03t!zK!_II*WJ$8)};wP=^DEC8&K$9Q-LlVvPCOb9Azx zrfeV_98Ob;SARdJJ>~QP0Zykn9|hwu0(MXrcFi<04M&!Kis$#Joh8V-m?uwIY6Svd zk@#VZ+T{WMH~upg>382vWB#Xm$2(c|4%I2Bwj1td*A~T1A=jj@e%~~0blm8e0Jgsb zUEh7D#_+$&xu-wGhQ1pALEh>OxXoU$-fK>j9I~?z3P)$3c&Mc=e|=#1rf@lIU#4pl z?)8Czgu;gS`vAnBod4GY5dSWJ9EbR);-AFs*CPRc3(bqGe;g3_r}Ce4$ye_8-{Sd_ zQZLGXGr|9<`sdEoE1&XjF+%unzyE_>`A;i1%J!k z%Oc@_K!ks)|5=z{8`a;E$M}c(f3>ZDYX4bOUQ5T{GRpLa_TQ!ap9cO+^M5y>#PYvZ XA|+YamnQ-O0_FAdl6TLnuXq0sM|VBa literal 0 HcmV?d00001 diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..44dcf52 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,5 @@ +requests +ur-rtde +openpyxl +pynput +numpy