Example #1
0
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)

kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70))
Example #2
0
        sleep(1)
    print kb.robot_state.sensed_config

    for plan in ["test plan", "test plan 2"]:
        task = cs.execute(plan)
        if not task:
            print "restarting ControlServer"
            cs.start()
            continue

        while not task.done:
            print "waiting..."
            sleep(1)

        if not task.error:
            print "success:", task.result
            print kb.robot_state.sensed_config
        else:
            print "failure:", task.error

    cs.close()

else:
    model = WorldModel().loadRobot("klampt_models/baxter_with_parallel_gripper_col.rob")
    robot = LowLevelController(model)

    kb.robot_state = RobotState(robot)

    c = ControlInterface(robot, kb)
    c.execute("test plan", sleep)