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()