def __init__(self, apc_order, test=False):
# if currently a testing run
if not test:
Master.__init__(self,apc_order)
# if currently not a testing run
else:
self._create_knowledge_base()
self._load_apc_order(apc_order)
# load the .csv file in /apc/high_level_planning/...csv
load_strategy_constants("apc/high_level_planning/Strategy Optimization.csv")
# defaultdict is a "high-performance container datatype". Similar to a dictionary
# Using "list" as the argument of defaultdict
self.order_by_bin = defaultdict(list)
# sort the order item by bin
# i.e.) [ (binA,[item1, item2]), (binB,[item4,item9]), (...), ... ]
for orderitem in self.order:
self.order_by_bin[orderitem['bin']].append(orderitem['item'])
self.cache = False
print "Loading prior point clouds"
# load prior point cloud, transforms from databse for items in the knowledgeBase
if self.cache:
for bin,objects in self.knowledge_base.bin_contents.iteritems():
for o in objects:
pcd_file_name = '/tmp/'+o+'_'+bin+".pcd"
if os.path.exists(pcd_file_name):
self.knowledge_base.object_xforms[o] = se3.identity()
self.knowledge_base.object_clouds[o] = pcd.read(open(pcd_file_name))[1]
#test: visualize result
#for v in kb.vantage_point_xforms:
# (save,result) = resource.edit(v,kb.vantage_point_xforms[v],'RigidTransform',description="vantage point for bin "+v,world=world)
r = PerceptionInterface(kb)
kb.shelf_xform = r.localizeShelf()
kb.order_bin_xform = r.localizeOrderBin()
#kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70))
# kb.object_xforms['kong_sitting_frog_dog_toy'] = se3.identity()
# kb.object_clouds['kong_sitting_frog_dog_toy'] = pcd.read(open('/tmp/kong_sitting_frog_dog_toy_bin_C.pcd'))[1]
kb.object_xforms['safety_works_safety_glasses'] = se3.identity()
kb.object_clouds['safety_works_safety_glasses'] = pcd.read(open('/tmp/safety_works_safety_glasses_bin_G.pcd'))[1]
kb.object_xforms['stanley_66_052'] = se3.identity()
kb.object_clouds['stanley_66_052'] = pcd.read(open('/tmp/stanley_66_052_bin_F.pcd'))[1]
p = PlanningInterface(kb)
p.planMoveToInitialPose()
kb.target_bin = 'bin_F'
plan = p.planGraspObjectInBin('bin_F','stanley_66_052')
plan = plan[0]
def localizeSpecificObject(self, bin, object):
logger.info('localizing {} in {}'.format(bin, object))
if fake_wait(10, 0.8):
return (se3.identity(), pcd.read(open('/tmp/expo_dry_erase_board_eraser_bin_D.pcd'))[1], dict(zip(self.knowledge_base.bin_contents[bin], [1.0 / len(self.knowledge_base.bin_contents[bin])] * len(self.knowledge_base.bin_contents))))
else:
return None
