Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Convert ur5_control to class based (untested)

Browse Source
This commit is contained in:
Cole Deck 2024-03-24 15:31:58 -05:00
parent 6887fa943b
commit 3de59f5985
3 changed files with 107 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
config.yml
View File

@ -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:

7
run.py
View File

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

162
ur5_control.py
View File

@ -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...")
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()
# power up robot here
def init_arm(self):
#sys.stdout = Logger()
fprint("Starting UR5 power up...")
# wait for power up (this function runs async)
# 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)