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

Pick-up routine

Browse Source
This commit is contained in:
BlueOceanWave 2024-03-26 15:24:18 -05:00
parent 77fdc43fce
commit 9893222335
1 changed files with 95 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

171
ur5_control.py
View File

@ -20,6 +20,7 @@ class Rob():
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
#
def __init__(self, config):
self.config = config
armc = config["arm"]
self.ip = armc["ip"]
tool = armc["tool"]
@ -54,7 +55,6 @@ class Rob():
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))
@ -281,7 +281,7 @@ def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_g
# Return result
return base, shoulder, elbow, wrist1, ry, rz
def get_joints_from_xyz_abs(robot, 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, use_closest_path=True):
rob = robot.robot
joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset)
@ -299,8 +299,37 @@ def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l
#else:
#print("Shoulder at", joints[1] * 180/math.pi)
# Return adjusted joint positions
return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
# Get adjusted joint positions
adjusted_joints = [o+j*i for j, o, i in zip(joints, offsets, inverse)]
curr_joints = rob.getj()
def get_complimentary_angle(joint_angle):
if joint_angle<0:
new_angle = joint_angle + 2*math.pi
else:
new_angle = joint_angle - 2*math.pi
if abs(new_angle) > math.radians(350):
return joint_angle
else:
return new_angle
# Use closest path (potentially going beyond 180 degrees)
if use_closest_path:
if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]):
adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0])
# final_joint_positions = []
# for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints):
# if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)):
# final_joint_positions.append(adjusted_joint)
# else:
# final_joint_positions.append(get_complimentary_angle(adjusted_joint))
# return final_joint_positions
return adjusted_joints
def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
rob = robot.robot
@ -317,7 +346,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=35, gripperlength=0.20+0.018, flip=False):
def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True):
# gripper angle: from vertical
# gripper length: from joint to start of grip
# to flip, you can use flip=True or make gripper angle negative
@ -336,7 +365,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, 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)
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)
else:
gripperangle = math.radians(gripperangle)
@ -345,11 +374,13 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.0
gripperx = math.sin(gripperangle) * gripperlength
gripperx += (1-math.cos(gripperangle)) * limb3
return get_joints_from_xyz_abs(robot, 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, use_closest_path=use_closest_path)
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"])
def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False):
joint = robot.config["position_map"][idx]
if verbose:
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
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)
@ -370,8 +401,8 @@ def flip(robot):
rob = robot.robot
# A list of safe positions to flip
safe_positions = [(-0.205, -0.108, 0.3),
(0.205, -0.108, 0.3)]
safe_positions = [(-0.18, -0.108, 0.35),
(0.18, -0.108, 0.35)]
# Find the closest safe position
curr_pos = rob.getl()[:3]
@ -384,12 +415,12 @@ 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()), vel=2, acc=2) # Move to safe position
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
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
def safe_move(robot, x, y, z):
rob = robot.robot
flip_radius = 0.17 # Min radius on which to flip
flip_radius = 0.22 # Min radius on which to flip
r = math.sqrt(x**2 + y**2) # Get position radius
# Flip gripper if needed
@ -398,6 +429,33 @@ def safe_move(robot, x, y, z):
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2)
def pick_up_routine(robot, holder_index, verbose=False):
rob = robot.robot
if verbose:
print('Pickup routine for index', holder_index)
goto_holder_index(robot, holder_index, 0.2)
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0
rob.movel(new_pos, vel=0.3, acc=1)
# goto_holder_index(robot, holder_index, 0.0)
# Close Gripper code
time.sleep(0.5)
new_pos[2] = 0.2
rob.movel(new_pos, vel=0.3, 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)
# Back to safe position
rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
if __name__ == "__main__":
@ -409,6 +467,7 @@ if __name__ == "__main__":
#global config
config = yaml.safe_load(fileread)
robot = Rob(config) # robot of type Rob is the custom class above
robot.init_arm()
rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
print("Current tool pose is: ", rob.getl())
@ -447,58 +506,37 @@ if __name__ == "__main__":
joints.append(get_joints_from_xyz_abs(robot, i, 0, 0))
# rob.movejs(joints, acc=2, vel=2, radius=0.1)
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=90, flip=is_flipped(robot)), vel=2, acc=2)
# move_arc(0, 0.3, 0.1)
# move_arc(0, -0.3, 0.3)
for i in range(20, 25):
pick_up_routine(robot, i, verbose=True)
# for i in range(20):
# goto_holder_index(i, 0.1)
# goto_holder_index(robot, 7, 0.0)
flip(robot)
flip(robot)
# pick_up_routine(robot, 8)
# rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2).
#joints = []
#for i in np.linspace(0, 340, 340):
# joints.append(goto_holder_index(24, 0.5, i))
#rob.movejs(joints, acc=1, vel=3)
angle = 30
rob.movej(goto_holder_index(robot, 26, 0.1, angle), acc=2, vel=2)
time.sleep(1)
rob.movej(goto_holder_index(robot, 25, 0.1, angle), acc=2, vel=2)
time.sleep(1)
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)
# angle = 30
# goto_holder_index(robot, 26, 0.1, angle)
# time.sleep(1)
# goto_holder_index(robot, 25, 0.1, angle)
# time.sleep(1)
# goto_holder_index(robot, 24, 0.1, angle)
#rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
#rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
#time.sleep(1)
#rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
#rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
#rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)
# rob.movej(goto_holder_index(53, 0.1, angle, flip=True), acc=2, vel=2)
#time.sleep(2)
#rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
#time.sleep(10)
# time.sleep(4)
# goto_holder_index(26, 0.1, 20)
# time.sleep(4)
# goto_holder_index(26, 0.1, 30)
# time.sleep(4)
# goto_holder_index(26, 0.1, 40)
# for joint in config["position_map"]:
#joint = config["position_map"][26]
#print("Going to cable holder index", joint["index"], "at position", joint["pos"])
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5)
#joints.append(angles)
#rob.movej(angles, acc=2, vel=2)
#time.sleep(10)
@ -511,25 +549,6 @@ if __name__ == "__main__":
# time.sleep(5)
# angles = get_joints_from_xyz_abs(0, -0.6, 0)
# rob.movej(angles, acc=2, vel=2)
# set_pos_abs(*p1)
# move = move_to_polar(p1, p2)
# for p in move:
# print(math.degrees(p))
# print("Safe? :", is_safe_move(p1, p2))
# #set_pos_rel_rot_abs(0, 0, -0.2, math.pi, 0, -math.pi)
# set_pos_abs(0.3, -0.2, 0.5, math.pi, 0, -math.pi)
# set_pos_abs(0, 0.2, 0.6, math.pi, 0, -math.pi)
# set_pos_abs(-0.5, -0.2, 0.4, math.pi, 0, -math.pi)
# #set_pos_rel_rot_abs(0, 0, 0, math.pi, 0, -math.pi)
# print("Current tool pose is: ", rob.getl())
# print("getj(): ", rob.getj())