commit bc68523d38c990fc434f6ed66c38e7cb1acb854e Author: marcsello Date: Thu Apr 1 17:45:59 2021 +0200 Added files from the original repo 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 0000000..a1a1042 Binary files /dev/null and b/dual_ursim_control/path.xlsx differ 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