# 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)
#test: visualize result
#for v in kb.vantage_point_xforms:
# (save,result) = resource.edit(v,kb.vantage_point_xforms[v],'RigidTransform',description="vantage point for bin "+v,world=world)
r = PerceptionInterface(kb)
kb.shelf_xform = r.localizeShelf()
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)
