Added files from the original repo
This commit is contained in:
133
.gitignore
vendored
Normal file
133
.gitignore
vendored
Normal file
@@ -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
|
||||
42
dual_ursim_control/config.ini
Normal file
42
dual_ursim_control/config.ini
Normal file
@@ -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
|
||||
25
dual_ursim_control/log_config.ini
Normal file
25
dual_ursim_control/log_config.ini
Normal file
@@ -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'
|
||||
876
dual_ursim_control/main.py
Normal file
876
dual_ursim_control/main.py
Normal file
@@ -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#
|
||||
|
||||
BIN
dual_ursim_control/path.xlsx
Normal file
BIN
dual_ursim_control/path.xlsx
Normal file
Binary file not shown.
5
requirements.txt
Normal file
5
requirements.txt
Normal file
@@ -0,0 +1,5 @@
|
||||
requests
|
||||
ur-rtde
|
||||
openpyxl
|
||||
pynput
|
||||
numpy
|
||||
Reference in New Issue
Block a user