world = WorldModel() model = world.loadRobot('klampt_models/baxter_with_reflex_gripper_col.rob') robot = LowLevelController(model) #initial config 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]) #bin f config # 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.30158718316595046, 0.1653559427347274, 2.2363132059607045, 1.834400059027026, 1.0639366601020779, 0.0, -1.0132113159072906, 2.7856916491114374, 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 = 'left' kb.bin_contents = apc_order['bin_contents'] kb.withdraw_point_xforms = json.load(open('kb/withdraw_point_xforms.json')) kb.bin_bounds = json.load(open('kb/bin_bounds.json')) try: reader = csv.reader(open(os.path.join(knowledge_base_dir,'Object Dimensions.csv'))) header = None for line in reader: if header == None: header = line else: kb.object_minimum_dims[line[0]] = float(line[1])*0.01 kb.object_maximum_dims[line[0]] = float(line[2])*0.01 kb.object_median_dims[line[0]] = float(line[3])*0.01 except IOError: logger.info("Object dimensions unknown, try filling in Object Dimensions.csv") pass