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)
