def plan_commands(state,
viewer=False,
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
sim_world = connect(use_gui=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])
assert all(i1 == i2 for i1, i2 in mapping.items())
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)
plan, cost, evaluations = solution
if MAX_COST <= cost:
plan = None
print_solution(solution)
print('Finite cost:', cost < MAX_COST)
commands = post_process(state, plan)
pr.disable()
if profile:
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
saved_world.restore()
disconnect()
return commands
def main(time_step=0.01):
parser = create_parser()
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
# TODO: argument for selecting prior
args = parser.parse_args()
print('Arguments:', args)
# TODO: nonuniform distribution to bias towards other actions
# TODO: closed world and open world
real_world = connect(use_gui=not args.viewer)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.25) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_user()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
wait_for_user()
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state, args)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_user()
disconnect()
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
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),
}
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
max_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)
print('Real cost:', float(cost) / SCALE_COST)
if profile:
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
saved_world.restore()
commands = post_process(state, plan)
disconnect()
return commands
def main(time_step=0.01):
# TODO: closed world and open world
real_world = connect(use_gui=True)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.5) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_interrupt()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_interrupt()
disconnect()