universalrobots/dual_ursim_control
3
0
Fork 0
Code Pull Requests Releases Activity

Compare commits

base: universalrobots:c6e5da518e16789bcd743c10cdc63423218effc6
Branches Tags
universalrobots:master
..
compare: universalrobots:9b85e3a633ab010fa348f2908233eef1aea6057a
Branches Tags
universalrobots:master

No commits in common. "c6e5da518e16789bcd743c10cdc63423218effc6" and "9b85e3a633ab010fa348f2908233eef1aea6057a" have entirely different histories.
c6e5da518e ... 9b85e3a633

3 changed files with 110 additions and 103 deletions
Show Stats Expand all files Collapse all files

36
docker-compose.yml
View File

@ -1,36 +0,0 @@
version: "3.9"
services:
controller:
build: .
ports:
- "8888:8888"
environment:
- MASTER_IP=10.6.6.8
- SLAVE_IP=10.6.6.8
#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=0
- SLAVE_ENABLE=0
- JOGGING_ENABLE=1
- 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

30
dual_ursim_control/config.py
View File

@ -1,30 +0,0 @@
import os
class Config:
MASTER_IP = os.environ.get("MASTER_IP")
SLAVE_IP = os.environ.get("SLAVE_IP")
MASTER_GRIPPER_WIDTH_OPENED = int(os.environ.get("MASTER_GRIPPER_WIDTH_OPENED"))
MASTER_GRIPPER_WIDTH_CLOSED = int(os.environ.get("MASTER_GRIPPER_WIDTH_CLOSED"))
SLAVE_GRIPPER_IP = os.environ.get("SLAVE_GRIPPER_IP")
SLAVE_GRIPPER_WIDTH_OPENED = int(os.environ.get("SLAVE_GRIPPER_WIDTH_OPENED"))
SLAVE_GRIPPER_WIDTH_CLOSED = int(os.environ.get("SLAVE_GRIPPER_WIDTH_CLOSED"))
MASTER_ENABLE = os.environ.get("MASTER_ENABLE", 'False').lower() in ('true', '1', 't')
SLAVE_ENABLE = os.environ.get("SLAVE_ENABLE", 'False').lower() in ('true', '1', 't')
JOGGING_ENABLE = os.environ.get("JOGGING_ENABLE", 'False').lower() in ('true', '1', 't')
# SLOW / FAST
EXECUTION = os.environ.get("EXECUTION")
L_SPEED_SLOW = float(os.environ.get("L_SPEED_SLOW"))
L_SPEED_FAST = float(os.environ.get("L_SPEED_FAST"))
L_ACC_SLOW = float(os.environ.get("L_ACC_SLOW"))
L_ACC_FAST = float(os.environ.get("L_ACC_FAST"))
J_SPEED_SLOW = float(os.environ.get("J_SPEED_SLOW"))
J_SPEED_FAST = float(os.environ.get("J_SPEED_FAST"))
J_ACC_SLOW = float(os.environ.get("J_ACC_SLOW"))
J_ACC_FAST = float(os.environ.get("J_ACC_FAST"))

147
dual_ursim_control/main.py
View File

