'''
Python 3.x library to control an UR robot through its TCP/IP interfaces
Copyright (C) 2017 Martin Huus Bjerge, Rope Robotics ApS, Denmark
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software
is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies
or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used
in advertising or otherwise to promote the sale, use or other dealings in this Software
without prior written authorization from "Rope Robotics ApS".
'''
__author__ = "Martin Huus Bjerge"
__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark"
__license__ = "MIT License"
import URBasic
import socket
import threading
import select
import re
import numpy as np
import time
DEFAULT_TIMEOUT = 1.0
class ConnectionState:
ERROR = 0
DISCONNECTED = 1
CONNECTED = 2
PAUSED = 3
STARTED = 4
class RealTimeClient(object):
'''
Interface to UR robot Real Time Client interface.
For more detailes see this site:
http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/remote-control-via-tcpip-16496/
The Real Time Client in this version is only used to send program and script commands
to the robot, not to read data from the robot, all data reading is done via the RTDE interface.
The constructor takes a UR robot hostname as input, and a RTDE configuration file.
Input parameters:
host (string): hostname or IP of UR Robot (RT CLient server)
conf_filename (string): Path to xml file describing what channels to activate
logger (URBasis_DataLogging obj): A instance if a logger object if common logging is needed.
Example:
rob = URBasic.realTimeClient.RT_CLient('192.168.56.101')
self.close_rtc()
'''
def __init__(self, robotModel):
'''
Constructor see class description for more info.
'''
if(False):
assert isinstance(robotModel, URBasic.robotModel.RobotModel) ### This line is to get code completion for RobotModel
self.__robotModel = robotModel
logger = URBasic.dataLogging.DataLogging()
name = logger.AddEventLogging(__name__, log2Consol=False,level = URBasic.logging.WARNING)
self.__logger = logger.__dict__[name]
self.__robotModel.rtcConnectionState = ConnectionState.DISCONNECTED
self.__reconnectTimeout = 60
self.__sock = None
self.__thread = None
if self.__connect():
self.__logger.info('RT_CLient constructor done')
else:
self.__logger.info('RT_CLient constructor done but not connected')
def __connect(self):
'''
Initialize RT Client connection to host .
Return value:
success (boolean)
Example:
rob = URBasic.realTimeClient.RT_CLient('192.168.56.101')
rob.connect()
'''
if self.__sock:
return True
t0 = time.time()
while (time.time()-t0<self.__reconnectTimeout) and self.__robotModel.rtcConnectionState < ConnectionState.CONNECTED:
try:
self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.__sock.settimeout(DEFAULT_TIMEOUT)
self.__sock.connect((self.__robotModel.ipAddress, 30003))
self.__robotModel.rtcConnectionState = ConnectionState.CONNECTED
time.sleep(0.5)
self.__logger.info('Connected')
return True
except (socket.timeout, socket.error):
self.__sock = None
self.__logger.error('RTC connecting')
return False
def Disconnect(self):
'''
Disconnect the RT Client connection.
'''
if self.__sock:
self.__sock.close()
self.__sock = None
self.__logger.info('Disconnected')
self.__robotModel.rtcConnectionState = ConnectionState.DISCONNECTED
return True
def IsRtcConnected(self):
'''
Returns True if the connection is open.
Return value:
status (boolean): True if connected and False of not connected.
Example:
rob = URBasic.realTimeClient.RT_CLient('192.168.56.101')
rob.connect()
print(rob.is_connected())
rob.disconnect()
'''
return self.__robotModel.rtcConnectionState > ConnectionState.DISCONNECTED
def SendProgram(self,prg=''):
'''
Send a new command or program (string) to the UR controller.
The command or program will be executed as soon as it's received by the UR controller.
Sending a new command or program while stop and existing running command or program and start the new one.
The program or command will also bee modified to include some control signals to be used
for monitoring if a program execution is successful and finished.
Input parameters:
prg (string): A string containing a single command or a whole program.
Example:
rob = URBasic.realTimeClient.RT_CLient('192.168.56.101',logger=logger)
rob.connect()
rob.send_srt('set_digital_out(0, True)')
rob.disconnect()
'''
if not self.IsRtcConnected():
if not self.__connect():
self.__logger.error('SendProgram: Not connected to robot')
if self.__robotModel.stopRunningFlag:
self.__logger.info('SendProgram: Send program aborted due to stopRunningFlag')
return
#Close down previous thread
if self.__thread is not None:
if self.__robotModel.rtcProgramRunning:
self.__robotModel.stopRunningFlag = True
while self.__robotModel.rtcProgramRunning: time.sleep(0.1)
self.__robotModel.stopRunningFlag = False
self.__thread.join()
#Rest status bits
self.__robotModel.rtcProgramRunning = True
self.__robotModel.rtcProgramExecutionError = False
#Send and wait from program
self.__sendPrg(self.__AddStatusBit2Prog(prg))
self.__thread = threading.Thread(target=self.__waitForProgram2Finish, kwargs={'prg': prg})
self.__thread.start()
#self.__waitForProgram2Finish(prg)
def Send(self,prg=''):
'''
Send a new command (string) to the UR controller.
The command or program will be executed as soon as it's received by the UR controller.
Sending a new command or program while stop and existing running command or program and start the new one.
The program or command will also bee modified to include some control signals to be used
for monitoring if a program execution is successful and finished.
Input parameters:
prg (string): A string containing a single command or a whole program.
Example:
rob = URBasic.realTimeClient.RT_CLient('192.168.56.101',logger=logger)
rob.connect()
rob.send_srt('set_digital_out(0, True)')
rob.disconnect()
'''
if not self.IsRtcConnected():
if not self.__connect():
self.__logger.error('SendProgram: Not connected to robot')
if self.__robotModel.stopRunningFlag:
self.__logger.info('SendProgram: Send command aborted due to stopRunningFlag')
return
#Rest status bits
self.__robotModel.rtcProgramRunning = True
self.__robotModel.rtcProgramExecutionError = False
#Send
self.__sendPrg(prg)
self.__robotModel.rtcProgramRunning = False
def __AddStatusBit2Prog(self,prg):
'''
Modifying program to include status bit's in beginning and end of program
'''
def1 = prg.find('def ')
if def1>=0:
prglen = len(prg)
prg = prg.replace('):\n', '):\n write_output_boolean_register(0, True)\n',1)
if len(prg) == prglen:
self.__logger.warning('Send_program: Syntax error in program')
return False
if (len(re.findall('def ', prg)))>1:
mainprg = prg[0:prg[def1+4:].find('def ')+def1+4]
mainPrgEnd = (np.max([mainprg.rfind('end '), mainprg.rfind('end\n')]))
prg = prg.replace(prg[0:mainPrgEnd], prg[0:mainPrgEnd] + '\n write_output_boolean_register(1, True)\n',1)
else:
mainPrgEnd = prg.rfind('end')
prg = prg.replace(prg[0:mainPrgEnd], prg[0:mainPrgEnd] + '\n write_output_boolean_register(1, True)\n',1)
else:
prg = 'def script():\n write_output_boolean_register(0, True)\n ' + prg + '\n write_output_boolean_register(1, True)\nend\n'
return prg
def __sendPrg(self,prg):
'''
Sending program str via socket
'''
programSend = False
self.__robotModel.forceRemoteActiveFlag = False
while not self.__robotModel.stopRunningFlag and not programSend:
try:
(_, writable, _) = select.select([], [self.__sock], [], DEFAULT_TIMEOUT)
if len(writable):
self.__sock.send(prg.encode())
self.__logger.info('Program send to Robot:\n' + prg)
programSend = True
except:
self.__sock = None
self.__robotModel.rtcConnectionState = ConnectionState.ERROR
self.__logger.warning('Could not send program!')
self.__connect()
if not programSend:
self.__robotModel.rtcProgramRunning = False
self.__logger.error('Program re-sending timed out - Could not send program!')
time.sleep(0.1)
def __waitForProgram2Finish(self,prg):
'''
waiting for program to finish
'''
waitForProgramStart = len(prg)/50
notrun = 0
prgRest = 'def resetRegister():\n write_output_boolean_register(0, False)\n write_output_boolean_register(1, False)\nend\n'
while not self.__robotModel.stopRunningFlag and self.__robotModel.rtcProgramRunning:
if self.__robotModel.SafetyStatus().StoppedDueToSafety:
self.__robotModel.rtcProgramRunning = False
self.__robotModel.rtcProgramExecutionError = True
self.__logger.error('SendProgram: Safety Stop')
elif self.__robotModel.OutputBitRegister()[0] == False:
self.__logger.debug('sendProgram: Program not started')
notrun += 1
if notrun > waitForProgramStart:
self.__robotModel.rtcProgramRunning = False
self.__logger.error('sendProgram: Program not able to run')
elif self.__robotModel.OutputBitRegister()[0] == True and self.__robotModel.OutputBitRegister()[1] == True:
self.__robotModel.rtcProgramRunning = False
self.__logger.info('sendProgram: Finished')
elif self.__robotModel.OutputBitRegister()[0] == True:
if self.__robotModel.RobotStatus().ProgramRunning:
self.__logger.debug('sendProgram: UR running')
notrun = 0
else:
notrun += 1
if notrun>10:
self.__robotModel.rtcProgramRunning = False
self.__robotModel.rtcProgramExecutionError = True
self.__logger.error('SendProgram: Program Stopped but not finiched!!!')
else:
self.__robotModel.rtcProgramRunning = False
self.__logger.error('SendProgram: Unknown error')
time.sleep(0.05)
self.__sendPrg(prgRest)
self.__robotModel.rtcProgramRunning = False
