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] paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ] visualization.debug_plan(kb, sum(paths, [ kb.robot_state.commanded_config ])) #plan = p.failsafeplan('bin_G','safety_works_safety_glasses')[0] #paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ] #p.robot.setConfig(paths[-1][-1]) #kb.robot_state.sensed_config = paths[-1][-1]
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') c.execute(plan[0], sleep) # print 'going to control interface' # print 'made control interface' print robot.isMoving() print 'waiting...' sleep(100)
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') if plan is None: print bcolors.FAIL+'=================================bin_'+letter+' failed================================'+bcolors.ENDC else: print bcolors.OKGREEN+'+++++++++++++++++++++++++++++++++bin_'+letter+' success++++++++++++++++++++++++++++++++'+bcolors.ENDC