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