def __init__(self, apc_order, test=False): # if currently a testing run if not test: Master.__init__(self,apc_order) # if currently not a testing run else: self._create_knowledge_base() self._load_apc_order(apc_order) # load the .csv file in /apc/high_level_planning/...csv load_strategy_constants("apc/high_level_planning/Strategy Optimization.csv") # defaultdict is a "high-performance container datatype". Similar to a dictionary # Using "list" as the argument of defaultdict self.order_by_bin = defaultdict(list) # sort the order item by bin # i.e.) [ (binA,[item1, item2]), (binB,[item4,item9]), (...), ... ] for orderitem in self.order: self.order_by_bin[orderitem['bin']].append(orderitem['item']) self.cache = False print "Loading prior point clouds" # load prior point cloud, transforms from databse for items in the knowledgeBase if self.cache: for bin,objects in self.knowledge_base.bin_contents.iteritems(): for o in objects: pcd_file_name = '/tmp/'+o+'_'+bin+".pcd" if os.path.exists(pcd_file_name): self.knowledge_base.object_xforms[o] = se3.identity() self.knowledge_base.object_clouds[o] = pcd.read(open(pcd_file_name))[1]
logging.getLogger('integration.jobs').setLevel(logging.WARNING) logging.getLogger('integration.visualization').setLevel(logging.WARNING) logging.getLogger('integration.interface.pcd').setLevel(logging.WARNING) logging.getLogger('integration.interface.real').setLevel(logging.WARNING) logging.getLogger('integration.interface.fake').setLevel(logging.WARNING) logging.getLogger('integration.control_server').setLevel(logging.ERROR) logging.getLogger('integration.master').setLevel(logging.INFO) logging.getLogger('planning.control').setLevel(logging.INFO) import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'top_shelf.json'))) from klampt import se3, so3, vectorops from klampt.robotsim import WorldModel master = Master(apc_order) from time import sleep from planning.planner import LimbPlanner import baxter_scoop as baxter # master._set_target('bin_A', 'crayola_64_ct') # master._set_target('bin_B', 'kong_duck_dog_toy') master._localize_shelf_and_order_bin() # master._localize_object('unused') # master.knowledge_base.active_limb = 'left' # master.knowledge_base.grasped_object = 'crayola_64_ct' # master.knowledge_base.active_grasp = [[0.8449032475481839, 0.5332477717385703, -0.0422530024777488, -0.053105482443014856, 0.005018582272109273, -0.9985762973185823, -0.532276535286903, 0.8459442226103591, 0.03255859663963079], [0.1039670743637746, -0.1590815806021303, 0.07642602363877261]] task = master.manager.control.update() while not task.done: sleep(0.1)
#logging.getLogger('integration.master').setLevel(logging.WARNING) logging.getLogger('perception.segmentation.shelf').setLevel(logging.WARNING) logging.getLogger('perception.segmentation.icp').setLevel(logging.WARNING) logging.getLogger('perception.segmentation.blob').setLevel(logging.WARNING) logging.getLogger('planning.control').setLevel(logging.WARNING) # allow importing from the repository root import sys, os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from integration.master import Master import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'all_bins_order.json'))) master = Master(apc_order) from time import sleep for request in master.order: print ' {}: {}'.format(request['bin'], request['item']) task = master.manager.control.update() while not task.done: sleep(0.1) master._move_initial() while master.order: request = master.order.pop(0) master._set_target(request['bin'], request['item']) master._move_vantage_point(master.knowledge_base.bin_vantage_points[request['bin']][0])
logging.getLogger('OpenGL').setLevel(99) logging.getLogger('integration.jobs').setLevel(logging.WARNING) logging.getLogger('integration.visualization').setLevel(logging.WARNING) logging.getLogger('integration.interface.pcd').setLevel(logging.WARNING) logging.getLogger('integration.interface.real').setLevel(logging.WARNING) logging.getLogger('integration.interface.fake').setLevel(logging.WARNING) logging.getLogger('integration.control_server').setLevel(logging.ERROR) logging.getLogger('integration.master').setLevel(logging.INFO) logging.getLogger('planning.control').setLevel(logging.INFO) import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'top_shelf.json'))) from klampt import se3 master = Master(apc_order) from time import sleep for request in master.order: print request['bin'], request['item'] master._set_target('bin_A', 'crayola_64_ct') # master._set_target('bin_B', 'kong_duck_dog_toy') master._localize_shelf_and_order_bin() # master._localize_object('unused') master.knowledge_base.active_limb = 'left' # master.knowledge_base.grasped_object = 'crayola_64_ct' # master.knowledge_base.active_grasp = [[0.8449032475481839, 0.5332477717385703, -0.0422530024777488, -0.053105482443014856, 0.005018582272109273, -0.9985762973185823, -0.532276535286903, 0.8459442226103591, 0.03255859663963079], [0.1039670743637746, -0.1590815806021303, 0.07642602363877261]] task = master.manager.control.update() while not task.done: sleep(0.1)
logging.getLogger('planning.control').setLevel(logging.WARNING) # allow importing from the repository root import sys, os, readline sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from integration.master import Master from integration.io import pcd import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'example2.json'))) from klampt import se3, so3, vectorops master = Master(apc_order) from time import sleep for request in master.order: print ' {}: {}'.format(request['bin'], request['item']) # master._set_target('bin_B', 'genuine_joe_plastic_stir_sticks') #master._set_target('bin_F','stanley_66_052') master._set_target('bin_C','champion_copper_plus_spark_plug') #master._set_target('bin_E','mead_index_cards') #master._set_target('bin_A', 'elmers_washable_no_run_school_glue') # master._set_target('bin_B', 'kong_duck_dog_toy') # master._set_target('bin_F', 'mead_index_cards') #master._set_target('bin_F', 'kong_sitting_frog_dog_toy') #master._set_target('bin_F', 'champion_copper_plus_spark_plug') # master._set_target('bin_C', 'first_years_take_and_toss_straw_cups')
logging.getLogger('integration.io.pcd').setLevel(logging.WARNING) logging.getLogger('integration.camera.client').setLevel(logging.WARNING) logging.getLogger('integration.camera.packet').setLevel(logging.WARNING) logging.getLogger('integration.interface.fake').setLevel(logging.WARNING) logging.getLogger('integration.control_server').setLevel(logging.WARNING) logging.getLogger('integration.master').setLevel(logging.INFO) logging.getLogger('perception.shelf').setLevel(logging.WARNING) logging.getLogger('perception.icp').setLevel(logging.WARNING) logging.getLogger('perception.blob').setLevel(logging.WARNING) # allow importing from the repository root import sys, os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from integration.master import Master import json apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'test_shelf.json'))) # speed up the tests from integration.interface import fake fake.delay_scale = 1 fake.fail_scale = 100 m = Master(apc_order) while m.order: request = m.order.pop(0) if not m.run(request['bin'], request['item']): m.order.append(request)