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)
while True:
print '{} {} {} >'.format(
master.knowledge_base.active_limb or '-',
master.knowledge_base.target_bin or '-',
master.knowledge_base.target_object or '-',
# 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])
master._localize_object(master.knowledge_base.bin_vantage_points[request['bin']][0])
task = master.manager.control.update()
while not task.done: sleep(0.1)
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')
# master.knowledge_base.object_xforms['crayola_64_ct'] = ( so3.mul(so3.rotation([1,0,0], 3.1415), so3.rotation([0,0,1], 3.1415/4+3.1415)), (0.995, 0.27, 0.84))
master.knowledge_base.active_limb = 'right'
master._localize_shelf_and_order_bin()
#master._localize_object('unused')
# master.knowledge_base.grasped_object = 'champion_copper_plus_spark_plug'
# master.knowledge_base.object_xforms['first_years_take_and_toss_straw_cups'] = se3.identity()
# master.knowledge_base.object_clouds['first_years_take_and_toss_straw_cups'] = pcd.read(open('/tmp/first_years_take_and_toss_straw_cups_bin_C.pcd'))[1]
# master.knowledge_base.object_xforms['safety_works_safety_glasses'] = se3.identity()
