Пример #1
0
from time import sleep

kb = KnowledgeBase()
parallel = False

if parallel:
    ps = PerceptionServer(kb)

    task = ps.localizeOrderBin()
    while task.count_success() < 1 and not task.all_done():
        print 'waiting...'
        sleep(1)
    # cancel tasks if timed out
    task.cancel()

    if task.count_success() >= 1:
        (xform, cloud) = task.get_success()[0].result

        print xform
        print cloud
    else:
        print 'failed'

else:
    p = PerceptionInterface(kb)

    (xform, cloud) = p.localizeSpecificObject('bin', 'object')

    print xform
    print cloud
Пример #2
0
robot = LowLevelController(model, 'baxter_with_reflex_gripper_col.rob')
print 'here'

kb = KnowledgeBase()
kb.robot_state = RobotState(robot)
print kb.robot_state
kb.active_limb = 'left'
kb.bin_contents = apc_order['bin_contents']

# make fake bin vantage points, need to update to real vantage points at some point
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)
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)
Пример #3
0
    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
# kb.object_xforms['cheezit_bit_original'] = (se3.identity()[0], (0.7, 0.5, 1))


# TODO: Proper shelf transform needs created here
r = PerceptionInterface(kb)

# make fake bin vantage points, need to update to real vantage points at some point
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)
#test: visualize result
#for v in kb.vantage_point_xforms:
#	(save,result) = resource.edit(v,kb.vantage_point_xforms[v],'RigidTransform',description="vantage point for bin "+v,world=world)

r = PerceptionInterface(kb)

kb.shelf_xform = r.localizeShelf()
kb.order_bin_xform = r.localizeOrderBin()
#kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70))