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

Fixed drop off routine to not hit walls

Browse Source
This commit is contained in:
BlueOceanWave
2024-04-22 21:56:07 -05:00
parent a698c4a753
commit 237319db14
1 changed files with 95 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
ur5_control.py
View File

@@ -83,6 +83,7 @@ def connect(robot):
# Set weight
robot.robot.set_payload(2, (0, 0, 0.1))
return robot
def init_arm(robot):
robot = connect(robot)
@@ -95,7 +96,7 @@ def init_arm(robot):
# setup - in case of fail. open gripper, move up, then go home.
rob = robot.robot
open_gripper()
# open_gripper()
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] += 0.025
@@ -248,6 +249,9 @@ def move_to_polar(robot, start_pos, end_pos):
def move_to_home(robot, speed=2):
rob = robot.robot
if is_flipped(robot):
flip(robot)
# Home position in degrees
home_pos = [0.10421807948612624,
-2.206111555015423,
@@ -423,7 +427,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 + rzoffset, 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=-3*math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path)
else:
gripperangle = math.radians(gripperangle)
@@ -451,7 +455,7 @@ def is_flipped(robot):
rob = robot.robot
wrist2 = rob.getj()[4]
if wrist2>0:
if wrist2<-math.pi:
return True
else:
return False
@@ -462,6 +466,7 @@ def flip(robot):
# A list of safe positions to flip
safe_positions = [(-0.18, -0.108, 0.35),
(0.18, -0.108, 0.35)]
# Find the closest safe position
curr_pos = rob.getl()[:3]
@@ -474,8 +479,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=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
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
# print('flip?: ', is_flipped(robot))
return True
def safe_move(robot, x, y, z, use_closest_path=True):
@@ -490,7 +496,7 @@ def safe_move(robot, x, y, z, use_closest_path=True):
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, slot=None, verbose=False):
def pick_up_routine(robot, holder_index, verbose=False):
robot = connect(robot)
rob = robot.robot
@@ -507,69 +513,73 @@ def pick_up_routine(robot, holder_index, slot=None, verbose=False):
# 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)
goto_holder_index(robot, 25, z=0.2)
#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)
def place_tube(robot, slot=0):
robot = connect(robot)
rob = robot.robot
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)
slot_prepositions = [(-9.93, -112.67, 144.02, -116.69, -54.13, -10.29),
(-12.35, -124.95, 148.61, -107.27, -54.36, -13.26),
(-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
(-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)]
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]
# Initial position
print('goto holder')
goto_holder_index(robot, 25, z=0.3)
rob.movej(angles,vel=1,acc=0.5)
print('aligning slot')
# Align robot to the slot
if slot in [2,3]:
angles = [(-2.77, -99.64, 131.02, 67.67, 70.04-360, 153.03),
slot_prepositions[slot]]
# (-16.44, -94.17, 139.46, 54, 54.92, 161.75),]
else:
flip(robot)
angles = [(-34.1, -117.51, 129.81, -70.05, -52.30, 315.25),
(-39.98, -124.92, 132.28, -61.56, -55.60, -50.77),
slot_prepositions[slot]]
angles = [[x*math.pi/180 for x in move] for move in angles]
rob.movejs(angles,vel=1,acc=0.5)
open_gripper()
# Positions for each slot
slot_distance = .052
slot_height = -.015-.0095
first_slot = -0.3084+0.02
slot_position = [
[first_slot, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+2*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
[first_slot+3*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
]
rob.movel(slot_position[slot],vel=0.2, acc=0.2)
time.sleep(2)
# Move back
tilt = 0.3
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] += 0.04
if slot==3:
new_pos[0] -= 0.05 #x
new_pos[1] += 0.15 #y
new_pos[2] = 0.09 #z
new_pos[3] += tilt
new_pos[4] += tilt
new_pos[5] += tilt
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
#time.sleep(10)
#rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=4, acc=3)
return True
# Go home to safe position
move_to_home(robot, speed=1)
# return True
def return_routine(robot, slot, holder_index=None, verbose=False):
robot = connect(robot)
@@ -652,13 +662,13 @@ def return_camera(robot, holder_index, verbose=False):
def open_gripper():
fprint("Opening gripper")
c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
c.write_single_register(0, 0b0)
c.write_single_register(435, 0b1)
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
c.write_single_register(0, 0b0)
c.write_single_register(435, 0b1)
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
time.sleep(0.5)
c.close()
@@ -666,12 +676,12 @@ def open_gripper():
def close_gripper():
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 = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
c.write_single_register(435, 0b0)
c.write_single_register(0, 0b1)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
time.sleep(0.5)
c.close()
#
@@ -697,20 +707,36 @@ if __name__ == "__main__":
rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
move_to_home(robot, speed=4)
# move_to_home(robot, speed=4)
#time.sleep(3)
#move_to_packup(robot)
pick_up_routine(robot, 0, 0, True)
# pick_up_routine(robot, 0)
# place_tube(robot, 0)
# time.sleep(0.5)
# place_tube(robot, 1)
# time.sleep(0.5)
# place_tube(robot, 2)
# time.sleep(0.5)
place_tube(robot, 2)
time.sleep(0.5)
# pick_up_routine(robot, 3, True)
# place_tube(robot, 3)
# angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15)
# angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72)
# angles = (10.39,-70.86,133.95,7.83,30.91,-159.00)
# angles = (13.31,-76.23,140.78,1.90,31.98,-153.76)
#move_to_home(robot)
#return_routine(robot, 0, 0, True)
time.sleep(100)
print("Current tool pose is: ", rob.getl())
@@ -718,9 +744,6 @@ if __name__ == "__main__":
config = None
joints = []
for i in np.linspace(-0.2, -0.7, 10):
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)