kb = KnowledgeBase()
kb.robot_state = RobotState(robot)
print kb.robot_state
kb.active_limb = 'left'
kb.bin_contents = apc_order['bin_contents']
# make fake bin vantage points, need to update to real vantage points at some point
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)
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')
# TODO: Proper shelf transform needs created here
r = PerceptionInterface(kb)
# make fake bin vantage points, need to update to real vantage points at some point
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')
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]
paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ]
visualization.debug_plan(kb, sum(paths, [ kb.robot_state.sensed_config ]))
p.robot.setConfig(paths[-1][-1])
kb.robot_state.commanded_config = paths[-1][-1]
plan = p.planMoveObjectToOrderBin('right')[0]
kb.bin_bounds = json.load(open('kb/bin_bounds.json'))
# kb.object_xforms['cheezit_bit_original'] = (se3.identity()[0], (0.7, 0.5, 1))
# make fake bin vantage points, need to update to real vantage points at some point
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)
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'
