From 3de59f598564ccb5a95a77cb238809f9e5aa2a60 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Sun, 24 Mar 2024 15:31:58 -0500 Subject: [PATCH] Convert ur5_control to class based (untested) --- config.yml | 11 ++++ run.py | 7 ++- ur5_control.py | 164 +++++++++++++++++++++++++++---------------------- 3 files changed, 107 insertions(+), 75 deletions(-) diff --git a/config.yml b/config.yml index a285918..fad6678 100644 --- a/config.yml +++ b/config.yml @@ -6,6 +6,17 @@ core: arm: ip: 192.168.1.145 + tool: + offset_x: 0 + offset_y: 0 + offset_z: 0.14 + limbs: + limb_base: 0.105 + limb1: 0.425 + limb2: 0.39225 + limb3: 0.1 + limb_wrist: 0.0997 + #cable_map: cameras: diff --git a/run.py b/run.py index 37b10d8..720140b 100755 --- a/run.py +++ b/run.py @@ -12,6 +12,7 @@ from util import fprint from util import run_cmd import sys import ur5_control +from ur5_control import Rob import os import signal import socket @@ -37,6 +38,7 @@ killme = None serverproc = None camera = None ledsys = None +arm = None to_server_queue = Queue() from_server_queue = Queue() @@ -233,8 +235,9 @@ def setup_server(pool): global arm_ready global serverproc global camera - - pool.apply_async(ur5_control.init, (config["arm"]["ip"],), callback=arm_start_callback) + global arm + arm = Rob(config) + pool.apply_async(arm.init_arm, callback=arm_start_callback) global ledsys ledsys = LEDSystem() pool.apply_async(ledsys.init, callback=led_start_callback) diff --git a/ur5_control.py b/ur5_control.py index 38f9f58..d4aa97e 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -14,46 +14,59 @@ from util import fprint -rob = None -offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset -limb_base, limb1, limb2, limb3, limb_wrist = (0.105, .425, .39225, .1, .0997) # Limb lengths -def init(ip): - global rob - #sys.stdout = Logger() - fprint("Starting UR5 power up...") - - # power up robot here +class Rob(): + robot = None + #offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset + # + def __init__(self, config): + armc = config["arm"] + self.ip = armc["ip"] + tool = armc["tool"] + limbs = armc["limbs"] + self.offset_x, self.offset_y, self.offset_z = (tool["offset_x"], tool["offset_y"], tool["offset_z"]) + self.limb_base = limbs["limb_base"] + self.limb1 = limbs["limb1"] + self.limb2 = limbs["limb2"] + self.limb3 = limbs["limb3"] + self.limb_wrist = limbs["limb_wrist"] + #self.init_arm() - # wait for power up (this function runs async) + def init_arm(self): + #sys.stdout = Logger() + fprint("Starting UR5 power up...") + + # power up robot here + + # wait for power up (this function runs async) - # trigger auto-initialize + # trigger auto-initialize - # wait for auto-initialize + # wait for auto-initialize + ip = self.ip + # init urx + fprint("Connecting to arm at " + ip) + trying = True + while trying: + try: + self.robot = urx.Robot(ip) + trying = False + except: + time.sleep(1) + robotiqgrip = Robotiq_Two_Finger_Gripper(rob) - # init urx - fprint("Connecting to arm at " + ip) - trying = True - while trying: - try: - rob = urx.Robot(ip) - trying = False - except: - time.sleep(1) - robotiqgrip = Robotiq_Two_Finger_Gripper(rob) + # 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)) - # Sets robot arm endpoint offset (x,y,z,rx,ry,rz) - rob.set_tcp((0, 0, 0.15, 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.") - # Set weight - rob.set_payload(2, (0, 0, 0.1)) - #rob.set_payload(2, (0, 0, 0.1)) - time.sleep(0.2) - fprint("UR5 ready.") - -def set_pos_abs(x, y, z, xb, yb, zb, threshold=None): - global rob +def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None): + rob = robot.robot new_orientation = m3d.Transform() new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis @@ -69,8 +82,8 @@ def set_pos_abs(x, y, z, xb, yb, zb, threshold=None): #rob.speedj(0.2, 0.5, 99999) rob.set_pose(new_trans, acc=2, vel=2, command="movej", threshold=threshold) # apply the new pose -def set_pos_rel_rot_abs(x, y, z, xb, yb, zb): - global rob +def set_pos_rel_rot_abs(robot, x, y, z, xb, yb, zb): + rob = robot.robot new_orientation = m3d.Transform() new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis @@ -87,8 +100,8 @@ def set_pos_rel_rot_abs(x, y, z, xb, yb, zb): #rob.speedj(0.2, 0.5, 99999) rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej") # apply the new pose -def set_pos_abs_rot_rel(x, y, z, xb, yb, zb): - global rob +def set_pos_abs_rot_rel(robot, x, y, z, xb, yb, zb): + rob = robot.robot new_orientation = m3d.Transform() new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis @@ -134,8 +147,8 @@ def polar_to_cartesian(r, theta): return x, y -def move_to_polar(start_pos, end_pos): - global rob +def move_to_polar(robot, start_pos, end_pos): + rob = robot.robot # Convert to polar coordinates start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1]) @@ -192,8 +205,8 @@ def move_to_polar(start_pos, end_pos): return rx_intermediate -def move_to_home(): - global rob +def move_to_home(robot): + rob = robot.robot # Home position in degrees home_pos = [0.10421807948612624, @@ -218,13 +231,14 @@ def normalize_degree(theta): # Return angle return normalized_theta -def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0): +def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0): # Get limbs and offsets - #l3=0.15 - l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist) # Limb lengths + l_bs, l1, l2, l3, l_wt = (robot.limb_base, robot.limb1, robot.limb2, robot.limb3, robot.limb_wrist) # Limb lengths l3 += l3offset # add wrist offset, used for gripper angle calculations - + offset_x = robot.offset_x + offset_y = robot.offset_y + offset_z = robot.offset_z # Calculate base angle and r relative to shoulder joint def calculate_theta(x, y, a): # Calculate if we need the + or - in our equations @@ -267,7 +281,8 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = # Return result return base, shoulder, elbow, wrist1, ry, rz -def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): +def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): + rob = robot.robot joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset) # Return current positions if coordinates don't make sense @@ -287,11 +302,11 @@ def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset # Return adjusted joint positions return [o+j*i for j, o, i in zip(joints, offsets, inverse)] -def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): +def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): - global rob + rob = robot.robot start_joints = rob.getj() - end_joint = get_joints_from_xyz_abs(x, y, z, rx, ry, rz) + end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz) n_points = 50 intermediate_joints = [] @@ -302,11 +317,11 @@ def move_arc(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(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False): +def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False): # gripper angle: from vertical # gripper length: from joint to start of grip # to flip, you can use flip=True or make gripper angle negative - + limb3 = robot.limb3 # Determine tool rotation depending on gripper angle if gripperangle < 0: rz = - math.pi / 2 @@ -330,20 +345,20 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, fli gripperx = math.sin(gripperangle) * gripperlength gripperx += (1-math.cos(gripperangle)) * limb3 - return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) -def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False): +def goto_holder_index(robot, idx, z=0.05, gripperangle=35, flip=False): joint = config["position_map"][idx] print("Going to cable holder index", joint["index"], "at position", joint["pos"]) - safe_move(joint["pos"][1]/1000, joint["pos"][0]/1000, z) + safe_move(robot, joint["pos"][1]/1000, joint["pos"][0]/1000, z) #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip) #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, ) -def is_flipped(): - global rob +def is_flipped(robot): + rob = robot.robot wrist2 = rob.getj()[4] if wrist2>0: @@ -351,8 +366,8 @@ def is_flipped(): else: return False -def flip(): - global rob +def flip(robot): + rob = robot.robot # A list of safe positions to flip safe_positions = [(-0.205, -0.108, 0.3), @@ -372,16 +387,16 @@ def flip(): rob.movej(offset_gripper_angle(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper -def safe_move(x, y, z): +def safe_move(robot, x, y, z): + rob = robot.robot flip_radius = 0.17 # Min radius on which to flip r = math.sqrt(x**2 + y**2) # Get position radius # Flip gripper if needed - if (r <= flip_radius and is_flipped()) or (r > flip_radius and not is_flipped()): - flip() + if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)): + flip(robot) - global rob - rob.movej(offset_gripper_angle(x, y, z, flip=is_flipped()), vel=2, acc=2) + rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2) @@ -389,9 +404,14 @@ if __name__ == "__main__": #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2) #rob.movel((x, y, z, rx, ry, rz), a, v) - init("192.168.1.145") + #init("192.168.1.145") + with open('config.yml', 'r') as fileread: + #global config + config = yaml.safe_load(fileread) + robot = Rob(config) + rob = robot.robot print("Current tool pose is: ", rob.getl()) - move_to_home() + move_to_home(robot) home_pose = [-0.4999999077032916, -0.2000072960336574, @@ -423,12 +443,10 @@ if __name__ == "__main__": config = None joints = [] for i in np.linspace(-0.2, -0.7, 10): - joints.append(get_joints_from_xyz_abs(i, 0, 0)) + joints.append(get_joints_from_xyz_abs(robot, i, 0, 0)) # rob.movejs(joints, acc=2, vel=2, radius=0.1) - with open('config.yml', 'r') as fileread: - #global config - config = yaml.safe_load(fileread) + # move_arc(0, 0.3, 0.1) # move_arc(0, -0.3, 0.3) @@ -437,8 +455,8 @@ if __name__ == "__main__": # for i in range(20): # goto_holder_index(i, 0.1) - flip() - flip() + flip(robot) + flip(robot) #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2) #joints = [] @@ -447,11 +465,11 @@ if __name__ == "__main__": # joints.append(goto_holder_index(24, 0.5, i)) #rob.movejs(joints, acc=1, vel=3) angle = 30 - rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2) + rob.movej(goto_holder_index(robot, 26, 0.1, angle), acc=2, vel=2) time.sleep(1) - rob.movej(goto_holder_index(25, 0.1, angle), acc=2, vel=2) + rob.movej(goto_holder_index(robot, 25, 0.1, angle), acc=2, vel=2) time.sleep(1) - rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2) + rob.movej(goto_holder_index(robot, 24, 0.1, angle, flip=True), acc=2, vel=2) #rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2) #rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2) #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)