for k in [ 'bin_vantage_points', 'vantage_point_xforms' ]:
path = os.path.join(knowledge_base_dir, '{}.json'.format(k))
data = json.load(open(path))
setattr(kb, k, data)
r = PerceptionInterface(kb)
kb.shelf_xform = r.localizeShelf()
kb.order_bin_xform = r.localizeOrderBin()
p = PlanningInterface(kb)
p.planMoveToInitialPose()
for letter in 'ABCDEFGHIJKL':
plan = p.planGraspObjectInBin('bin_'+letter,'kong_air_dog_squeakair_tennis_ball')
if(plan is None):
print bcolors.FAIL+'=================================bin_'+letter+' failed================================'+bcolors.ENDC
continue
p.robot.setConfig(plan[0][-1][1][0])
kb.robot_state.sensed_config = plan[0][-1][1][0]
kb.grasped_object = 'kong_air_dog_squeakair_tennis_ball'
plan = p.planMoveObjectToOrderBin('right')
if plan is None:
print bcolors.FAIL+'=================================bin_'+letter+' failed================================'+bcolors.ENDC
else:
print bcolors.OKGREEN+'+++++++++++++++++++++++++++++++++bin_'+letter+' success++++++++++++++++++++++++++++++++'+bcolors.ENDC
