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!'
import goToHome
temp_bias = 0.04 #TO-DO: Remove this bias, calculate object height correctly
topple.topple(objPose = [1.55620419979, 0.281148612499, 1.14214038849-temp_bias,0,0,0,0],
objId = 0,
binNum=0,
robotConfig=None,
shelfPosition = [1.9019,0.00030975,-0.503],
forceThreshold = 1,
effect = 'topple',
binDepthReach = 0.37,
isExecute = True,
withPause = False)
scoop.scoop(
objPose = [1.55620419979, 0.281148612499, 1.14214038849,0,0,0,0],
binNum=0,
robotConfig=None,
shelfPosition = [1.9019,0.00030975,-0.503],
isExecute = True,
withPause = False)
goToBin.goToBin(robotConfig=None,
objectiveBinPos = [0.9,0,0],
isExecute = True,
withPause = False)
goToHome.goToHome(robotConfig = None,
homePos = [1,0,1.2],
isExecute = True,
withPause = False)
