def plan_commands(state, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) sim_world = connect(use_gui=False) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) #robot = load_model(DRAKE_PR2_URDF, fixed_base=True) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) clone_world(client=sim_world, exclude=[task.robot]) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, hierarchy=hierarchy, debug=False, success_cost=MAX_COST, verbose=verbose) pr.disable() plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) if profile: pstats.Stats(pr).sort_stats('tottime').print_stats(10) saved_world.restore() commands = post_process(state, plan) disconnect() return commands
def plan_commands(state, args, profile=True, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world sim_world = connect(use_gui=args.viewer) #clone_world(client=sim_world) task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) mapping = clone_world(client=sim_world, exclude=[ task.robot ]) # TODO: TypeError: argument 5 must be str, not bytes assert all(i1 == i2 for i1, i2 in mapping.items()) set_client(sim_world) saver = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=args.teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] with Profiler(field='cumtime', num=10 if profile else None): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, hierarchy=hierarchy, debug=False, success_cost=MAX_COST, verbose=verbose) plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) commands = post_process(state, plan) saver.restore() disconnect() return commands