def __init__(self,
robo_host: str,
robo_port: int,
config_filename: str,
record: bool = False):
self.record = record
# get recipes!
conf = rtde_config.ConfigFile(config_filename)
self.state_names, self.state_types = conf.get_recipe('state')
self.gantry_names, self.gantry_types = conf.get_recipe('gantry')
self.internal_names, self.internal_types = conf.get_recipe('internal')
self.home_names, self.home_types = conf.get_recipe('home')
self.control_names, self.control_types = conf.get_recipe('control')
self.positions_names, self.positions_types = conf.get_recipe(
'positions')
# connect, get controller version
self.con = rtde.RTDE(robo_host, robo_port)
self.con.connect()
self.con.get_controller_version()
# setup recipes
self.con.send_output_setup(self.state_names, self.state_types)
self.gantry = self.con.send_input_setup(self.gantry_names,
self.gantry_types)
self.internal = self.con.send_input_setup(self.internal_names,
self.internal_types)
self.home = self.con.send_input_setup(self.home_names, self.home_types)
self.control = self.con.send_input_setup(self.control_names,
self.control_types)
self.positions = self.con.send_input_setup(self.positions_names,
self.positions_types)
def __init__(self, ROBOT_HOST='192.168.68.23',
ROBOT_PORT=30004,
config_filename='control_loop_configuration.xml'):
keep_running = True
logging.getLogger().setLevel(logging.INFO)
conf = rtde_config.ConfigFile(config_filename)
state_names, state_types = conf.get_recipe('state')
setp_names, setp_types = conf.get_recipe('setp')
watchdog_names, watchdog_types = conf.get_recipe('watchdog')
gripper_names, gripper_types = conf.get_recipe("gripper")
self.con = rtde.RTDE(ROBOT_HOST, ROBOT_PORT)
self.con.connect()
# get controller version
self.con.get_controller_version()
# setup recipes
self.con.send_output_setup(state_names, state_types)
self.setp = self.con.send_input_setup(setp_names, setp_types)
self.watchdog = self.con.send_input_setup(watchdog_names, watchdog_types)
self.gripper = self.con.send_input_setup(gripper_names, gripper_types)
### Initialize variables of the register
self.initialize()
# start data synchronization
if not self.con.send_start():
sys.exit()
def __enter__(self):
conf = rtde_config.ConfigFile(self.config_file)
output_names, output_types = conf.get_recipe('out')
self.connection = rtde.RTDE(self.host, self.port)
self.connection.connect()
if not self.connection.send_output_setup(
output_names, output_types,
frequency=1 / self.sampling_interval):
raise RuntimeError('Unable to configure output')
if not self.connection.send_start():
raise RuntimeError('Unable to start synchronization')
return self.connection
def __init__(self, configfile, hostname, port, frequency):
self.configfile = configfile
self.hostname = hostname
self.port = port
self.frequency = frequency
self.conf = rtde_config.ConfigFile(self.configfile)
self.output_names, self.output_types = self.conf.get_recipe('out')
self.con = rtde.RTDE(self.hostname, self.port)
self.con.connect()
self.con.get_controller_version()
self.con.send_output_setup(self.output_names, self.output_types, self.frequency)
self.con.send_start()
def _initialize_robot_sync(host: str):
"""Set up communication with UR robot
Args:
host: IP address
Returns:
Connection to robot
Package containing the specific input data registers
Raises:
RuntimeError: if protocol do not match
RuntimeError: if script is unable to configure output
RuntimeError: if synchronization is not possible
"""
conf = rtde_config.ConfigFile(
Path(Path.cwd() / "universal_robots_communication_file.xml"))
output_names, output_types = conf.get_recipe("out")
input_names, input_types = conf.get_recipe("in")
# port 30004 is reserved for rtde
con = rtde.RTDE(host, 30004)
con.connect()
# To ensure that the application is compatable with further versions of UR controller
if not con.negotiate_protocol_version():
raise RuntimeError("Protocol do not match")
if not con.send_output_setup(output_names, output_types, frequency=200):
raise RuntimeError("Unable to configure output")
robot_input_data = con.send_input_setup(input_names, input_types)
if not con.send_start():
raise RuntimeError("Unable to start synchronization")
print("Communication initialization completed. \n")
return con, robot_input_data
help='data configuration file to use (record_configuration.xml)')
parser.add_argument('--output',
default='robot_data.csv',
help='data output file to write to (robot_data.csv)')
parser.add_argument("--verbose",
help="increase output verbosity",
action="store_true")
args = parser.parse_args()
if args.verbose:
logging.basicConfig(level=logging.INFO)
conf = rtde_config.ConfigFile(args.config)
output_names, output_types = conf.get_recipe('out')
con = rtde.RTDE(args.host, args.port)
con.connect()
# get controller version
con.get_controller_version()
# setup recipes
if not con.send_output_setup(
output_names, output_types, frequency=args.frequency):
logging.error('Unable to configure output')
sys.exit()
#start data synchronization
if not con.send_start():
logging.error('Unable to start synchronization')
sys.exit()
ROBOT_HOST = 'localhost'
ROBOT_PORT = 30004
config_filename = 'control_loop_configuration.xml'
RTDE_PROTOCOL_VERSION = 1
keep_running = True
logging.getLogger().setLevel(logging.INFO)
conf = rtde_config.ConfigFile(config_filename)
state_names, state_types = conf.get_recipe('state')
setp_names, setp_types = conf.get_recipe('setp')
watchdog_names, watchdog_types = conf.get_recipe('watchdog')
con = rtde.RTDE(ROBOT_HOST, ROBOT_PORT)
con.connect()
# get controller version
con.get_controller_version()
# set protocol version
if not con.negotiate_protocol_version(RTDE_PROTOCOL_VERSION):
sys.exit()
# setup recipes
con.send_output_setup(state_names, state_types)
setp = con.send_input_setup(setp_names, setp_types)
watchdog = con.send_input_setup(watchdog_names, watchdog_types)
# Setpoints to move the robot to
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
ROBOT_HOST_IP = '192.168.1.112'
RTDE_PORT = 30004
SECONDARY_PORT = 30002
secondary_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sampling_frequency = 125
config_file = 'record_configuration.xml'
save_in_binary = False
conf = rtde_config.ConfigFile(config_file)
output_names, output_types = conf.get_recipe('out')
rtde_connection = rtde.RTDE(ROBOT_HOST_IP, RTDE_PORT)
rtde_connection.connect()
# setup recipes
if not rtde_connection.send_output_setup(
output_names, output_types, frequency=sampling_frequency):
logging.error('Unable to configure output')
sys.exit()
#start data synchronization
if not rtde_connection.send_start():
logging.error('Unable to start synchronization')
sys.exit()
int = 1
isidle = True
def _enstablish_connection(self):
# enstablish control connection
self.ur5_control = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.ur5_control.settimeout(10)
self.ur5_control.connect((self.host, self.control_port_number))
time.sleep(1.00)
self.ur5_control.send(b"set_digital_out(2, True)" + b"\n")
time.sleep(.1)
self.ur5_control.send(b"set_digital_out(7, True)" + b"\n")
time.sleep(1.00)
print("CONTROL CONNECTION TO UR5 ESTABLISHED...")
# s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# s.settimeout(10)
# s.connect((self.host, self.data_port_number))
# time.sleep(1.00)
# print("DATA CONNECTION TO UR5 ESTABLISHED...")
# enstablish Dashboard Server connection
#"unlock protective stop" 29999
self.dashboard = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.dashboard.settimeout(10)
self.dashboard.connect((self.host, 29999))
time.sleep(1.00)
print("UR5 DASHBOARD SERVER CONNECTION ESTABLISHED...")
self.dashboard.send(encode("unlock protective stop\n"))
print("protective stop unlocked")
# parameters
parser = argparse.ArgumentParser()
parser.add_argument('--host',
default='localhost',
help='name of host to connect to (localhost)')
parser.add_argument('--port',
type=int,
default=30004,
help='port number (30004)')
parser.add_argument('--samples',
type=int,
default=0,
help='number of samples to record')
parser.add_argument('--frequency',
type=int,
default=125,
help='the sampling frequency in Herz')
parser.add_argument(
'--config',
default='rtde/UR5_config.xml',
help='data configuration file to use (record_configuration.xml)')
parser.add_argument("--verbose",
help="increase output verbosity",
action="store_true")
args = parser.parse_args()
if args.verbose:
logging.basicConfig(level=logging.INFO)
self.conf = rtde_config.ConfigFile(args.config)
state_names, state_types = self.conf.get_recipe('out')
watchdog_names, watchdog_types = self.conf.get_recipe('watchdog')
# enstablish data connection
self.rtde = rtde.RTDE(self.host, self.data_port_number)
self.rtde.connect()
# get controller version
self.rtde.get_controller_version()
# setup recipes
if not self.rtde.send_output_setup(
state_names, state_types, frequency=args.frequency):
logging.error('Unable to configure output')
sys.exit()
self.watchdog = self.rtde.send_input_setup(watchdog_names,
watchdog_types)
print("DATA CONNECTION TO UR5 ESTABLISHED...")
# The function "rtde_set_watchdog" in the "rtde_control_loop.urp" creates a 1 Hz watchdog
self.watchdog.input_int_register_0 = 0
# start data synchronization
if not self.rtde.send_start():
sys.exit()
blocking = 1/float(args.frequency)
lookahead = 0.05
gain = 800
maxPositionError = 0.0000000001
maxOrientationError = 0.0000000001
now = datetime.datetime.now()
if args.verbose:
logging.basicConfig(level=logging.INFO)
conf = rtde_config.ConfigFile(args.config)
state_names, state_types = conf.get_recipe('state')
setp_names, setp_types = conf.get_recipe('setp')
watchdog_names, watchdog_types = conf.get_recipe('watchdog')
con = rtde.RTDE(HOST, args.port)
con.connect()
# get controller version
con.get_controller_version()
# setup recipes
if not con.send_output_setup(state_names, state_types, frequency = args.frequency):
logging.error('Unable to configure output')
sys.exit()
setp = con.send_input_setup(setp_names, setp_types)
watchdog = con.send_input_setup(watchdog_names, watchdog_types)
#start data synchronization
if not con.send_start():
logging.error('Unable to start synchronization')
def run():
# parameters
parser = argparse.ArgumentParser()
parser.add_argument('--host',
default='10.20.0.25',
help='name of host to connect to (localhost)')
parser.add_argument('--port',
type=int,
default=30004,
help='port number (30004)')
parser.add_argument('--frequency',
type=int,
default=10,
help='the sampling frequency in Herz')
parser.add_argument(
'--config',
default='record_configuration.xml',
help='data configuration file to use (record_configuration.xml)')
args = parser.parse_args()
print args.host
conf = rtde_config.ConfigFile(args.config)
output_names, output_types = conf.get_recipe('out')
con = rtde.RTDE(args.host, 30004)
con.connect()
# get controller version
con.get_controller_version()
# setup recipes
if not con.send_output_setup(
output_names, output_types, frequency=args.frequency):
logging.error('Unable to configure output')
sys.exit()
# start data synchronization
if not con.send_start():
logging.error('Unable to start synchronization')
sys.exit()
# get the data
keep_running = True
while keep_running:
try:
state = con.receive()
if state is not None:
data = []
for i in range(len(output_names)):
size = serialize.get_item_size(output_types[i])
value = state.__dict__[output_names[i]]
if size > 1:
data.extend(value)
else:
data.append(value)
# first 3 need to be multiplied by 1000
for i in range(0, 3):
data[i] *= 1000
print(", ".join(str(v) for v in data))
else:
# lost connection retrying
print "Lost connection retrying..."
run()
except KeyboardInterrupt:
keep_running = False
con.send_pause()
con.disconnect()
sys.exit()
sleep(1 / args.frequency)
con.send_pause()
con.disconnect()
default='robot_data.csv',
help='data output file to write to (robot_data.csv)')
parser.add_argument("--verbose",
help="increase output verbosity",
action="store_true")
args = parser.parse_args()
if args.verbose:
logging.basicConfig(level=logging.INFO)
RTDE_PROTOCOL_VERSION = 1
conf = rtde_config.ConfigFile(args.config)
output_names, output_types = conf.get_recipe('out')
con = rtde.RTDE(args.robothost, args.robotport)
con.connect()
# get controller version
con.get_controller_version()
# set protocol version
if not con.negotiate_protocol_version(RTDE_PROTOCOL_VERSION):
logging.error('Unable to negotiate protocol version')
sys.exit()
# setup recipes
if not con.send_output_setup(output_names, output_types):
logging.error('Unable to configure output')
sys.exit()
def connect_rtde(self):
self.config_filename = rospy.get_param(
"/ur_rtde_driver/config_file",
'src/ur_rtde/src/control_loop_configuration.xml'
) # Get param, otherwise default
self.logger = logging.getLogger().setLevel(logging.INFO)
#Load and parse recipe
self.conf = rtde_config.ConfigFile(self.config_filename)
self.state_names, self.state_types = self.conf.get_recipe('state')
self.setp_names, self.setp_types = self.conf.get_recipe('setp')
self.mode_names, self.mode_types = self.conf.get_recipe('mode')
self.enable_names, self.enable_types = self.conf.get_recipe('enable')
self.ping_names, self.ping_types = self.conf.get_recipe('ping')
self.avtr_names, self.avtr_types = self.conf.get_recipe('avtr')
self.servoj_params_names, self.servoj_params_types = self.conf.get_recipe(
'servoj_params')
self.setp2_names, self.setp2_types = self.conf.get_recipe('setp2')
#Define connection and connect to robot
self.con = rtde.RTDE(ROBOT_HOST, ROBOT_PORT)
self.con.connect()
# get controller version
self.con.get_controller_version()
# setup recipes
self.con.send_output_setup(self.state_names, self.state_types)
self.setp_obj = self.con.send_input_setup(self.setp_names,
self.setp_types)
self.mode_obj = self.con.send_input_setup(self.mode_names,
self.mode_types)
self.enable_obj = self.con.send_input_setup(self.enable_names,
self.enable_types)
self.avtr_obj = self.con.send_input_setup(self.avtr_names,
self.avtr_types)
self.servoj_params_obj = self.con.send_input_setup(
self.servoj_params_names, self.servoj_params_types)
self.setp2_obj = self.con.send_input_setup(self.setp2_names,
self.setp2_types)
self.ping_obj = self.con.send_input_setup(self.ping_names,
self.ping_types)
# RTDE State RECEIVE THREAD
self.cb_state(None)
rospy.Timer(rospy.Duration(0.002), self.cb_state)
self.initialize_variables()
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.servoj_params_obj)
self.con.send(self.avtr_obj)
self.con.send(self.setp2_obj)
self.con.send(self.enable_obj)
self.con.send(self.ping_obj)
#start data synchronization or exit
if not self.con.send_start():
sys.exit()
def record_ur_data(host, port, csvfilename):
#parameters
parser = argparse.ArgumentParser()
parser.add_argument('--host', default=host, help='name of host to connect to (localhost)')
parser.add_argument('--port', type=int, default=30004, help='port number (30004)')
parser.add_argument('--samples', type=int, default=0, help='number of samples to record')
parser.add_argument('--frequency', type=int, default=125, help='the sampling frequency in Herz')
parser.add_argument('--config', default='record_configuration.xml', help='data configuration file to use (record_configuration.xml)')
parser.add_argument('--output', default='robot_data.csv', help='data output file to write to (robot_data.csv)')
parser.add_argument("--verbose", help="increase output verbosity", action="store_true")
parser.add_argument("--binary", help="save the data in binary format", action="store_true")
args = parser.parse_args()
args.output = csvfilename
args.host = host
args.port = port
if args.verbose:
logging.basicConfig(level=logging.INFO)
conf = rtde_config.ConfigFile(args.config)
output_names, output_types = conf.get_recipe('out')
con = rtde.RTDE(args.host, args.port)
con.connect()
# get controller version
con.get_controller_version()
# setup recipes
if not con.send_output_setup(output_names, output_types, frequency = args.frequency):
logging.error('Unable to configure output')
sys.exit()
#start data synchronization
if not con.send_start():
logging.error('Unable to start synchronization')
sys.exit()
writeModes = 'wb' if args.binary else 'w'
with open(args.output, writeModes) as csvfile:
writer = None
if args.binary:
writer = csv_binary_writer.CSVBinaryWriter(csvfile, output_names, output_types)
else:
writer = csv_writer.CSVWriter(csvfile, output_names, output_types)
writer.writeheader()
i = 1
global keep_running
while keep_running:
timestamp = (time.time() - timestart)*1000
if i%args.frequency == 0:
if args.samples > 0:
sys.stdout.write("\r")
sys.stdout.write("{:.2%} done.".format(float(i)/float(args.samples)))
sys.stdout.flush()
else:
sys.stdout.write("\r")
sys.stdout.write("{:3d} samples.".format(i))
sys.stdout.flush()
if args.samples > 0 and i >= args.samples:
keep_running = False
try:
state = con.receive(args.binary)
if state is not None:
writer.writerow(state, timestamp)
else:
sys.exit()
except KeyboardInterrupt:
keep_running = False
i += 1
sys.stdout.write("\rComplete! \n")
con.send_pause()
con.disconnect()
def main():
####### Connect to controller
ROBOT_PORT = 30004
ROBOT_HOST = '192.168.1.50' #'localhost'
logging.getLogger().setLevel(logging.INFO)
con = rtde.RTDE(ROBOT_HOST, ROBOT_PORT)
error = con.connect()
while error != 0:
time.sleep(0.5)
error = con.connect()
print 'Connected to robot'
####### Config recipes
FREQUENCY = 500
config_filename = 'servoj_control_loop_configuration.xml'
conf = rtde_config.ConfigFile(config_filename)
state_names, state_types = conf.get_recipe('state')
set_q_names, set_q_types = conf.get_recipe('set_q')
watchdog_names, watchdog_types = conf.get_recipe('watchdog')
con.send_output_setup(state_names, state_types, FREQUENCY)
set_q = con.send_input_setup(set_q_names, set_q_types)
watchdog = con.send_input_setup(watchdog_names, watchdog_types)
####### Read joint positions from file
name = 'q_example.csv'
file = open(name, 'rb')
reader = csv.reader(file, delimiter=',')
q1 = []
q2 = []
q3 = []
q4 = []
q5 = []
q6 = []
for row in reader:
q1.append(float(row[0]))
q2.append(float(row[1]))
q3.append(float(row[2]))
q4.append(float(row[3]))
q5.append(float(row[4]))
q6.append(float(row[5]))
####### Initial joint pos
set_q.input_double_register_0 = q1[0]
set_q.input_double_register_1 = q2[0]
set_q.input_double_register_2 = q3[0]
set_q.input_double_register_3 = q4[0]
set_q.input_double_register_4 = q5[0]
set_q.input_double_register_5 = q6[0]
####### Log actual joint pos
name = 'log.csv'
csvfile = open(name, 'w')
writer = csv.writer(csvfile, delimiter=',')
#start data synchronization
if not con.send_start():
sys.exit()
watchdog.input_int_register_0 = 0
print 'Wait for the Robot to start'
PROGRAM_STARTED_AND_READY = False
while PROGRAM_STARTED_AND_READY == False:
state = con.receive()
#while state == None:
#state = con.receive()
con.send(watchdog)
if state.output_bit_registers0_to_31 == True:
PROGRAM_STARTED_AND_READY = True
print 'Send initial qs'
con.send(set_q)
time.sleep(0.01)
watchdog.input_int_register_0 = 1
con.send(watchdog)
# Wait for moveJ to finish
state = con.receive()
PROGRAM_STARTED_AND_READY = False
while PROGRAM_STARTED_AND_READY == False:
state = con.receive()
if state.runtime_state > 1:
con.send(watchdog)
if state.output_bit_registers0_to_31 == True:
PROGRAM_STARTED_AND_READY = True
print 'Control loop'
idx = 1
watchdog.input_int_register_0 = 2
con.send(watchdog)
###### control loop
while idx < len(q1):
state = con.receive()
writer.writerow([
state.actual_q[0], state.actual_q[1], state.actual_q[2],
state.actual_q[3], state.actual_q[4], state.actual_q[5]
])
if state.runtime_state > 1:
###### send to the robot
list_to_set_q(
set_q, [q1[idx], q2[idx], q3[idx], q4[idx], q5[idx], q6[idx]])
con.send(set_q)
con.send(watchdog)
idx = idx + 1
###### CERRAR
print 'End and close'
watchdog.input_int_register_0 = 3
con.send(watchdog)
time.sleep(0.01)
con.send_pause()
con.disconnect()
