from klampt import se3,so3 from time import sleep 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()