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()
