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 __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(): 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_receive from rsd import conf ur_rcv = rtde_receive.RTDEReceiveInterface(conf.UR_IP) print(tuple(ur_rcv.getActualQ()))
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()
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")
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] home = [-0.537, -0.193, 0.4300, 2.40, -2.0000, -0.079] # We might not need this
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)
#!/usr/bin/env python # -*- coding: utf-8 -*- from __future__ import print_function import rtde_control import rtde_receive import rospy import time import sys import threading from threading import _Timer from Queue import Queue from sensor_msgs.msg import JointState # from omni_msgs.msg import OmniState rtde_r = rtde_receive.RTDEReceiveInterface("192.168.0.104") actualQ = Queue() actualV = Queue() actualC = Queue() targetQ = Queue() targetV = Queue() targetC = Queue() TouchQ = Queue() timestart = time.time() timeinversial = time.time() a_ur_joint_degree = open("ur_joint_actual_degree.txt", mode='w') a_ur_joint_degree.write( 'time shoulder_pan_joint shoulder_lift_joint elbow_joint wrist_1_joint wrist_2_joint wrist_3_joint\n' ) a_ur_joint_velocity = open("ur_joint_actual_velocity.txt", mode='w') a_ur_joint_velocity.write(
#!/usr/bin/env python # license removed for brevity import rospy from std_msgs.msg import Float64 import rtde_io import rtde_control import rtde_receive import time pi = 3.141592654 rtde_r = rtde_receive.RTDEReceiveInterface("0.0.0.0") init_q = rtde_r.getActualQ() target = rtde_r.getJointMode() def Joints(): pub1 = rospy.Publisher('B', Float64, queue_size=10) pub2 = rospy.Publisher('S', Float64, queue_size=10) pub3 = rospy.Publisher('E', Float64, queue_size=10) rospy.init_node('UR10', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): target = rtde_r.getActualQ() B = target[0] * 57.29 S = target[1] * 57.29 E = target[2] * 57.29 rospy.loginfo("Joints") rospy.loginfo("Base: %s, Shoulder: %s, Elbow: %s", B, S, E) pub1.publish(B) pub2.publish(S)
import rtde_io import rtde_receive import time from gpiozero import RGBLED rtde_receive_ = rtde_receive.RTDEReceiveInterface("192.168.2.66") led = RGBLED(23, 24, 25) while True: try: if rtde_receive_.getDigitalOutState(0): led.value = (0,1,0) else: led.value = (0,0,0) time.sleep(0.1) except: break
def resrart(self, ip): self.__rtde_r = rtde_receive.RTDEReceiveInterface(ip) self.__rtde_c = rtde_control.RTDEControlInterface(ip)
"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 = [ tf_transform_base_component.transform.rotation.x,
def __read_data_from_manip(self,address): self.rob_r = rtde_receive.RTDEReceiveInterface(address) while True: self.X_cur_shared[:] = self.rob_r.getActualTCPPose() self.dX_cur_shared[:] = self.rob_r.getActualTCPSpeed()
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()