def main(focused=False, deterministic=False, unit_costs=True):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
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),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
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,
verbose=False)
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, args) in enumerate(plan):
user_input('Continue?')
print(i, action, args)
s2 = args[-1]
state = TAMPState(
s2[R], s2[H],
{b: s2[b]
for b in state.block_poses if s2[b] is not None})
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def apply_action(state, action):
conf, holding, block_poses = state
# TODO: don't mutate block_poses?
name, args = action
if name == 'move':
_, traj, _ = args
for conf in traj[1:]:
yield TAMPState(conf, holding, block_poses)
elif name == 'pick':
holding, _, _ = args
del block_poses[holding]
yield TAMPState(conf, holding, block_poses)
elif name == 'place':
block, pose, _ = args
holding = None
block_poses[block] = pose
yield TAMPState(conf, holding, block_poses)
else:
raise ValueError(name)
def apply_action(state, action):
conf, holding, block_poses = state
# TODO: don't mutate block_poses?
name, args = action
if name == 'move':
_, _, conf = args
elif name == 'pick':
holding, _, _ = args
del block_poses[holding]
elif name == 'place':
block, pose, _ = args
holding = None
block_poses[block] = pose
else:
raise ValueError(name)
return TAMPState(conf, holding, block_poses)