path = os.path.join(knowledge_base_dir, '{}.json'.format(k)) data = json.load(open(path)) setattr(kb, k, data) r = PerceptionInterface(kb) c = ControlInterface(robot, kb) p = PlanningInterface(kb) kb.shelf_xform = r.localizeShelf() plan = p.planMoveToInitialPose() c.execute(plan[0], sleep) kb.robot_state = RobotState(robot) sleep(5) plan = p.planMoveToVantagePoint('bin_A', 'bin_A_center') c.execute(plan[0], sleep) kb.robot_state = RobotState(robot) sleep(5) kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70)) plan = p.planGraspObjectInBin('bin_A', 'crayola_64_ct') c.execute(plan[0], sleep) # print 'going to control interface' # print 'made control interface' print robot.isMoving() print 'waiting...' sleep(100)
path = os.path.join(knowledge_base_dir, '{}.json'.format(k)) data = json.load(open(path)) setattr(kb, k, data) kb.shelf_xform = localizeShelf() kb.order_bin_xform = localizeOrderBin() p = PlanningInterface(kb) p.planMoveToInitialPose() # cache planning code for move to bin for letter in 'ACDFGIJLKHEB': plan = p.planMoveToVantagePoint('bin_'+letter, 'bin_'+letter+'_center') p.robot.setConfig(plan[0][0][1][-1]) kb.robot_state.sensed_config = plan[0][0][1][-1] # plan = p.makeCachePlans('bin_'+letter, 'bin_'+letter+'_center') # path = plan[0] # limb = plan[1] # #write it out # filename = 'planning/cachedPlans/'+limb+'_from_home_to_bin_' + letter + '.json' # file = open(filename,'w') # json.dump(path,file) # file.close() # p.planMoveToInitialPose()