def main(focused=True, unit_costs=False):
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=unit_costs)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs)
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
print(evaluations)
colors = dict(zip(tamp_problem.initial.block_poses, COLORS))
viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial')
state = tamp_problem.initial
print(state)
draw_state(viewer, state, colors)
for action in plan:
user_input('Continue?')
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def solve_pddlstream(focused=True):
pddlstream_problem = get_problem()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=True)
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
problem_fn = holding_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
problem = problem_fn()
state_id = p.saveState()
#saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
#StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1,
# 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0},
# from_fn(get_base_motion_synth(problem, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
p.disconnect()
return
if viewer:
p.restoreState(state_id)
else:
disconnect()
connect(use_gui=True)
problem = problem_fn() # TODO: way of doing this without reloading?
user_input('Execute?')
commands = post_process(problem, plan)
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
def solve_pddlstream(focused=True):
problem_fn = get_problem1 # get_problem1 | get_problem2
pddlstream_problem = problem_fn()
print('Init:', pddlstream_problem.init)
print('Goal:', pddlstream_problem.goal)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def main(focused=True):
# TODO: maybe load problems as a domain explicitly
pddlstream_problem = pddlstream_from_belief()
_, _, _, _, init, goal = pddlstream_problem
print(sorted(init, key=lambda f: f[0]))
print(goal)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=False)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=False)
solution = solve_incremental(pddlstream_problem, unit_costs=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main(deterministic=False,
observable=False,
collisions=True,
focused=True,
factor=True):
# TODO: global search over the state
belief_problem = get_belief_problem(deterministic, observable)
pddlstream_problem = to_pddlstream(belief_problem, collisions)
pr = cProfile.Profile()
pr.enable()
planner = 'ff-wastar1'
if focused:
stream_info = {
'GE': StreamInfo(from_test(ge_fn), eager=False),
'prob-after-move':
StreamInfo(from_fn(get_opt_move_fn(factor=factor))),
'MoveCost': FunctionInfo(move_cost_fn),
'prob-after-look':
StreamInfo(from_fn(get_opt_obs_fn(factor=factor))),
'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0,
p_look_fn=0)),
}
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
planner=planner,
debug=False,
max_cost=0,
unit_costs=False,
max_time=30)
else:
solution = solve_incremental(pddlstream_problem,
planner=planner,
debug=True,
max_cost=MAX_COST,
unit_costs=False,
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, init = solution
print('Real cost:', float(cost) / SCALE_COST)
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())
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
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(focused=True, deterministic=False, unit_costs=False):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
#'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'plan-motion': StreamInfo(p_success=1), # bound_fn is None
#'trajcollision': StreamInfo(p_success=1), # bound_fn is None
#'cfree': StreamInfo(eager=True),
}
dynamic = [
StreamSynthesizer('cfree-motion', {
'plan-motion': 1,
'trajcollision': 0
},
gen_fn=from_fn(cfree_motion_fn)),
#StreamSynthesizer('optimize', {'sample-pose': 1, 'inverse-kinematics': 1,
# 'posecollision': 0, 'distance': 0},
# gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))),
]
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
action_info=action_info,
stream_info=stream_info,
synthesizers=dynamic,
max_time=10,
max_cost=INF,
debug=False,
effort_weight=None,
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
for i, action in enumerate(plan):
user_input('Continue?')
print(i, *action)
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
StreamSynthesizer(
'safe-free-motion', {
'plan-free-motion': 1,
'trajcollision': 0
}, from_fn(get_free_motion_synth(robot, movable, teleport))),
StreamSynthesizer(
'safe-holding-motion', {
'plan-holding-motion': 1,
'trajcollision': 0
}, from_fn(get_holding_motion_synth(robot, movable, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', stream_map.keys())
print(names)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
p.disconnect()
return
paths = []
for name, args in plan:
if name == 'place':
paths += args[-1].reverse().body_paths
elif name in ['move', 'move_free', 'move_holding', 'pick']:
paths += args[-1].body_paths
print(paths)
command = Command(paths)
if not viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if simulate:
command.control()
else:
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()