def display_plan(tamp_problem,
plan,
display=True,
save=False,
time_step=0.025,
sec_per_step=1e-3):
from examples.continuous_tamp.viewer import ContinuousTMPViewer
from examples.discrete_tamp.viewer import COLORS
example_name = os.path.basename(os.path.dirname(__file__))
directory = os.path.join(VISUALIZATIONS_DIR, example_name + '/')
safe_rm_dir(directory)
ensure_dir(directory)
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(SUCTION_HEIGHT,
tamp_problem.regions,
title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
duration = compute_duration(plan)
real_time = None if sec_per_step is None else (duration * sec_per_step /
time_step)
print('Duration: {} | Step size: {} | Real time: {}'.format(
duration, time_step, real_time))
draw_state(viewer, state, colors)
if display:
user_input('Start?')
if plan is not None:
for t in inclusive_range(0, duration, time_step):
for action in plan:
if action.start <= t <= get_end(action):
update_state(state, action, t - action.start)
print('t={} | {}'.format(t, state))
draw_state(viewer, state, colors)
if save:
viewer.save(os.path.join(directory, 't={}'.format(t)))
if display:
if sec_per_step is None:
user_input('Continue?')
else:
time.sleep(sec_per_step)
if display:
user_input('Finish?')
return state
def step_plan(tamp_problem, plan):
if plan is None:
return
# TODO: could also use the old version of this
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
user_input('Start?')
for i, action in enumerate(plan):
name, args = action
print(i, name, args)
for state in apply_action(state, action):
print(state)
draw_state(viewer, state, colors)
user_input('Continue?')
user_input('Finish?')
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm')
parser.add_argument('-s', '--skeleton', action='store_true', help='Enforces skeleton plan constraints')
tamp_problem, args = initialize(parser)
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP')
draw_state(viewer, tamp_problem.initial, colors)
[robot] = tamp_problem.initial.robot_confs
blocks = sorted(tamp_problem.initial.block_poses)
regions = sorted(set(tamp_problem.regions) - {GROUND_NAME})
print(robot)
print(blocks)
print(regions)
print(tamp_problem)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = DEBUG # TODO: reuse same streams
stream_info = {
't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None
}
streams_from_problem = {}
for order in permutations(blocks):
for region in regions: # Objects could also have separate goals
print(SEPARATOR)
print('Block order:', order)
print('Goal region:', region)
#tamp_problem = dual(n_blocks=args.number, goal_regions=[region])
tamp_problem.goal_regions.update({block: region for block in order})
init, goal = create_problem(tamp_problem)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
dump_pddlstream(problem)
skeleton = create_skeleton(robot, order, home=tamp_problem.goal_conf is not None)
print(skeleton)
skeletons = [skeleton]
#skeletons = None
constraints = PlanConstraints(skeletons=skeletons, exact=True)
solution = solve_focused(problem, constraints=constraints, stream_info=stream_info,
planner='max-astar', max_planner_time=10, debug=False,
max_time=args.max_time, verbose=True,
unit_costs=True, unit_efforts=True, effort_weight=1)
print_solution(solution)
plan, cost, evaluations = solution
assert plan is not None
# TODO: params not sufficient if no output stream or output not in plan
streams = set()
for name, params in plan:
for param in params:
value = get_stream_value(param)
if isinstance(value, StreamValue):
# TODO: propagate recursively
streams.add((value.stream, value.inputs))
streams_from_problem[order, region] = streams
#print(streams)
#user_input()
print(SEPARATOR)
dump_statistics()
print(SEPARATOR)
best_problems, best_p = None, -INF
for problems in combinations(streams_from_problem, r=2): # Make r a parameter
stream_plans = [streams_from_problem[problem] for problem in problems]
intersection = set.intersection(*stream_plans)
p = p_disjunction(stream_plans, STATS_PER_STREAM)
assert 0 <= p <= 1
print('Problems: {} | Intersection: {} | p={:.3f}'.format(problems, len(intersection), p)) #, intersection)
if p > best_p:
best_problems, best_p = problems, p
print('\nBest: {} (p={:.3f})'.format(best_problems, best_p))
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?')