def get_Tc2r(self): rtde_c = rtde_control.RTDEControlInterface("192.168.178.15") rtde_r = rtde_receive.RTDEReceiveInterface("192.168.178.15") print("[INFO] AQUIRING ROBOT TO CAMERA TRANSFORMATION") print("[INFO] 1. move robot to idle position (jointq)") print("[INFO] 2. aquire an image with 3 aruco markers on it.") rtde_c.moveJ(joint_q) TCPpose = rtde_r.getActualTCPPose() print("[INFO] 1. move robot to idle position (jointq)") print("[INFO] starting camera") self._setup_camera() warmup_counter = 0 while True: self.frame = self.wait_for_frames() if warmup_counter > self.warmup_length: align = rs.align(rs.stream.color) aligned = align.process(frames) c_frames = aligned.get_color_frame() img = np.asanyarray(c_frames.get_data()) img, rvec, tvec = """ jog ur5 https://www.universal-robots.com/articles/ur/programming/how-to-jog-the-robot/ teach mode for freedrive start and stop https://sdurobotics.gitlab.io/ur_rtde/api/api.html """ pass
def main(): ur_ctrl = rtde_control.RTDEControlInterface(rsd_conf.UR_IP) ur_ctrl.moveJ(q.IDLE) ur_modbus_client = ModbusTcpClient(rsd_conf.UR_IP) ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, False) pi_modbus_client = ModbusTcpClient(rsd_conf.PI_IP, rsd_conf.PI_MODBUS_PORT) def gripper_grasp(grasp_q, pre_post_q): ur_ctrl.moveJ(pre_post_q) ur_ctrl.moveJ(grasp_q) ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, True) ur_ctrl.moveJ(pre_post_q) def gripper_release(): ur_modbus_client.write_coil(16 + rsd_conf.IO_GRIPPER, False) order = {"blue": 0, "red": 0, "yellow": 20} for brick_color_id in range(len(rsd_conf.BRICK_COLORS)): brick_color = rsd_conf.BRICK_COLORS[brick_color_id] q_grasp_brick, q_above_brick = q.GRASP_BRICKS[ brick_color_id], q.ABOVE_BRICKS[brick_color_id] count = order[brick_color] for _ in range(count): gripper_grasp(q_grasp_brick, q_above_brick) ur_ctrl.moveJ(q.ABOVE_CAMERA) pi_modbus_client.read_input_registers(0, 1) ur_ctrl.moveJ(q.BRICK_DROP_ORDER_BOX) gripper_release() ur_ctrl.moveJ(q.IDLE)
def __init__(self, IP="192.168.92.99", direction=False): self.__rtde_r = rtde_receive.RTDEReceiveInterface(IP) self.__rtde_c = rtde_control.RTDEControlInterface(IP) self.__direction = direction self.__initialPos = [137.57 / 180 * np.pi, -114.70 / 180 * np.pi, 124.21 / 180 * np.pi, -100.04 / 180 * np.pi, 91.38 / 180 * np.pi, 23.93 / 180 * np.pi] self.angle = 0 self.track = LineTrack(video=0, mode=1) self.tool = Tool(dev="/dev/ttyUSB1")
def main(): print("Starting Control... <" + str(os.getpid()) + ">") global rtde_c rtde_c = rtde_control.RTDEControlInterface("192.168.2.66") rtde_r = rtde_receive.RTDEReceiveInterface("192.168.2.66") rtde_c.setTcp(TCP_FRAME) rtde_c.setPayload(0.5, [0,0,0]) while(True): cook, meat = getNextSteak() if cook != None and meat != None: getSteak(meat) place_time = time.time() placeSteak() delta_time = time.time() - place_time thickness = measureThickness(rtde_c, rtde_r) print("THICKNESS: " + str(thickness)) first_time, second_time = getCookTimes(thickness, cook) print("TIMES: < " + str(first_time) + ", " + str(second_time) + " >") rtde_c.moveJ(HOME) if first_time > 0 and second_time > 0: turn1 = threading.Timer(first_time/2.0 - delta_time, turnSteak) turn1.start() flip = threading.Timer(first_time - delta_time, flipSteak) flip.start() turn2 = threading.Timer(first_time + second_time/2.0 - delta_time, turnSteak) turn2.start() finish = threading.Timer(first_time + second_time - delta_time, finishSteak) finish.start() bar = progressbar.ProgressBar(max_value=1.0) while(True): if turn1.is_alive() or turn2.is_alive() or finish.is_alive() or flip.is_alive(): bar.update( min( ( time.time() - place_time ) / ( first_time + second_time ) , 1.0 ) ) time.sleep(1) else: print("MmmmMmMMMMMmmm") break returnSteak() else: print("Measurement Error") os.remove("steak.yaml")
def main(): parser = argparse.ArgumentParser() parser.add_argument('--ur-ip', type=str, required=True) parser.add_argument('--n', type=int, default=3) parser.add_argument('--grid-size', type=float, default=0.05) parser.add_argument('--tcp-tool-offset', type=float, default=0.012) args = parser.parse_args() ur_ip = args.ur_ip n = args.n grid_size = args.grid_size tcp_tool_offset = args.tcp_tool_offset rtde_c = rtde_control.RTDEControlInterface(ur_ip) rtde_r = rtde_receive.RTDEReceiveInterface(ur_ip) rtde_c.teachMode() table_poses = [] input( 'Insert the calibration tool close to the table origin with aligned tool axes' ) table_poses.append(get_stable_table_pose(rtde_c, rtde_r)) table_offset_x = (int(input('How many holes to the table origin (x)?')) + 0.5) * grid_size table_offset_y = (int(input('How many holes to the table origin (y)?')) + 0.5) * grid_size for i in range(1, n): input( 'Insert the calibration tool at another position. ({}/{})'.format( i + 1, n)) table_poses.append(get_stable_table_pose(rtde_c, rtde_r)) pts_base = np.array([T @ (0, 0, tcp_tool_offset) for T in table_poses]) # estimate table_T_base just from the first table pose table_T_base = table_poses[0] @ Transform( t=(-table_offset_x, -table_offset_y, tcp_tool_offset)) base_T_table = table_T_base.inv() # find the acutal positions in the table-coordinates pts_table = np.array([ base_T_table @ pose @ (0, 0, tcp_tool_offset) for pose in table_poses ]) # nearest possible table positions pts_table[:, 2] = 0 pts_table[:, :2] = np.round(((pts_table[:, :2] - grid_size / 2) / grid_size)) * grid_size + grid_size / 2 base_T_table = kabsch(pts_base, pts_table) print(base_T_table)
def __init__(self, ur_ipaddress=None, speed_ms=0.25, accel_mss=0.5): if ur_ipaddress == None: self.ur_ipaddress = "10.3.15.240" else: self.ur_ipaddress = ur_ipaddress self.urrobot_r = rtde_receive.RTDEReceiveInterface(self.ur_ipaddress) self.r_control = rtde_control.RTDEControlInterface(self.ur_ipaddress) self.HOST = self.ur_ipaddress # The UR IP address self.PORT = 30003 # UR secondary client self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.s.connect((self.HOST, self.PORT)) self.speed_ms = speed_ms self.accel_mss = accel_mss self.path = []
def main(): print("Starting Control...") rtde_c = rtde_control.RTDEControlInterface("192.168.2.66") rtde_c.setTcp([0.0, 0.0, 0.05, 0.0, 0.0, 0.0]) rtde_c.setPayload(0.1, [0, 0, 0]) while (True): updateSettings() if state: rtde_c.moveJ([-1.5707, -2.26893, 2.26893, -3.22886, -1.5707, 0]) for p in traps: if do_traps and state: makeMove(rtde_c, p) else: break if do_traps and state: makeMove(rtde_c, p, True) else: break for p in lats: if do_lats and state: makeMove(rtde_c, p) else: break if do_lats and state: makeMove(rtde_c, p, True) else: break for p in erects_tehe: if do_erector and state: makeMove(rtde_c, p) else: break if do_erector and state: makeMove(rtde_c, p, True) else: break else: rtde_c.moveJ([-1.5707, -3.14, 0.0, -3.22886, -1.5707, 0]) print("Done")
def main(): rtde_c = rtde_control.RTDEControlInterface(args.IP) rtde_r = rtde_receive.RTDEReceiveInterface(args.IP) im = Image.open(args.Image) px = im.convert('RGB') pose_to_pixel = max(im.width / args.Width, im.height / args.Height) print(pose_to_pixel) led = RGBLED(23, 24, 25) rtde_c.setTcp([0, 0, 0, 0, 3.14159, 0]) image_complete = False current_target = None move_target = None row_num = 0 while (True): current_pose = rtde_r.getActualTCPPose() updatePixel(current_pose, pose_to_pixel, px, led, [args.Width / 2.0, args.Height / 2.0]) if movementComplete(move_target, current_pose): current_target, image_complete, row_num = nextMove( rtde_c, current_target, args.Width, args.Height, args.Spacing, args.Overhang, row_num) if not image_complete: move_target = rtde_c.poseTrans([ -args.Width / 2, -args.Distance, args.Height / 2, 0, 0, -1.5707 ], current_target) rtde_c.moveL(move_target, args.Speed, 0.5, True) else: break time.sleep(1.0 / args.Rate) rtde_c.stopScript()
import rtde_control import rtde_receive import time from std_msgs.msg import String from copy import deepcopy UR_IP = "192.168.131.40" rtde_c = rtde_control.RTDEControlInterface( UR_IP, rtde_control.RTDEControlInterface.FLAG_VERBOSE | rtde_control.RTDEControlInterface.FLAG_USE_EXT_UR_CAP) rtde_r = rtde_receive.RTDEReceiveInterface(UR_IP) inp = input("Fold the robot? y/n: ")[0] if (inp == 'y'): folded_joints = [1.602, -2.869, 2.683, -2.869, -1.584, -0.001] rtde_c.moveJ(folded_joints, 0.1, 0.01) else: print("Skipping")
rst = rtde_c.zeroFtSensor() print(rst, "=== Force Reset") for i in range(5): contact = rtde_r.getActualTCPForce() print("Current Force Values ", contact) # print("Forces #",i," ->",contact,rtde_c.isSteady()) time.sleep(0.5) # while (contact[2] > 20): # contact = rtde_r.getActualTCPForce() # stopRobt() ##################################################### # UR COMM CODE HOST = "10.0.0.6" # The remote host PORT = 30002 rtde_c = rtde_control.RTDEControlInterface(HOST) rtde_r = rtde_receive.RTDEReceiveInterface(HOST) ################################################### v = 2 a = 0.3 blend =0.00 print("Connection: ",rtde_c.isConnected()) # coordinates move_joints_1 = [-0.5,-0.22,0.3,2.89,-1.29,0.126] move_joints_2 = [-0.5,-0.22,0.4,2.89,-1.29,0.126] drop = [ 0.145, -0.623, 0.0479, 3.09, 0.0169, -0.041] placing = [ 0.116, -0.559, 0.4300, 3.10, 0.0169, -0.050] # placing1 = [-0.252, -0.511, 0.4300, 2.95, -0.9491, -0.063]
#!/usr/bin/env python import rtde_control import time ip = "127.0.0.1" # ip = "172.16.101.225" rtde_c = rtde_control.RTDEControlInterface(ip) dt = 1.0 / 500 # 2ms joint_q = [0, -1.57, 1.57, 0, 1.57, 3.14] # Move to initial joint position with a regular moveJ rtde_c.moveJ(joint_q, 0.2, 0.2, False) rtde_c.stopScript()
import math import numpy as np import rtde_control import rtde_receive rtde_r = rtde_receive.RTDEReceiveInterface("192.168.178.15") actual_q = np.array(rtde_r.getActualQ()) * 180 / (2 * math.pi) rtde_c = rtde_control.RTDEControlInterface("192.168.178.15") modified_q = actual_q modified_q[0] = 0 from rtde_control import RTDEControlInterface as RTDEControl rtde_c = RTDEControl("192.168.178.15", RTDEControl.FLAG_USE_EXT_UR_CAP) rtde_c.moveL(modified_q, 0.1, 0.1) print(actual_q)
from math import pi import rtde_control import time JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] Q1 = [2.2,-0,-1.57,0,0,0] Q2 = [1.5,0,-1.57,0,0,0] Q3 = [1.5,-0.2,-1.57,0,0,0] Q4 = [0,-pi/2,0,-pi/2,0,0] QNum = 60 TNUM = 5 Q5 = np.zeros((6,QNum)) client = None rtde_c = rtde_control.RTDEControlInterface("127.0.0.1") # Parameters acceleration = 0.5 dt = 1.0/500 # 2ms joint_q = [-1.54, -1.83, -2.28, -0.59, 1.60, 0.023] joint_speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Move to initial joint position with a regular moveJ rtde_c.moveJ(joint_q) # Move to initial joint position with a regular moveJ rtde_c.moveJ(joint_q) def OnceMove(): g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES
import rtde_control from rsd import conf from rsd.robot import q import time ur_ctrl = rtde_control.RTDEControlInterface(conf.UR_IP) ur_ctrl.moveJ(q.IDLE) time.sleep(5)
def resrart(self, ip): self.__rtde_r = rtde_receive.RTDEReceiveInterface(ip) self.__rtde_c = rtde_control.RTDEControlInterface(ip)
def __init__(self, robot_ip): self.vel = 0.15 self.acc = 0.5 self.stop_acc = 0.3 self.cmd_velocity_vector = [] self.move_vel = False self.item_height = 0.11 self.robot_c = rtde_control.RTDEControlInterface( robot_ip) #urx.Robot(robot_ip, True) self.robot_r = rtde_receive.RTDEReceiveInterface( robot_ip) #urx.Robot(robot_ip, True) self.robot_io = rtde_io.RTDEIOInterface( robot_ip) #urx.Robot(robot_ip, True) self.my_tcp = m3d.Transform() # create a matrix for our tool tcp self.current_TCP = 'TCP' self.set_TCP('pressure_ft') # self.robot.set_payload(4.25) # self.robot.set_gravity([0, 0, 0.15]) time.sleep(0.2) self.ros_node = rospy.init_node('ur10_node', anonymous=True) self.pose_publisher = rospy.Publisher('tcp_pose', PoseStamped, queue_size=1) self.velocity_publisher = rospy.Publisher('/dvs/vel', Twist, queue_size=1) self.speed_publisher = rospy.Publisher('/dvs/spd', Float64, queue_size=1) self.cam_pose_publisher = rospy.Publisher('/dvs/pose', PoseStamped, queue_size=1) self.cmd_vel_subs = rospy.Subscriber("ur_cmd_vel", Twist, self.move_robot_callback, queue_size=1) self.cmd_pose_subs = rospy.Subscriber("ur_cmd_pose", Pose, self.move_pose_callback) self.cmd_adjust_pose_subs = rospy.Subscriber("ur_cmd_adjust_pose", Pose, self.adjust_pose_callback) self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee", Float64, self.angle_callback) self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee_x", Float64, self.angle_callback_x) self.pressure_movement_subs = rospy.Subscriber("move_pressure_to_cam", Bool, self.move_PF_to_cam) self.pickup_service = rospy.Service("ur_pickup", Empty, self.pick_item) self.set_tcp_service = rospy.Service("set_TCP", desiredTCP, self.set_TCP_cb) self.move_service = rospy.Service('move_ur', moveUR, self.moveUR_cb) self.move_service = rospy.Service('fire_drill', fireDrill, self.fire_drill_cb) self.rate = rospy.Rate(100) #TF self.tfBuffer = tf2_ros.Buffer() self.tl = tf2_ros.TransformListener(self.tfBuffer) self.br = tf2_ros.TransformBroadcaster() self.brs = tf2_ros.StaticTransformBroadcaster() self.robot_pose = PoseStamped() self.camera_pose = PoseStamped() self.prev_camera_pose = PoseStamped() self.pressure_ft_pose = PoseStamped() self.cam_vel = Twist() self.cam_speed = Float64() self.seq = 1 self.pose = [] self.initial_pose = [] self.center_pose = [] self.static_transform_list = [] self.setup_tf()
def __init__(self, address="172.31.1.25", debug = 1, enable_force = 1, file_name=None, folder_name="futek_data"): self._debug = debug self._enable_force = enable_force self.rob_c = rtde_control.RTDEControlInterface(address) manager = Manager() self.glob = manager.Namespace() # shared memory for receiving data from manupulator self.X_cur_shared = Array('d',[0,0,0,0,0,0]) self.dX_cur_shared = Array('d',[0,0,0,0,0,0]) p = Process(target=self.__read_data_from_manip,args=(address,)) p.start() # shared memory self.Xd_shared = Array('d',[0,0,0]) self.dXd_shared = Array('d',[0,0,0]) self.__force_from_sensor = Value('d',0.0) self.term_process = Value('i',0) # shared memory self.Xg_shared = Array('d', [0, 0, 0]) self.dXg_shared = Array('d', [0, 0, 0]) self.Fd_ideal_shared = Value('d', 0.0) self.Fd_real_shared = Value('d', 0.0) self.start_vel_control = Value('i', 0) # For plotting self.glob.x_act = [] self.glob.x_des = [] self.glob.time = [] self.glob.force_data = [] # vel and acc constraints self.__max_operational_acc = 0.4 self.__max_lin_acc = 0.5 self.__max_lin_vel = 0.5 # modification trajectory tune params self.__vir_stiff = 400000 self.__freq_mod_traj = 500 # vel control parameters self.__freq_log = 500 self.__k = 0.99 # sensor parameters self.force_threshold = 1 # todo self._common_orient=[3.14,0.1,0] self.starting_pose = [-0.6284734457983073, 0.04110124901844167, 0.24322792682955954, 2.885542842994124, -0.09630215284222861, -0.8197553730344054] if self._enable_force: self.__start_futek_sensor(file_name,folder_name)
"Sample", "X", "Y", "Z", "Rx", "Ry", "Rz", "x1", "x2", "x3", "y1", "y2", "y3", "z1", "z2", "z3", "Picked correctly" ] for item in headers: worksheet.write(row, column, item) column += 1 rospy.init_node('test_results_picking_node') base_frame = "base" tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) broadcaster = tf2_ros.TransformBroadcaster() static_broadcaster = tf2_ros.StaticTransformBroadcaster() rtde_c = rtde_control.RTDEControlInterface("192.168.12.245") rtde_r = rtde_receive.RTDEReceiveInterface("192.168.12.245") tool_frame = "wrist_3_link" tf_transform_base_component = tfBuffer.lookup_transform( base_frame, "tool0", rospy.Time(), rospy.Duration(2.0)) t, rot_vector, y_axis_original = get_t_rotvector_component( tf_transform_base_component=tf_transform_base_component) t = [ tf_transform_base_component.transform.translation.x, tf_transform_base_component.transform.translation.y, tf_transform_base_component.transform.translation.z ] quaternion = [
import rtde_control rtde_c = rtde_control.RTDEControlInterface("192.168.0.100") res = rtde_c.moveL([-0.143, -0.435, 0.10, 0, 0, 0], 0.5, 0.3) res = rtde_c.moveL([-0.143, -0.435, 0.20, 0, 0, 0], 0.5, 0.3) res = rtde_c.moveL([-0.143, -0.435, 0.30, 0, 0, 0], 0.5, 0.3) print("result: " + str(res))
import rtde_receive import rtde_control robot_ip = "192.168.100.2" rtde_r = rtde_receive.RTDEReceiveInterface(robot_ip) rtde_c = rtde_control.RTDEControlInterface(robot_ip) def go_to_next(): anw = input("Go to next [y]:") if anw == "y": return else: exit() speed = 0.25 acc = 1 go_to_next() fist_pose = [ -0.24126775044997928, -0.42075908202903856, 0.6586334885483427, 2.599420154499895, 0.09288735719897556, -0.6802083104833498 ] rtde_c.moveP(fist_pose, speed, acc, 0.0) go_to_next() overRockwool_pos = [ -0.08203968530815126, -0.5326926026406975, 0.3, -3.0392028741044914,
def setup(self): """ Connects Arm Movement to the UR5 and the Server. Starts up the rest of the functionality for the class. Args: None. Returns: None. """ connected = False conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM) rtde_c = 0 rtde_r = 0 #Connect to arm while not (connected): try: rtde_c = rtde_control.RTDEControlInterface(self.armIP) rtde_r = rtde_receive.RTDEReceiveInterface(self.armIP) connected = True except Exception as e: print(e) finally: time.sleep(1) #Move arm to home position # Connect to server. connected = False while not (connected): try: conn.connect((self.serverIP, self.serverPORT)) connected = True except Exception as e: print(e) finally: time.sleep(1) q = Queue() t1 = threading.Thread(target=self.moniterUserInput, args=( q, conn, )) t2 = threading.Thread(target=self.TCPwrapper, args=( q, conn, rtde_c, rtde_r, )) t1.start() t2.start() t1.join() t2.join()