import json
apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'example.json')))
knowledge_base_dir = './kb/'
world = WorldModel()
model = world.loadRobot('klampt_models/baxter_with_reflex_gripper_col.rob')
print 'before'
robot = LowLevelController(model, 'baxter_with_reflex_gripper_col.rob')
print 'here'
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()
world = WorldModel()
model = world.loadRobot('klampt_models/baxter_with_reflex_gripper_col.rob')
robot = LowLevelController(model)
robot.setMilestone([ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1, 0.0, 2.358, 0.0, 0.0, 0.3, -0.98, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.18, 0.0, 2.118, 0.04, 0.0, 0.62, 0.901, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
# robot.setMilestone([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0999999999999996, 0.0, 2.358, 0.0, 0.0, 0.3, -0.9800000000000004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06679184745163541, 0.365135941112352, 2.5041869579036335, 1.5646287249338475, 0.8377567543461182, 0.0, -1.097366825056886, 2.493786406695833, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
kb = KnowledgeBase()
kb.robot_state = RobotState(robot)
kb.active_limb = 'right'
kb.withdraw_point_xforms = json.load(open('kb/withdraw_point_xforms.json'))
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()
