Made pickup and dropoff routines for tray and holder. Adjusted home position
This commit is contained in:
BlueOceanWave
parent
237319db14
commit
ae97ed1a14
2
tempCodeRunnerFile.py
Normal file
2
tempCodeRunnerFile.py
Normal file
@ -0,0 +1,2 @@
|
||||
|
||||
drop_off_tray(robot, 0)
|
||||
195
ur5_control.py
195
ur5_control.py
@ -187,7 +187,6 @@ def polar_to_cartesian(r, theta):
|
||||
y = r * np.sin(theta)
|
||||
return x, y
|
||||
|
||||
|
||||
def move_to_polar(robot, start_pos, end_pos):
|
||||
rob = robot.robot
|
||||
|
||||
@ -246,22 +245,16 @@ def move_to_polar(robot, start_pos, end_pos):
|
||||
|
||||
return rx_intermediate
|
||||
|
||||
def move_to_home(robot, speed=2):
|
||||
|
||||
def move_to_home(robot, keep_flip=False, speed=2):
|
||||
rob = robot.robot
|
||||
|
||||
if is_flipped(robot):
|
||||
if is_flipped(robot) and not keep_flip:
|
||||
flip(robot)
|
||||
|
||||
# Home position in degrees
|
||||
home_pos = [0.10421807948612624,
|
||||
-2.206111555015423,
|
||||
1.710679229503537,
|
||||
-1.075834511928354,
|
||||
-1.569301366430687,
|
||||
1.675098295930943]
|
||||
# Move robot to home position
|
||||
rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.35), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
|
||||
|
||||
# Move robot
|
||||
rob.movej(home_pos, acc=2, vel=speed)
|
||||
return True
|
||||
|
||||
def move_to_packup(robot, speed=0.25):
|
||||
@ -453,9 +446,9 @@ def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_close
|
||||
|
||||
def is_flipped(robot):
|
||||
rob = robot.robot
|
||||
wrist2 = rob.getj()[4]
|
||||
|
||||
if wrist2<-math.pi:
|
||||
wrist1 = rob.getj()[3]
|
||||
|
||||
if wrist1 > 0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
@ -496,31 +489,46 @@ 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, verbose=False):
|
||||
def holder_routine(robot, holder_index, pick_up, verbose=False):
|
||||
robot = connect(robot)
|
||||
rob = robot.robot
|
||||
|
||||
if verbose:
|
||||
fprint('Pickup routine for index' + str(holder_index))
|
||||
|
||||
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
|
||||
open_gripper()
|
||||
# Go to the correct holder
|
||||
if pick_up:
|
||||
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
|
||||
else:
|
||||
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
|
||||
|
||||
if pick_up:
|
||||
open_gripper()
|
||||
|
||||
# Move down
|
||||
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()
|
||||
|
||||
# Pick up or drop off
|
||||
if pick_up:
|
||||
close_gripper()
|
||||
else:
|
||||
open_gripper()
|
||||
|
||||
# Move up
|
||||
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)
|
||||
# goto_holder_index(robot, 25, z=0.2)
|
||||
def pick_up_holder(robot, holder_index, verbose=False):
|
||||
holder_routine(robot, holder_index, True, verbose=verbose)
|
||||
def drop_off_holder(robot, holder_index, verbose=False):
|
||||
holder_routine(robot, holder_index, False, verbose=verbose)
|
||||
|
||||
|
||||
def place_tube(robot, slot=0):
|
||||
def tray_routine(robot, slot=0, pick_up=True):
|
||||
robot = connect(robot)
|
||||
rob = robot.robot
|
||||
|
||||
@ -529,24 +537,26 @@ def place_tube(robot, slot=0):
|
||||
(-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
|
||||
(-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)]
|
||||
|
||||
# Initial position
|
||||
print('goto holder')
|
||||
goto_holder_index(robot, 25, z=0.3)
|
||||
# Initial position depending on slot and robot orientation
|
||||
if slot in [0, 1]:
|
||||
if is_flipped(robot):
|
||||
flip(robot)
|
||||
else:
|
||||
move_to_home(robot, keep_flip=True)
|
||||
else:
|
||||
goto_holder_index(robot, 25, z=0.3)
|
||||
|
||||
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),
|
||||
angles = [(-58, -114.45, 100.52, -45.24, -96.95, 120),
|
||||
(-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)
|
||||
rob.movejs(angles,vel=2,acc=1)
|
||||
|
||||
# Positions for each slot
|
||||
slot_distance = .052
|
||||
@ -558,9 +568,14 @@ def place_tube(robot, slot=0):
|
||||
[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)
|
||||
rob.movel(slot_position[slot],vel=0.2, acc=1)
|
||||
|
||||
# Place/Grab the tube
|
||||
if pick_up:
|
||||
close_gripper()
|
||||
else:
|
||||
open_gripper()
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
# Move back
|
||||
tilt = 0.3
|
||||
@ -573,13 +588,16 @@ def place_tube(robot, slot=0):
|
||||
new_pos[3] += tilt
|
||||
new_pos[4] += tilt
|
||||
new_pos[5] += tilt
|
||||
rob.movel(new_pos, vel=0.1, acc=1)
|
||||
rob.movel(new_pos, vel=0.2, acc=1)
|
||||
|
||||
|
||||
# Go home to safe position
|
||||
move_to_home(robot, speed=1)
|
||||
|
||||
# return True
|
||||
move_to_home(robot, speed=1, keep_flip=True)
|
||||
def pick_up_tray(robot, slot=0):
|
||||
tray_routine(robot, slot, True)
|
||||
def drop_off_tray(robot, slot=0):
|
||||
tray_routine(robot, slot, False)
|
||||
|
||||
|
||||
def return_routine(robot, slot, holder_index=None, verbose=False):
|
||||
robot = connect(robot)
|
||||
@ -688,16 +706,11 @@ def close_gripper():
|
||||
|
||||
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")
|
||||
with open('config.yml', 'r') as fileread:
|
||||
#global config
|
||||
config = yaml.safe_load(fileread)
|
||||
|
||||
#open_gripper()
|
||||
#time.sleep(1)
|
||||
#close_gripper()
|
||||
|
||||
|
||||
robot = Rob(config) # robot of type Rob is the custom class above
|
||||
@ -707,96 +720,26 @@ 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)
|
||||
|
||||
#time.sleep(3)
|
||||
#move_to_packup(robot)
|
||||
|
||||
|
||||
pick_up_holder(robot, 0)
|
||||
drop_off_tray(robot, 0)
|
||||
pick_up_holder(robot, 1)
|
||||
drop_off_tray(robot, 3)
|
||||
|
||||
pick_up_tray(robot, 0)
|
||||
drop_off_holder(robot, 0)
|
||||
pick_up_tray(robot, 3)
|
||||
drop_off_holder(robot, 1)
|
||||
|
||||
|
||||
|
||||
# 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)
|
||||
|
||||
print("Current tool pose is: ", rob.getl())
|
||||
|
||||
|
||||
curr_pos = rob.getl()
|
||||
|
||||
|
||||
config = None
|
||||
# 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)
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
|
||||
# goto_holder_index(robot, 7, 0.0)
|
||||
|
||||
# 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).
|
||||
#joints = []
|
||||
|
||||
|
||||
# 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(angles, acc=2, vel=2)
|
||||
#time.sleep(10)
|
||||
#rob.movejs(joints, acc=2, vel=2)
|
||||
# joints = []
|
||||
# for i in np.linspace(-0.3, -0.7, 50):
|
||||
# joints.append(get_joints_from_xyz_abs(i, 0, 0))
|
||||
# rob.movejs(joints, acc=2, vel=2)
|
||||
|
||||
# time.sleep(5)
|
||||
|
||||
|
||||
|
||||
# print("Current tool pose is: ", rob.getl())
|
||||
# print("getj(): ", rob.getj())
|
||||
|
||||
# move_to_home()
|
||||
|
||||
|
||||
rob.stop()
|
||||
os.kill(os.getpid(), 9) # dirty kill of self
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user