From a698c4a7539ee65345cdce66dcefd195e8e1a838 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Thu, 18 Apr 2024 20:33:55 -0500 Subject: [PATCH] Add more routines --- ur5_control.py | 328 ++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 257 insertions(+), 71 deletions(-) diff --git a/ur5_control.py b/ur5_control.py index d655f70..7f9ec8a 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -5,13 +5,14 @@ import math import numpy as np import time import os -import logging +#import logging import yaml -from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper import sys from util import fprint from pyModbusTCP.client import ModbusClient +import subprocess +from util import win32 @@ -33,37 +34,76 @@ class Rob(): self.limb_wrist = limbs["limb_wrist"] #self.init_arm() - def init_arm(self): - #sys.stdout = Logger() - fprint("Starting UR5 power up...") +def ping(host): + #Returns True if host (str) responds to a ping request. - # power up robot here + # Option for the number of packets as a function of + if win32: + param1 = '-n' + param2 = '-w' + param3 = '250' + else: + param1 = '-c' + param2 = '-W' + param3 = '0.25' - # wait for power up (this function runs async) + # Building the command. Ex: "ping -c 1 google.com" + command = ['ping', param1, '1', param2, param3, host] + return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0 - # trigger auto-initialize +def powerup_arm(robot): + #sys.stdout = Logger() + fprint("Starting UR5 power up...") + # power up robot here via PCB - # wait for auto-initialize - ip = self.ip - # init urx + # + # wait for power up (this function runs async) + + while not ping(robot.ip): + time.sleep(0.5) + # trigger auto-initialize + fprint("Arm online. Waiting for calibration.") + # wait for auto-initialize +def connect(robot): + if robot.robot is None: + newrobot = Rob(robot.config) + robot = newrobot + ip = robot.ip fprint("Connecting to arm at " + ip) trying = True while trying: try: - self.robot = urx.Robot(ip) + robot.robot = urx.Robot(ip) trying = False except: time.sleep(1) - # Sets robot arm endpoint offset (x,y,z,rx,ry,rz) - self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0)) - + robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0)) # Set weight - self.robot.set_payload(2, (0, 0, 0.1)) - #rob.set_payload(2, (0, 0, 0.1)) - time.sleep(0.2) - fprint("UR5 ready.") + robot.robot.set_payload(2, (0, 0, 0.1)) + return robot +def init_arm(robot): + robot = connect(robot) + + # init urx + + #rob.set_payload(2, (0, 0, 0.1)) + time.sleep(0.2) + fprint("UR5 ready.") + #return robot.robot + + # setup - in case of fail. open gripper, move up, then go home. + rob = robot.robot + open_gripper() + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] += 0.025 + rob.movel(new_pos, vel=0.05, acc=1) + + move_to_home(robot, speed=0.5) + + return True def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None): rob = robot.robot @@ -205,7 +245,7 @@ def move_to_polar(robot, start_pos, end_pos): return rx_intermediate -def move_to_home(robot): +def move_to_home(robot, speed=2): rob = robot.robot # Home position in degrees @@ -217,7 +257,25 @@ def move_to_home(robot): 1.675098295930943] # Move robot - rob.movej(home_pos, acc=2, vel=2) + rob.movej(home_pos, acc=2, vel=speed) + return True + +def move_to_packup(robot, speed=0.25): + rob = robot.robot + + # known good starting point to reach store position + goto_holder_index(robot, 12, 0.3, flip=False, use_closest_path=False) + + # Home position in degrees + store_pos = [-1.5708, + -1.3, + 2.362, + 0.7056, + -1.425, + 1.5708] + # Move robot + rob.movej(store_pos, acc=0.1, vel=speed) + return True def normalize_degree(theta): # Normalizes degree theta from -1.5pi to 1.5pi @@ -346,7 +404,7 @@ def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): rob.movejs(joints, acc=2, vel=2, radius=0.1) -def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True): +def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True, rzoffset=0): # gripper angle: from vertical # gripper length: from joint to start of grip # to flip, you can use flip=True or make gripper angle negative @@ -365,7 +423,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0 gripperx -= (1-math.cos(gripperangle)) * limb3 rz = math.pi / 2 # flip the whole wrist - return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path) else: gripperangle = math.radians(gripperangle) @@ -387,6 +445,7 @@ def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_close #rob.movej(angles, acc=2, vel=2) #return angles #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, ) + return True def is_flipped(robot): rob = robot.robot @@ -415,8 +474,9 @@ def flip(robot): safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0] # Flip at safe position - rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position - rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=4, acc=3) # Move to safe position + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=4, acc=3) # Flip gripper + return True def safe_move(robot, x, y, z, use_closest_path=True): rob = robot.robot @@ -427,15 +487,17 @@ def safe_move(robot, x, y, z, use_closest_path=True): if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)): flip(robot) - rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2) + rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3) + return True -def pick_up_routine(robot, holder_index, verbose=False): +def pick_up_routine(robot, holder_index, slot=None, verbose=False): + robot = connect(robot) rob = robot.robot if verbose: - print('Pickup routine for index', holder_index) + fprint('Pickup routine for index' + str(holder_index)) - goto_holder_index(robot, holder_index, 0.2, use_closest_path=False) + goto_holder_index(robot, holder_index, 0.05, use_closest_path=False) open_gripper() curr_pos = rob.getl() @@ -448,22 +510,153 @@ def pick_up_routine(robot, holder_index, verbose=False): new_pos[2] = 0.2 - rob.movel(new_pos, vel=0.1, acc=1) + rob.movel(new_pos, vel=2, acc=1) was_flipped = is_flipped(robot) - # # Tray position 1 - rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2) - rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2) - time.sleep(0.5) + + #move_to_home(robot, speed=4) + + # starting pos + angles = (-47.04,-115.81,138.33,-58.11,-56.55,-68.03) + angles = [x*math.pi/180 for x in angles] + rob.movej(angles,vel=1,acc=0.5) + + angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39) + angles = [x*math.pi/180 for x in angles] + rob.movej(angles,vel=1,acc=0.5) + + if slot is None: + slot = 0 + if slot == 0: + angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15) + angles = [x*math.pi/180 for x in angles] + if slot == 1: + angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72) + angles = [x*math.pi/180 for x in angles] + if slot == 2: + angles = (10.39,-70.86,133.95,7.83,30.91,-159.00) + angles = [x*math.pi/180 for x in angles] + if slot == 3: + angles = (13.31,-76.23,140.78,1.90,31.98,-153.76) + angles = [x*math.pi/180 for x in angles] + + rob.movej(angles,vel=1,acc=0.5) + open_gripper() + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] += 0.04 + rob.movel(new_pos, vel=0.1, acc=1) + + angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39) + angles = [x*math.pi/180 for x in angles] + rob.movej(angles,vel=1,acc=0.5) + + move_to_home(robot, speed=3) + # # Tray position 1 + # if slot is None: + # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) + # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3) + # open_gripper() + # else: + # xoffset = 0.051 * slot + # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) + # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=60, flip=was_flipped, use_closest_path=False,rzoffset=math.pi), vel=4, acc=3) + # #rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0, math.pi/2, 0.1), vel=4, acc=3) + # open_gripper() + # Back to safe position - rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2) + #time.sleep(10) + #rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=4, acc=3) + return True + +def return_routine(robot, slot, holder_index=None, verbose=False): + robot = connect(robot) + rob = robot.robot + + + open_gripper() + was_flipped = is_flipped(robot) + if slot is None: + rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) + rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3) + close_gripper() + else: + xoffset = 0.051 * slot + rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) + rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3) + close_gripper() + + + if holder_index is not None: + goto_holder_index(robot, holder_index, 0.2, use_closest_path=False) + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] = 0.015 + rob.movel(new_pos, vel=0.1, acc=1) + open_gripper() + new_pos[2] = 0.1 + rob.movel(new_pos, vel=2, acc=1) + return True + else: + # go to camera + rob.movej(offset_gripper_angle(robot, 0.35, -0.35, 0.3, flip=was_flipped, use_closest_path=False), vel=2, acc=2) + return True + + +def to_camera(robot, holder_index, verbose=False): + robot = connect(robot) + fprint("Bringing tube at " + str(holder_index) + " to camera") + rob = robot.robot + + goto_holder_index(robot, holder_index, 0.05, use_closest_path=False) + + open_gripper() + + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] = 0.005 + rob.movel(new_pos, vel=0.1, acc=1) + # goto_holder_index(robot, holder_index, 0.0) + close_gripper() + + + + new_pos[2] = 0.2 + rob.movel(new_pos, vel=2, acc=1) + was_flipped = is_flipped(robot) + + # start inwards + rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2) + + # go to camera + rob.movej(offset_gripper_angle(robot, 0.3, -0.2, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2) + return True + +def return_camera(robot, holder_index, verbose=False): + robot = connect(robot) + rob = robot.robot + + # move inwards first + rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2) + goto_holder_index(robot, holder_index, 0.2, use_closest_path=False) + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] = 0.015 + rob.movel(new_pos, vel=0.1, acc=1) + open_gripper() + new_pos[2] = 0.1 + rob.movel(new_pos, vel=2, acc=1) + def open_gripper(): - print("opening") + fprint("Opening gripper") c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False) + c.write_single_register(0, 0b0) + c.write_single_register(435, 0b1) + c.write_single_register(0, 0b0) c.write_single_register(435, 0b1) time.sleep(0.5) @@ -472,8 +665,11 @@ def open_gripper(): #c.close() def close_gripper(): - print("closing") + fprint("Closing gripper") c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False) + c.write_single_register(435, 0b0) + c.write_single_register(0, 0b1) + c.write_single_register(435, 0b0) c.write_single_register(0, 0b1) time.sleep(0.5) @@ -489,44 +685,34 @@ if __name__ == "__main__": #global config config = yaml.safe_load(fileread) - open_gripper() - #time.sleep(5) + #open_gripper() + #time.sleep(1) #close_gripper() - - #time.sleep(100) - - - + + robot = Rob(config) # robot of type Rob is the custom class above - robot.init_arm() + #powerup_arm(robot) + robot = connect(robot) + init_arm(robot) + rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously + + move_to_home(robot, speed=4) + + #time.sleep(3) + #move_to_packup(robot) + + pick_up_routine(robot, 0, 0, True) + + #move_to_home(robot) + + #return_routine(robot, 0, 0, True) + + + time.sleep(100) print("Current tool pose is: ", rob.getl()) - move_to_home(robot) - - home_pose = [-0.4999999077032916, - -0.2000072960336574, - 0.40002172976662786, - 0, - -3.14152741295329, - math.radians(62)] - - # time.sleep(.5) - - p1 = [0, - - 0.6, - .4, - 0.2226, - 3.1126, - 0.0510] - p2 = [0.171, - -0.115, - 0.2, - 0.2226, - 3.1126, - 0.0510] curr_pos = rob.getl() @@ -545,8 +731,8 @@ if __name__ == "__main__": # rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2) # goto_holder_index(robot, 27, 0.05) - for i in [6,7,8]: - pick_up_routine(robot, i, verbose=True) + #for i in [6,7,8]: + #pick_up_routine(robot, i, verbose=True)