def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005):
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))
colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots}
for i, t in enumerate(inclusive_range(0, duration, time_step)):
print('Step={} | t={}'.format(i, t))
for action in plan:
name, args, start, duration = action
if not (action.start <= t <= get_end(action)):
continue
if name == 'move':
robot, conf1, traj, conf2 = args
traj = [conf1.values, conf2.values]
fraction = (t - action.start) / action.duration
conf = get_value_at_time(traj, fraction)
body = problem.get_body(robot)
color = COLOR_FROM_NAME[robot]
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
set_base_conf(body, conf)
elif name == 'recharge':
robot, conf = args
body = problem.get_body(robot)
color = YELLOW
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
else:
raise ValueError(name)
if sec_per_step is None:
wait_if_gui('Continue?')
else:
wait_for_duration(sec_per_step)
def display_plan(tamp_problem,
plan,
display=True,
time_step=0.01,
sec_per_step=0.002):
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 action in plan:
# i = action.start
# print(action)
# for j, state in enumerate(apply_action(state, action)):
# print(i, j, state)
# draw_state(viewer, state, colors)
# viewer.save(os.path.join(directory, '{}_{}'.format(i, j)))
# if display:
# if sec_per_step is None:
# user_input('Continue?')
# else:
# time.sleep(sec_per_step)
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)
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?')