Fixed drop off routine to not hit walls
This commit is contained in:
BlueOceanWave
parent
a698c4a753
commit
237319db14
167
ur5_control.py
167
ur5_control.py
@ -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)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user