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")