def test_pick():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
rospy.wait_for_service('update_calibration')
s = rospy.ServiceProxy('update_calibration', CalibrationUpdate)
response = s(upper_left=Point(1250, 35, 1037),
upper_right=Point(1250, -835, 1037))
r = RandomLocationStow("pos_info_dummy")
item = "jane_eyre_dvd"
print "try to find a location for %s" % item
target = r.find_location(item)
print "result:", target
if raw_input("press enter to continue or q to quit: ") == "q":
return
#arm_control_wrapper.move_right_arm_global(target)
if raw_input("press enter to continue or q to quit: ") == "q":
return
# get into position
bin = "bin_G"
pos = "photo"
print "move to %s/%s" % (bin, pos)
#result = arm_control_wrapper.move_left_arm_global([698,-356,-260,179,0,179])
#print "result:", result
result = arm_control_wrapper.move_left_arm_to_bin(bin,
pos,
keitai='nut000',
kakujiku='chokusen')
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check grabbability
v = VacuumGrabbing()
print "check if we can grab %s from %s" % (item, [
"elmers_washable_no_run_school_glue", "dasani_water_bottle",
"jane_eyre_dvd"
])
result = v.can_grab_item(item, bin, [
"elmers_washable_no_run_school_glue", "dasani_water_bottle",
"jane_eyre_dvd"
])
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# take the item
print "take item"
result = v.grab_from_shelf(item, bin)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# put it into the box
print "stow it"
result = v.stow_in_tote(target)
print "result:", result
def test_stow():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
#rospy.wait_for_service('update_calibration')
#s = rospy.ServiceProxy('update_calibration',CalibrationUpdate)
#response = s(upper_left=Point(1250,35,1037), upper_right=Point(1250,-835,1037))
items = ["jane_eyre_dvd"]
pos_info = ItemLocations({"bin_G": ["elmers_washable_no_run_school_glue"]},
items)
# find stow location
r = ShelfRandomLocationStow(pos_info)
item = "jane_eyre_dvd"
print "try to find a location for %s" % item
(target_bin, target_pos) = r.find_location(item)
print "result:", (target_bin, target_pos)
if raw_input("press enter to continue or q to quit: ") == "q":
return
# move to tote
#result = arm_control_wrapper.move_left_arm_to_bin('bin_D', 'pre', keitai='nut000', kakujiku='kakujiku')
result = arm_control_wrapper.move_left_arm_to_bin("tote",
"photo",
keitai='fut000',
kakujiku='kakujiku')
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check grabbability
v = VacuumGrabbing()
print "check if we can grab %s from %s" % (item, "tote")
result = v.can_grab_item(item, "tote", items)
print "result:", result
if not result:
print "did not detect item"
return
if raw_input("press enter to continue or q to quit: ") == "q":
return
# take the item
print "take item"
result = v.grab_from_tote(item)
print "result:", result
if not result:
print "did not take item"
return
if raw_input("press enter to continue or q to quit: ") == "q":
return
# put it into the shelf
print "stow it"
result = v.stow_in_shelf(target_bin, target_pos)
print "result:", result
def test_pick_bin2bin():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
arm_control_wrapper.move_both_hand_start()
#rospy.wait_for_service('update_calibration')
#s = rospy.ServiceProxy('update_calibration',CalibrationUpdate)
#response = s(upper_left=Point(1250,35,1037), upper_right=Point(1250,-835,1037))
items = [
"laugh_out_loud_joke_book", "hanes_tube_socks", "crayola_24_ct",
"oral_b_toothbrush_green"
]
#items = ["crayola_24_ct", "jane_eyre_dvd","rawlings_baseball","scotch_duct_tape","scotch_duct_tape","command_hooks","folgers_classic_roast_coffee","kyjen_squeakin_eggs_plush_puppies","cool_shot_glue_sticks"]
#items = ["soft_white_lightbulb", "expo_dry_erase_board_eraser", "womens_knit_gloves", "dove_beauty_bar", "elmers_washable_no_run_school_glue"]#["rawlings_baseball", "staples_index_cards", "kleenex_tissue_box"] #["dasani_water_bottle","expo_dry_erase_board_eraser", "creativity_chenille_stems"] # "elmers_washable_no_run_school_glue", "dove_beauty_bar"
pos_info = ItemLocations({"bin_A": items}, [])
get_item_strat = get_item.MoveUntilGrabbablePicking(pos_info)
r = RandomLocationStow("pos_info_dummy")
item = items[0]
print "try to find a location for %s" % item
target = r.find_location(item)
print "result:", target
if raw_input("press enter to continue or q to quit: ") == "q":
return
# get into position
bin = "bin_D"
pos = "pre"
print "move to %s/%s" % (bin, pos)
result = arm_control_wrapper.move_left_arm_to_bin(bin,
pos,
keitai='nut000',
kakujiku='kakujiku')
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# now do everything to grab that item
(success, strategy,
moved_items) = get_item_strat.get_item(item, "bin_D", items)
print success, strategy, moved_items
if not success:
print "failed to take the item"
return
# put it into the box
if raw_input("press enter to continue or q to quit: ") == "q":
return
result = strategy.stow_in_tote(target)
print "result:", result
print pos_info.to_json()
arm_control_wrapper.move_both_hand_end()
def test_arm_control_highlevel():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
# test all bins
for bin_key in "IJKL":
for pos in ["pre", "photo", "left", "right", "pre", "front", "pre"]:
if bin_key == 'C' and pos == 'right':
continue
print "move to bin_%s/%s" % (bin_key, pos)
result = arm_control_wrapper.move_left_arm_to_bin(
"bin_" + bin_key, pos)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# test tote
for pos in ["photo", "photo_down", '']:
print "move to tote/%s" % pos
result = arm_control_wrapper.move_left_arm_to_bin("tote", pos)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
def test_can_grab_item():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
rospy.wait_for_service('update_calibration')
s = rospy.ServiceProxy('update_calibration', CalibrationUpdate)
response = s(upper_left=Point(1300, 35, 1037),
upper_right=Point(1250, -835, 1037))
v = VacuumGrabbing()
# get into position
bin = "bin_G"
pos = "photo"
print "move to %s/%s" % (bin, pos)
result = arm_control_wrapper.move_left_arm_to_bin(bin,
pos,
keitai='nut000',
kakujiku='chokusen')
print "result:", result
# check if we can grab various items
correct_inventory = [
"dasani_water_bottle", "elmers_washable_no_run_school_glue",
"peva_shower_curtain_liner", "i_am_a_bunny_book",
"dr_browns_bottle_brush", "staples_index_cards"
] #"ticonderoga_12_pencils", "rawlings_baseball", "easter_turtle_sippy_cup", "fiskars_scissors_red"]
extended_inventory = [
"elmers_washable_no_run_school_glue", "dasani_water_bottle"
]
wrong_inventory = ["dasani_water_bottle"]
for inventory in [correct_inventory, extended_inventory, wrong_inventory]:
for item in [
"elmers_washable_no_run_school_glue", "dasani_water_bottle"
]:
print "check if we can grab %s from %s" % (item, inventory)
result = v.can_grab_item(item, bin, inventory)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check how completely bogus item names work
print "check if we can grab %s from %s" % ("hogehoge", correct_inventory)
result = v.can_grab_item("hogehoge", bin, correct_inventory)
print "result:", result
def test_arm_control_highlevel_bad_input():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
# test bad bin
# not implemented yet
#bin = "hoge"
#pos = "photo"
#print "move to %s/%s" % (bin, pos)
#result = arm_control_wrapper.move_left_arm_to_bin(bin, pos)
#print "result:", result
#raw_input("press enter to continue or ctrl-c to quit ")
# test bad position
bin = "bin_A"
pos = "foobar"
print "move to %s/%s" % (bin, pos)
result = arm_control_wrapper.move_left_arm_to_bin(bin, pos)
print "result:", result
def move(self, grabber):
grabber.pipe_0_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_global(self.pre_target_pos,
keitai='nut000',
kakujiku='chokusen',
speed="fast")
print 'approach_target_pos'
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen',
speed="fast")
grabber.vacuum_on()
print 'target_pos'
arm_control_wrapper.move_left_arm_global(self.target_pos,
keitai='nut000',
kakujiku='chokusen')
time.sleep(1.0)
# konoyouna shori wo ireru?
#if grabber.vac_high(self.item).success:
# grabber.vacuum_off()
# time.sleep(0.5)
# grabber.vacuum_on()
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_global(self.approach_target_pos_ret,
keitai='nut000',
kakujiku='chokusen')
force_retry = True if self.item in force_retry_items else False
time.sleep(0.5)
if self.item == "scotch_duct_tape" and not grabber.vac_ok(
self.item).success:
print 'high pressure -> retry # duct tape specific'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([-30, 0, -10, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
elif self.item != "scotch_duct_tape":
if (force_retry or not grabber.vac_ok(
self.item).success) and self.target_pos[0] > 50:
grabber.vacuum_on()
print 'high pressure -> retry #1'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, 0, -10, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(
self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
grabber.vacuum_on()
print 'high pressure -> retry #2'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([-20, 0, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(
self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([20, 0, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(
self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
print 'high pressure -> retry #4'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, -20, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(
self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #5'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, 20, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(
self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
grabber.vacuum_off()
return False
print 'pre_target_pos_ret'
arm_control_wrapper.move_left_arm_global(self.pre_target_pos_ret,
keitai='nut000',
kakujiku='chokusen')
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
grabber.vacuum_off()
return False
#arm_control_wrapper.move_left_arm_to_bin('tote','', keitai='nut000', kakujiku='chokusen')
#arm_control_wrapper.move_left_arm_to_bin(self.bin,'pull', keitai='fut000', kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
return True
def move(self, grabber):
grabber.pipe_0_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_global(self.pre_target_pos,
keitai='nut000',
kakujiku='kakujiku',
speed="fast")
print 'approach_target_pos'
grabber.vacuum_on()
arm_control_wrapper.move_left_arm_global(self.approach_target_pos0,
keitai='nut000',
kakujiku='chokusen',
speed="fast")
time.sleep(0.5)
if grabber.vac_ok("").success:
rospy.loginfo("pick tried but seemes not success (butsukatta?)")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
return False
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen',
speed="fast")
time.sleep(0.5)
if grabber.vac_ok("").success:
rospy.loginfo("pick tried but seemes not success (butsukatta?)")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
return False
grabber.pipe_90_deg()
print 'target_pos'
arm_control_wrapper.move_left_arm_global(self.target_pos,
keitai='nut000',
kakujiku='chokusen')
time.sleep(1.0)
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_global(self.approach_target_pos_ret,
keitai='nut000',
kakujiku='chokusen')
force_retry = True if self.item in force_retry_items else False
time.sleep(0.5)
if (force_retry or not grabber.vac_ok(
self.item).success) and self.target_pos[0] > 50:
grabber.vacuum_on()
print 'high pressure -> retry #1'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, 0, -10, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #2'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([-20, 0, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([20, 0, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
print 'high pressure -> retry #4'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, -20, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if (force_retry or not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #5'
arm_control_wrapper.move_left_arm_global(
self.target_pos + np.asarray([0, 20, 0, 0, 0, 0]),
keitai='nut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_global(self.approach_target_pos,
keitai='nut000',
kakujiku='chokusen')
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
return False
print 'pre_target_pos_ret'
grabber.pipe_0_deg_slow()
arm_control_wrapper.move_left_arm_global(self.pre_target_pos_ret,
keitai='nut000',
kakujiku='chokusen')
#arm_control_wrapper.move_left_arm_to_bin('tote','', keitai='nut000', kakujiku='chokusen')
#arm_control_wrapper.move_left_arm_to_bin(self.bin,'pull', keitai='fut000', kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
#grabber.pipe_90_deg_very_slow()
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
grabber.vacuum_off()
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_to_bin('tote',
'',
keitai='nut000',
kakujiku='chokusen')
return False
return True
def execute(self):
# get a stratey to determine free space
stow_strategy = shelf_stow_locator.select_tote2bin_strategy(
self.pos_info)
loginfo("ShelfStowLocatorStrategy: %s" % stow_strategy)
calib_ok = super(VisibleGreedyStowing, self).calibrate()
if not calib_ok:
logerr("calibration failed")
return False
secondary_item_priorities = []
checkpoint = time.time()
try_count = 0
success_count = 0
items_in_tote = self.pos_info.tote[:]
item_list = []
for item in items_in_tote:
if item == "fitness_gear_3lb_dumbbell":
# treat the dumbbell as if it was not in the box
continue
elif item == "scotch_duct_tape":
# take the tape later
item_list.append((3, item))
#elif item in ["fiskars_scissors_red","oral_b_toothbrush_red","oral_b_toothbrush_green"]:
# item_list.append((2, item))
else:
item_list.append((1, item))
# make item_list a list with the count of tries
item_list = zip([1] * len(items_in_tote), items_in_tote[:])
while item_list and try_count < 100:
if try_count != 0:
loginfo("completed %d-th item in %s (total: %s)" %
(try_count, util.get_elapsed_time(checkpoint),
util.get_elapsed_time(self.start)))
checkpoint = time.time()
try_count += 1
loginfo("remaining items (unordered): %s" % item_list)
if CHOICE_MODE == ItemChoiceMode.RANDOM:
(num_tries, next_item) = random.choice(item_list)
elif CHOICE_MODE == ItemChoiceMode.SMALLEST:
(num_tries, next_item) = \
util.get_try_smallest_item_with_margin(item_list)
elif CHOICE_MODE == ItemChoiceMode.LARGEST:
raise NotImplementedError(
"largest item order is not implemented")
elif CHOICE_MODE == ItemChoiceMode.VISIBLE:
# if we have a list of priorities from the previous run,
# take the one with the lowest index in that list that
# we haven't tried yet before
if secondary_item_priorities:
(num_tries, next_item) = \
min(item_list,
key=lambda x: (x[0], secondary_item_priorities.index(util.item_string_to_id(x[1]))))
else:
# otherwise, take the smallest one
(num_tries, next_item) = \
util.get_try_smallest_item_with_margin(item_list)
loginfo("======================================")
loginfo("attempt to take item %s from tote" % next_item)
# first, find a suitable bin
loginfo("find a location to move that item to")
(target_bin, target_pos) = \
stow_strategy.find_location(next_item)
loginfo("if we succeed to take %s, we will put it to %s/%s" %
(next_item, target_bin, target_pos))
# move the arm back to tote
try:
(mv_success, _, _,
_) = arm_control_wrapper.move_left_arm_to_bin(
"tote", "", keitai='nut000', kakujiku='kakujiku')
except Exception as e:
logerr("error while moving arm to tote: %s" % e)
# try all possible grab strategies
tried_strategies = 0
for grab_strategy in grab.select_strategies(next_item, "tote"):
# check if this strategy can locate the item
try:
can_grab = grab_strategy.can_grab_item(
next_item, "tote", items_in_tote)
except Exception as e:
logerr("error while checking for grabability: %s" % e)
continue
if not can_grab:
loginfo("strategy %s cannot grab item %s" %
(grab_strategy, next_item))
# if we can, we should find out which items are grabbable
# from here and then use this as priority list
if CHOICE_MODE == ItemChoiceMode.VISIBLE and \
isinstance(grab_strategy, grab.vacuum.VacuumGrabbing):
try:
secondary_item_priorities = grab_strategy.sort_item_priority_tote(
).tolist()
except Exception as e:
logerr(
"error during sort_item_priority_tote(): %s" %
e)
raise e
secondary_item_priorities = []
continue
else:
secondary_item_priorities = []
loginfo("can_grab_item was True, try strategy %s for item %s" %
(grab_strategy, next_item))
tried_strategies += 1
# try to grab the item with this strategy
try:
success = grab_strategy.grab_from_tote(next_item)
except Exception as e:
logerr("error while taking item %s: %s" % (next_item, e))
# NB. this is not good, we have no idea what happened here;
# the arm is in an undefined state
# TODO: need to reset the state somehow
continue
if not success:
loginfo("failed to take %s with %s" %
(next_item, grab_strategy))
continue
else:
loginfo("succeeded to take item %s" % next_item)
try:
success = grab_strategy.stow_in_shelf(
target_bin, target_pos)
except Exception as e:
logerr("error while stowing item %s: %s" % (next_item, e))
# uh, this is not good, we removed the item from the shelf,
# but we failed to put it somewhere.
success = False
if not success:
# same as exception case before
arm_control_wrapper.move_left_arm_to_bin(
"bin_H",
"rpre",
keitai='nut000',
kakujiku='kakujiku',
speed="fast")
logwarn("we failed to stow item %s in shelf" % next_item)
#self.pos_info.take_from_tote(next_item)
#self.pos_info.drop(next_item)
continue
else:
# if we arrive here, we put the item to the shelf successfully
self.pos_info.take_from_tote(next_item)
self.pos_info.put_into_bin(target_bin, next_item)
try:
(mv_success, _, _,
_) = arm_control_wrapper.move_left_arm_to_bin(
"bin_H", "rpre", keitai='nut000', kakujiku='kakujiku')
except Exception as e:
logerr("error while moving arm to tote: %s" % e)
#raise e
# write intermediate output file
util.write_output_file(self.pos_info,
"current_stow_status.json")
items_in_tote.remove(next_item)
item_list.remove((num_tries, next_item))
break
else:
if tried_strategies > 0:
logwarn("we failed to take %s with any strategy" %
next_item)
item_list.remove((num_tries, next_item))
item_list.append((num_tries + 1, next_item))
else:
logwarn("we failed to detect %s with any strategy" %
next_item)
item_list.remove((num_tries, next_item))
item_list.append((num_tries + 1, next_item))
def execute(self):
super(SmallBinFirstStowing, self).calibrate()
# get a stratey to determine free space
stow_strategy = shelf_stow_locator.select_strategy(self.pos_info)
loginfo("ShelfStowLocatorStrategy: %s" % stow_strategy)
try_count = 0
items_in_tote = self.pos_info.tote[:]
while items_in_tote and try_count < 20:
try_count += 1
next_item = util.get_smallest_item(items_in_tote)
loginfo("======================================")
loginfo("attempt to take item %s from tote" % next_item)
# first, find a suitable bin
loginfo("find a location to move that item to")
(target_bin, target_pos) = \
stow_strategy.find_location(next_item)
loginfo("if we succeed to take %s, we will put it to %s/%s" %
(next_item, target_bin, target_pos))
# move pre->front->item_front->item_adj
arm_control_wrapper.move_left_arm_to_bin(target_bin,'pre')
(_,_,_,photo_pos) = arm_control_wrapper.move_left_arm_to_bin(target_bin,'photo')
time.sleep(3.0) # wait for camera delay
# take snapshot and imgprocs
# TODO another photo directions
rospy.loginfo('take snapshot and imgprocs')
resp = self.segm_service_left(['a'])
Snapshot.save_snapshot(resp, str(self.log_cnt), 'log_imgs_raw')
target_pos = self.stow_imgproc(bin, resp, photo_pos, next_item)
# back to pre
rospy.loginfo('back to pre')
arm_control_wrapper.move_left_arm_to_bin(bin,'pre')
# move the arm back to tote
try:
(mv_success, _, _, _) = arm_control_wrapper.move_left_arm_to_bin("tote",
"photo", keitai='nut000', kakujiku='kakujiku')
except Exception as e:
logerr("error while moving arm to tote: %s" % e)
# try all possible grab strategies
tried_strategies = 0
for grab_strategy in grab.select_strategies(next_item,
"tote"):
# check if this strategy can locate the item
try:
can_grab = grab_strategy.can_grab_item(next_item, "tote",
items_in_tote)
except Exception as e:
logerr("error while checking for grabability: %s" % e)
continue
if not can_grab:
loginfo("strategy %s cannot grab item %s" %
(grab_strategy, next_item))
continue
loginfo("can_grab_item was True, try strategy %s for item %s" %
(grab_strategy, next_item))
tried_strategies += 1
# try to grab the item with this strategy
try:
success = grab_strategy.grab_from_tote(next_item)
except Exception as e:
logerr("error while taking item %s: %s" % (next_item, e))
# NB. this is not good, we have no idea what happened here;
# the arm is in an undefined state
# TODO: need to reset the state somehow
continue
if not success:
loginfo("failed to take %s with %s" % (next_item, grab_strategy))
continue
else:
loginfo("succeeded to take item %s" % next_item)
try:
success = grab_strategy.stow_in_shelf(target_bin, target_pos)
except Exception as e:
logerr("error while stowing item %s: %s" % (next_item, e))
# uh, this is not good, we removed the item from the shelf,
# but we failed to put it somewhere.
continue
if not success:
# same as exception case before
logwarn("we failed to stow item %s in shelf" % next_item)
continue
# if we arrive here, we put the item to the shelf successfully
self.pos_info.take_from_tote(next_item)
self.pos_info.put_into_bin(target_bin, next_item)
items_in_tote.remove(next_item)
break
else:
if tried_strategies > 0:
logwarn("we failed to take %s with any strategy" % next_item)
else:
logwarn("we failed to detect %s with any strategy" % next_item)
def get_item(self, main_item, source_bin, items_in_bin, try_count=1):
FAIL_MODE = (False, None, [])
# check input data plausability
if not items_in_bin:
logerr("according to the inventory, there are no items " +
"in the bin, but we tried to grab %s" % main_item)
return FAIL_MODE
elif main_item not in items_in_bin:
logerr(("according to the inventory, the items in %s are " +
"%s, but we tried to grab %s") %
(source_bin, items_in_bin, main_item))
return FAIL_MODE
# move the left arm to the bin at hand (actually this is not
# exactly necessary, as every grab strategy will move the
# arm as required)
try:
(success, _, _,
_) = arm_control_wrapper.move_left_arm_to_bin(source_bin,
"pre",
keitai='nut000',
kakujiku='kakujiku',
speed="fast")
except Exception as e:
logerr("error while moving arm to bin: %s" % e)
return FAIL_MODE
if not success:
return FAIL_MODE
# first, we try to take the item at hand (that is,
# detect whether we can take it)
(result, strategy, other_item_priorities) = \
self.__try_take_item(main_item, source_bin, items_in_bin)
main_item_strategy = strategy
if result == GrabResult.SUCCESS:
# great, we got it
return (True, strategy, [])
elif result == GrabResult.FAILED_GRAB:
# all strategies that declared that they can
# grab the item failed to do so
return (False, strategy, []) #FAIL_MODE
elif result == GrabResult.CANNOT_GRAB:
if len(items_in_bin) > try_count * 3:
# we cannot see this item currently and the box is
# quite crowded (at least four items at the first
# try, at least seven items at the second), therefore
# we skip this for now
raise util.TooCrowdedTryLater()
# if we arrive here, none of the strategies claimed the
# item to be grabbable, so we move away some other object
logwarn(("none of the known strategies could take %s, " +
"we will move away other items") % main_item)
# get a strategy to discover place for intermediate items
stow_strategy = shelf_stow_locator.select_bin2bin_strategy(
self.pos_info)
# due to the plausability checks on top, we know that items_in_bin
# is non-empty and that it contains main_item
other_items = items_in_bin[:]
other_items.remove(main_item)
# remove items not good for bin2bin
notgood = [
"cherokee_easy_tee_shirt", "creativity_chenille_stems",
"kleenex_paper_towels", "scotch_bubble_mailer", "hanes_tube_socks",
"fiskars_scissors_red", "oral_b_toothbrush_red",
"oral_b_toothbrush_green", "cool_shot_glue_sticks",
"dasani_water_bottle"
]
for ng in notgood:
if ng in other_items:
loginfo(
"remove %s from the list of potential bin2bin candidates" %
ng)
other_items.remove(ng)
# if we are lucky, `other_item_priorities` contains all known
# item_ids in an easy-to-move order
try:
other_item_priorities = other_item_priorities.tolist()
except:
other_item_priorities = []
if other_item_priorities:
other_items.sort(key=lambda x: other_item_priorities.index(
util.item_string_to_id(x)))
if len(other_items) == 0:
# there is no item to move away, we simply failed to detect
# the item in the bin
logwarn("we failed to detect %s with all strategies, and " %
main_item + "there were no other (movable) items :-(")
return (False, main_item_strategy, []) #FAIL_MODE
else:
moved_items = []
for try_item in other_items:
loginfo("we try to move %s out of the way" % try_item)
# find stow position in case we can take this item
loginfo("find a location to move that item to")
(stow_bin, stow_pos) = stow_strategy.find_location(
try_item, forbidden_bin=source_bin)
loginfo("if we succeed to take %s, we will put it to %s/%s" %
(try_item, stow_bin, stow_pos))
# maybe find_location has moved the arm away, but
# moving back is the responsibility of can_grab_item,
# so we do not explicitly do that here
# move the arm back
# now try to take that item
(result, strategy,
_) = self.__try_take_item(try_item, source_bin, items_in_bin)
if result == GrabResult.SUCCESS:
break
else:
loginfo("failed to take %s, try next item" % try_item)
else:
# if we arrive here, we were not able to grab a single
# item from that bin. this means failure
return (False, main_item_strategy, [])
# if we arrive here, we were able to take try_item from the bin
try:
loginfo("trying to stow %s in %s" % (try_item, stow_bin))
stow_success = strategy.stow_in_shelf(stow_bin,
stow_pos,
item=try_item)
except Exception as e:
# uh, this is not good, we removed the item from the shelf,
# but we failed to put it somewhere.
logerr("error while stowing item %s in shelf: %s" %
(try_item, e))
self.pos_info.take_from_bin(source_bin, try_item)
self.pos_info.drop(try_item)
return (False, main_item_strategy, [])
if not stow_success:
# we failed to put the item in the shelf, but still it
# is removed out of the previous shelf, so we basically
# achieved our intermediate goal, even if we will get
# minus points. proceed like normal
logwarn("we failed to stow item %s in shelf" % try_item)
self.pos_info.take_from_bin(source_bin, try_item)
self.pos_info.drop(try_item)
else:
loginfo("succeeded to stow item %s in shelf" % try_item)
self.pos_info.take_from_bin(source_bin, try_item)
self.pos_info.put_into_bin(stow_bin, try_item)
# write intermediate output file
util.write_output_file(self.pos_info, "current_pick_status.json")
# now that we moved one item away, start over again
remaining_items_in_bin = items_in_bin[:]
remaining_items_in_bin.remove(try_item)
(success, strategy,
sub_moved_items) = self.get_item(main_item, source_bin,
remaining_items_in_bin)
return (success, strategy, moved_items + sub_moved_items)
def move(self, grabber):
grabber.pipe_90_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'approach_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.target_pos,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
force_retry = True if self.item in force_retry_items else False
time.sleep(0.5)
if (not grabber.vac_ok(self.item).success):
grabber.vacuum_on()
print 'high pressure -> retry #1'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([0, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([0, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #2'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
print 'pre_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pull',
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pre',
keitai='nut000',
kakujiku='kakujiku')
if force_retry:
arm_control_wrapper.move_left_arm_to_bin('bin_K',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('bin_H',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
else:
grabber.vacuum_off()
return False
arm_control_wrapper.move_left_arm_to_bin(
self.bin, 'pull', keitai='fut000',
kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
return True
def move(self, grabber):
grabber.pipe_90_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'approach_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.target_pos,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
force_retry = True if self.item in force_retry_items else False
# retry
if (not grabber.vac_ok(self.item).success) and self.target_pos[2] > 40:
grabber.vacuum_on()
print 'high pressure -> retry #1'
grabber.pipe_45_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 0, -20, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(
self.item).success) and self.target_pos[0] < 450:
print 'high pressure -> retry #2'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
if self.item in [
'dasani_water_bottle', 'command_hooks',
'platinum_pets_dog_bowl'
]: # transparent items. (they are sometimes depth deeper)
if not grabber.vac_ok(self.item).success:
print 'high pressure -> retry #4'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-70, 0, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
else:
if (not grabber.vac_ok(
self.item).success) and self.target_pos[0] < 400:
print 'high pressure -> retry #4'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([50, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
#if not grabber.vac_ok(self.item).success:
# print 'high pressure -> retry #5'
# grabber.pipe_90_deg()
# arm_control_wrapper.move_left_arm_local(self.bin, self.approach_target_pos, keitai='fut000', kakujiku='chokusen')
# arm_control_wrapper.move_left_arm_local(self.bin, self.target_pos + np.asarray([-50,0,0,0,0,0]), keitai='fut000', kakujiku='chokusen')
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
print 'pre_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pull',
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pre',
keitai='nut000',
kakujiku='kakujiku')
if force_retry:
arm_control_wrapper.move_left_arm_to_bin('bin_K',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('bin_H',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
else:
grabber.vacuum_off()
return False
arm_control_wrapper.move_left_arm_to_bin(
self.bin, 'pull', keitai='fut000',
kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
return True
def execute(self, items):
# get the strategy for how to prepare the actual pick
get_item_strategy = get_item.select_strategy(self.pos_info)
loginfo("GetItemFromBinStrategy: %s" % get_item_strategy)
# get the strategy for where to store items
target_locator = tote_stow_locator.select_strategy(self.pos_info)
loginfo("ToteStowLocatorStrategy: %s" % target_locator)
calib_ok = super(DifficultyOrderPicking, self).calibrate()
if not calib_ok:
logerr("calibration failed")
return False
arm_control_wrapper.move_left_arm_to_bin('bin_A','pre', keitai='nut000', kakujiku='kakujiku', speed="fast")
checkpoint = time.time()
#calc score and sort
scores = self.pos_info.score_to_pick()
scores_with_try_num = []
for bin, num_items_score in scores.items():
for item, bin_of_item in items:
if bin_of_item == bin:
item_name = item
break
if item_name in ["rolodex_jumbo_pencil_cup", "fitness_gear_3lb_dumbbell"]:
# try them after we have tried everything else twice
try_num = 10
elif item_name in ["dasani_water_bottle"]:
# try them after we have tried everything else twice
try_num = 30
else:
try_num = 1
# we re-evaluate the score for the order here. we have:
# - num_items_score: 9 (one item in bin) ~ 0 (ten items in bin)
# - item_bonus: 0 (simple item) ~ 3 (hard item)
# - item_difficulty: 0 (simple item) ~ 3 (hard item)
# - pick_points: 10 (1/2 items), 15 (3/4 items), 20 (5+ items)
item_bonus = util.item_string_to_bonus(item_name)
item_difficulty = util.item_string_to_difficulty(item_name)
pick_points = self.pos_info.bin_pick_points(bin)
# compute some reasonable the-lower-the-better score
# - try easy items first.
# - if equally easy, take the emptier bin first.
score = 0*item_difficulty - num_items_score
scores_with_try_num.append((bin, score, item_name, try_num))
# we sort by try_num (asc), then by score (desc). that is, first
# we try all the items, then we try earlier failures again
sorted_scores = sorted(scores_with_try_num,
key=lambda x: (x[3], x[1]))
i = 0
while sorted_scores:
# sort list in case earlier iterations have appended items
sorted_scores.sort(key=lambda x: (x[3], x[1]))
bin, score, item_name, try_num = sorted_scores.pop(0)
if i != 0:
loginfo("completed %d-th item in %s (total: %s)" %
(i,
util.get_elapsed_time(checkpoint),
util.get_elapsed_time(self.start)))
checkpoint = time.time()
i += 1
if i > 100:
logwarn("exiting after 1000 main iterations")
break
loginfo("======================================")
loginfo("attempt to pick item %s from %s (attempt %d, iteration %d)" %
(item_name, bin, try_num, i))
items_in_bin = [str(s) for s in self.pos_info.shelf[bin]]
loginfo("items in that bin: %s" % items_in_bin)
# compute the target_position for the item we want to move
# (this may use any of the arms and move it to the tote!)
try:
target_position = target_locator.find_location(item_name)
except Exception as e:
logerr("failed to find a stow location for %s: %s" %
(item_name, e))
# we cannot continue with that item, proceed with the next one
continue
loginfo("target position for %s in the tote: %s" %
(item_name, target_position))
# take the item
try:
(pick_success, grab_strategy, moved_items) = \
get_item_strategy.get_item(item_name, bin, items_in_bin, try_num)
except util.TooCrowdedTryLater:
logwarn("couldn't find item %s in a crowded bin, try again later" % item_name)
sorted_scores.append((bin, score, item_name, try_num + len(items_in_bin)))
continue
except Exception as e:
logerr("an exception occured during get_item(%s, %s, %s, %d): %s" %
(item_name, bin, items_in_bin, try_num, e))
continue
# NB. we used to update the moved_items here, but this
# was moved into the get_item_strategy
# check if our pick was successful
if not pick_success:
logwarn("we failed to pick the item %s" % item_name)
try:
# clear cache
logwarn("clear cache!")
if grab_strategy.final_mode[bin]:
grab_strategy.strategy_cache[bin] = []
grab_strategy.final_mode[bin] = False
else:
grab_strategy.final_mode[bin] = True
except Exception as e:
logwarn("clear cache tried but not vacuumgrabbing %s"%e)
sorted_scores.append((bin, score, item_name, try_num + 5 + len(items_in_bin)))
continue
# put the item to the tote
try:
grab_strategy.stow_in_tote(target_position, item=item_name)
except Exception as e:
logerr("failed to put item to box: %s" % e)
try:
# update the locations
self.pos_info.take_from_bin(bin, item_name)
self.pos_info.put_into_tote(item_name)
except Exception as e:
logerr("failed to update item locations: %s" % e)
# write intermediate output file
util.write_output_file(self.pos_info, "current_pick_status.json")