@ -15,12 +15,15 @@ import logging
import logging.config
from xmlrpc.server import SimpleXMLRPCServer
from configparser import ConfigParser
from config import Config
# from pynput.keyboard import Listener, Key
SCRIPT_DIR = os.path.dirname(__file__)
config_object = ConfigParser()
config_object.read(os.path.join(SCRIPT_DIR, 'config.ini'))
config = config_object['DEFAULT']
# TODO: export these to json?
# Config file name
# CONFIG_FILE = 'path.xlsx'
@ -255,18 +258,54 @@ def read_path_waypoints_from_file():
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.JOGGING_ENABLE:
if config.getboolean('JOGGING_ENABLE'):
logging.debug("Master-Fred: Press 'f' to continue")
# while not jogging_wait['Master']:
# pass
# jogging_wait['Master'] = False
jogging_event['Master'].clear()
jogging_event['Master'].wait()
def slave_jogging_wait():
if Config.JOGGING_ENABLE:
if config.getboolean('JOGGING_ENABLE'):
logging.debug("Slave-Erik: Press 'e' to continue")
# while not jogging_wait['Slave']:
# pass
# jogging_wait['Slave'] = False
jogging_event['Slave'].clear()
jogging_event['Slave'].wait()
@ -279,7 +318,7 @@ def master_operate_gripper(target_width):
return
port = 30001
host = Config.MASTER_IP
host = config['MASTER_IP']
tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcp_socket.connect((host, port))
@ -447,10 +486,10 @@ def master_gripper_toggle(state='Open'):
# This function accepts either 'Open' or 'Close'
if state == 'Open':
master_operate_gripper(Config.MASTER_GRIPPER_WIDTH_OPENED)
master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_OPENED'))
elif state == 'Close':
master_operate_gripper(Config.MASTER_GRIPPER_WIDTH_CLOSED)
master_operate_gripper(config.getint('MASTER_GRIPPER_WIDTH_CLOSED'))
else:
logging.info('Invalid gripper option given')
@ -459,15 +498,15 @@ def master_gripper_toggle(state='Open'):
def master_connect():
try:
robot = rtde_control.RTDEControlInterface(Config.MASTER_IP)
robot = rtde_control.RTDEControlInterface(config['MASTER_IP'])
except:
text = f"Cannot connect to robot at: {Config.MASTER_IP}"
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}")
logging.info(f"Connected to robot at: {config['MASTER_IP']}")
# robot.setPayload(1, (0, 0, 0.01))
return robot
@ -475,15 +514,15 @@ def master_connect():
def slave_connect():
try:
robot = rtde_control.RTDEControlInterface(Config.SLAVE_IP)
robot = rtde_control.RTDEControlInterface(config['SLAVE_IP'])
except:
text = f"Cannot connect to robot at: {Config.SLAVE_IP}"
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}")
logging.info(f"Connected to robot at: {config['SLAVE_IP']}")
# robot.setPayload(1, (0, 0, 0.01))
return robot
@ -505,13 +544,13 @@ 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
ip = config['SLAVE_GRIPPER_IP']
if state == 'Open':
width = Config.SLAVE_GRIPPER_WIDTH_OPENED
width = config.getint('SLAVE_GRIPPER_WIDTH_OPENED')
elif state == 'Close':
width = Config.SLAVE_GRIPPER_WIDTH_CLOSED
width = config.getint('SLAVE_GRIPPER_WIDTH_CLOSED')
else:
# log error
logging.info('Invalid gripper option given')
@ -528,7 +567,7 @@ 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.connect((config['MASTER_IP'], 30001))
s.send(str.encode(message))
s.close()
@ -540,7 +579,7 @@ 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.connect((config['SlAVE_IP'], 30001))
s.send(str.encode(message))
s.close()
@ -548,6 +587,32 @@ def slave_movel_time_based(pose, a=1, v=0.06, t=2):
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')
@ -593,7 +658,7 @@ def master_thread(sync_event, path):
robot.moveJ(path['CM4_J'], j_speed, j_acc)
logging.info(f'CM4: M Sync Begin')
if Config.SLAVE_ENABLE:
if config.getboolean('SLAVE_ENABLE'):
sync_event['Master'].set()
logging.info('Signaling to slave')
@ -619,7 +684,7 @@ def master_thread(sync_event, path):
# robot = master_connect()
# return
if Config.SLAVE_ENABLE:
if config.getboolean('SLAVE_ENABLE'):
logging.debug('Waiting for slave')
sync_event['Slave'].wait()
@ -688,7 +753,7 @@ def slave_thread(sync_event, path):
time.sleep(0.5)
if Config.MASTER_ENABLE:
if config.getboolean('MASTER_ENABLE'):
logging.debug('Waiting for master')
sync_event['Master'].wait()
@ -701,7 +766,7 @@ def slave_thread(sync_event, path):
# robot = slave_connect()
logging.info(f'CS5: S Sync End')
if Config.MASTER_ENABLE:
if config.getboolean('MASTER_ENABLE'):
sync_event['Slave'].set()
logging.debug('Signaling to master')
@ -735,7 +800,7 @@ def slave_thread(sync_event, path):
robot.moveJ(path['CS3_J'], j_speed, j_acc)
logging.info(f'CS3: S Start Oriented')
if Config.MASTER_ENABLE:
if config.getboolean('MASTER_ENABLE'):
sync_event['Slave'].set()
logging.debug('Signaling to master')
@ -788,27 +853,28 @@ class ServerThread(threading.Thread):
if __name__ == '__main__':
logging.config.fileConfig(os.path.join(SCRIPT_DIR, 'log_config.ini'))
if Config.EXECUTION == 'SLOW':
j_speed = Config.J_SPEED_SLOW
l_speed = Config.L_SPEED_SLOW
if config['EXECUTION'] == 'SLOW':
j_speed = config.getfloat('J_SPEED_SLOW')
l_speed = config.getfloat('L_SPEED_SLOW')
j_acc = Config.J_ACC_SLOW
l_acc = Config.L_ACC_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.J_SPEED_FAST
l_speed = Config.L_SPEED_FAST
elif config['EXECUTION'] == 'FAST':
j_speed = config.getfloat('J_SPEED_FAST')
l_speed = config.getfloat('L_SPEED_FAST')
j_acc = Config.J_ACC_FAST
l_acc = Config.L_ACC_FAST
j_acc = config.getfloat('J_ACC_FAST')
l_acc = config.getfloat('L_ACC_FAST')
logging.info("Execution speed: fast")
if Config.JOGGING_ENABLE:
if config.getboolean('JOGGING_ENABLE'):
logging.info('Jogging enabled')
# TODO: eliminate excel
@ -828,18 +894,25 @@ if __name__ == '__main__':
xmlrpcServer = ServerThread()
xmlrpcServer.start()
if Config.MASTER_ENABLE:
# 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.SLAVE_ENABLE:
if config.getboolean('SLAVE_ENABLE'):
slave_thread = threading.Thread(name='SlaveThread', target=slave_thread, args=(sync_event, path_slave))
slave_thread.start()
if Config.MASTER_ENABLE:
if config.getboolean('MASTER_ENABLE'):
master_thread.join()
if Config.SLAVE_ENABLE:
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#