def jsonTest(heuristic, strategies, jsonFilename, useVision = False, withPause = False, isExecute = True, toSort = False): objectiveBinPos = [1.15, 0 , 0.2] import json from pprint import pprint with open(jsonFilename) as data_file: data = json.load(data_file) bin_contents_all = data['bin_contents'] work_order = data['work_order'] if toSort: work_order = sortOrder(work_order, bin_contents_all) displayOrder(work_order, bin_contents_all) for orderInd, order in enumerate(work_order): #displayOrder(work_order, bin_contents_all, orderInd) bin_id = order['bin'] # e.g. bin_A binNum = bin_id2num(bin_id) # e.g. 0 obj_id = order['item'] # object's id in string print '\n%s\n[Heuristic] [Order %d] bin_id:' % ('-'*70, orderInd), bin_id, binNum, '; obj_id:', obj_id , '\n%s' % ('-'*70) strategy_list = heuristic[obj_id] print '\n[Heuristic] We are done!'
def jsonTest(heuristic, strategies, jsonFilename, useVision=False, withPause=False, isExecute=True, toSort=False): objectiveBinPos = [1.15, 0, 0.2] import json from pprint import pprint with open(jsonFilename) as data_file: data = json.load(data_file) bin_contents_all = data['bin_contents'] work_order = data['work_order'] if toSort: work_order = sortOrder(work_order, bin_contents_all) displayOrder(work_order, bin_contents_all) for orderInd, order in enumerate(work_order): #displayOrder(work_order, bin_contents_all, orderInd) bin_id = order['bin'] # e.g. bin_A binNum = bin_id2num(bin_id) # e.g. 0 obj_id = order['item'] # object's id in string print '\n%s\n[Heuristic] [Order %d] bin_id:' % ( '-' * 70, orderInd), bin_id, binNum, '; obj_id:', obj_id, '\n%s' % ('-' * 70) strategy_list = heuristic[obj_id] print '\n[Heuristic] We are done!'
def jsonTest(heuristic, strategies, jsonFilename, useVision=False, withPause=False, isExecute=True, toSort=False, forceSucc=False): globalSpeedSet = 'yolo' ik.ik.setSpeedByName(globalSpeedSet) objectiveBinPos = [1.15, 0, 0.2] import json from pprint import pprint with open(jsonFilename) as data_file: data = json.load(data_file) if useVision: capsen.capsen.init() bin_contents_all = data['bin_contents'] work_order = data['work_order'] if toSort: work_order = sortOrder(work_order, bin_contents_all) displayOrder(work_order, bin_contents_all) goToHome.goToHome(robotConfig=None, homePos=[1, 0, 1.2], isExecute=isExecute, withPause=withPause) count_suck = 0 count_grasp = 0 for orderInd, order in enumerate(work_order): displayOrder(work_order, bin_contents_all, orderInd) ik.ik.setSpeedByName(globalSpeedSet) count_try = 0 try_max = 10 isDirty = False bin_id = order['bin'] # e.g. bin_A binNum = bin_id2num(bin_id) # e.g. 0 obj_id = order['item'] # object's id in string print '\n%s\n[Heuristic] [Order %d] bin_id:' % ( '-' * 70, orderInd), bin_id, binNum, '; obj_id:', obj_id, '\n%s' % ('-' * 70) if useVision: try: obj_pose, pose_type = percept.percept( obj_id=obj_id, binNum=binNum, bin_contents=bin_contents_all[bin_id], isExecute=isExecute, withPause=withPause) if obj_pose is None: print '[Heuristic] vision failed, continue to next order.' continue except: print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc( ) continue else: obj_pose, pose_type = percept.perceptFromManualFit(obj_id) print '[Heuristic] pose_type:', pose_type, '; pose:', map( shortfloat, obj_pose) resetStrat = True tryFlag = True visionSucc = True while resetStrat and tryFlag and visionSucc: resetStrat = False strategy_list = heuristic[obj_id][pose_type] for i in range(len(strategy_list)): strategy_name = strategy_list[i] print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name count_try += 1 if count_try == try_max: print '[Heuristic] reached maximum trial number' tryFlag = False break if strategy_name == 'percept' or strategy_name == 'percept-hand': if isDirty: print '[Heuristic] perception has been called' if useVision: ik.ik.setSpeedByName(globalSpeedSet) obj_pose, pose_type_new = percept.percept( obj_pose=obj_pose, binNum=binNum, obj_id=obj_id, bin_contents=bin_contents_all[bin_id], isExecute=isExecute, withPause=withPause) else: obj_pose, pose_type_new = percept.perceptFromManualFit( obj_id) if obj_pose is None: visionSucc = False break else: isDirty = False print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose if pose_type_new == pose_type: pose_type = pose_type_new continue else: pose_type = pose_type_new resetStrat = True break else: try: if strategy_name in strategies: goToMouth.goToMouth(robotConfig=None, binNum=binNum, isExecute=True, withPause=withPause) (isDirty, succ) = strategies[strategy_name]( obj_pose, binNum, obj_id, bin_contents_all[bin_id], isExecute=isExecute, withPause=withPause) #run it if forceSucc and (i == len(strategy_list) - 1 or count_try == try_max - 1 ): ###### for testing in virtual succ = True else: print '[Heuristic] Strategy', strategy_name, 'not implemented yet. Skip it.' continue if succ: print '[Heuristic] Strategy', strategy_name, 'success' ik.ik.setSpeedByName('faster') print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name if strategy_name == 'suck-down' or strategy_name == 'suck-side': count_suck = count_suck + 1 attemptGoToBin = goToBin.goToBin( robotConfig=None, objectiveBinPos=objectiveBinPos, isExecute=isExecute, withPause=withPause, withSuction=True, counter=count_suck) else: count_grasp = count_grasp + 1 attemptGoToBin = goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos=objectiveBinPos, isExecute=isExecute, withPause=withPause, withSuction=False, counter=count_grasp) if not attemptGoToBin: goToHome.goToHome(robotConfig=None, homePos=[1, 0, 1.2], isExecute=isExecute, withPause=withPause) goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos=objectiveBinPos, isExecute=isExecute, withPause=withPause, withSuction=False, counter=count_grasp) break except: print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc( ) if withPause: raw_input( '[Heuristic] Press any key to continue next strategy.' ) ik.ik.setSpeedByName(globalSpeedSet) goToHome.goToHome(robotConfig=None, homePos=[1, 0, 1.2], isExecute=isExecute, withPause=withPause) if withPause: raw_input('[Heuristic] Press any key to continue next object.') print '\n[Heuristic] We are done!'
def jsonTest(heuristic, strategies, jsonFilename, useVision = False, withPause = False, isExecute = True, toSort = False, forceSucc = False): globalSpeedSet = 'yolo' ik.ik.setSpeedByName(globalSpeedSet) objectiveBinPos = [1.15, 0 , 0.2] import json from pprint import pprint with open(jsonFilename) as data_file: data = json.load(data_file) if useVision: capsen.capsen.init() bin_contents_all = data['bin_contents'] work_order = data['work_order'] if toSort: work_order = sortOrder(work_order, bin_contents_all) displayOrder(work_order, bin_contents_all) goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause) count_suck = 0 count_grasp = 0 for orderInd, order in enumerate(work_order): displayOrder(work_order, bin_contents_all, orderInd) ik.ik.setSpeedByName(globalSpeedSet) count_try = 0 try_max = 10 isDirty = False bin_id = order['bin'] # e.g. bin_A binNum = bin_id2num(bin_id) # e.g. 0 obj_id = order['item'] # object's id in string print '\n%s\n[Heuristic] [Order %d] bin_id:' % ('-'*70, orderInd), bin_id, binNum, '; obj_id:', obj_id , '\n%s' % ('-'*70) if useVision: try: obj_pose, pose_type = percept.percept(obj_id=obj_id, binNum=binNum, bin_contents = bin_contents_all[bin_id], isExecute = isExecute, withPause = withPause) if obj_pose is None: print '[Heuristic] vision failed, continue to next order.' continue except: print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc() continue else: obj_pose, pose_type = percept.perceptFromManualFit(obj_id) print '[Heuristic] pose_type:', pose_type, '; pose:', map(shortfloat, obj_pose) resetStrat = True tryFlag = True visionSucc = True while resetStrat and tryFlag and visionSucc: resetStrat = False strategy_list = heuristic[obj_id][pose_type] for i in range(len(strategy_list)): strategy_name = strategy_list[i] print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name count_try += 1 if count_try == try_max: print '[Heuristic] reached maximum trial number' tryFlag = False break if strategy_name == 'percept' or strategy_name == 'percept-hand': if isDirty: print '[Heuristic] perception has been called' if useVision: ik.ik.setSpeedByName(globalSpeedSet) obj_pose, pose_type_new = percept.percept(obj_pose = obj_pose, binNum = binNum, obj_id = obj_id, bin_contents = bin_contents_all[bin_id], isExecute = isExecute, withPause = withPause) else: obj_pose, pose_type_new = percept.perceptFromManualFit(obj_id) if obj_pose is None: visionSucc = False break else: isDirty = False print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose if pose_type_new == pose_type: pose_type = pose_type_new continue else: pose_type = pose_type_new resetStrat = True break else: try: if strategy_name in strategies: goToMouth.goToMouth(robotConfig=None, binNum = binNum, isExecute = True, withPause = withPause) (isDirty,succ) = strategies[ strategy_name ](obj_pose, binNum, obj_id, bin_contents_all[bin_id], isExecute = isExecute, withPause = withPause) #run it if forceSucc and (i == len(strategy_list)-1 or count_try == try_max-1): ###### for testing in virtual succ = True else: print '[Heuristic] Strategy', strategy_name , 'not implemented yet. Skip it.' continue if succ: print '[Heuristic] Strategy', strategy_name , 'success' ik.ik.setSpeedByName('faster') print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name if strategy_name == 'suck-down' or strategy_name == 'suck-side': count_suck = count_suck + 1 attemptGoToBin = goToBin.goToBin( robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, withPause = withPause, withSuction = True, counter = count_suck) else: count_grasp = count_grasp + 1 attemptGoToBin = goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, withPause = withPause, withSuction = False, counter = count_grasp) if not attemptGoToBin: goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause) goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, withPause = withPause, withSuction = False, counter = count_grasp) break except: print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc() if withPause: raw_input('[Heuristic] Press any key to continue next strategy.') ik.ik.setSpeedByName(globalSpeedSet) goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause) if withPause: raw_input('[Heuristic] Press any key to continue next object.') print '\n[Heuristic] We are done!'