def simulate_parallel(robots,
plan,
time_step=0.1,
speed_up=10.,
record=None): # None | video.mp4
# TODO: ensure the step size is appropriate
makespan = compute_duration(plan)
print('\nMakespan: {:.3f}'.format(makespan))
trajectories = extract_parallel_trajectories(plan)
if trajectories is None:
return
wait_if_gui('Begin?')
num_motion = sum(action.name == 'move' for action in plan)
with VideoSaver(record):
for t in inclusive_range(0, makespan, time_step):
# if action.start <= t <= get_end(action):
executing = Counter(traj.robot for traj in trajectories
if traj.at(t) is not None)
print('t={:.3f}/{:.3f} | executing={}'.format(
t, makespan, executing))
for robot in robots:
num = executing.get(robot, 0)
if 2 <= num:
raise RuntimeError(
'Robot {} simultaneously executing {} trajectories'.
format(robot, num))
if (num_motion == 0) and (num == 0):
set_configuration(robot, DUAL_CONF)
#step_simulation()
wait_for_duration(time_step / speed_up)
wait_if_gui('Finish?')
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?')
def simulate_parallel(robots,
plan,
time_step=0.1,
speed_up=10.,
record=None): # None | video.mp4
# TODO: ensure the step size is appropriate
makespan = compute_duration(plan)
print('\nMakespan: {:.3f}'.format(makespan))
if plan is None:
return
trajectories = []
for action in plan:
command = action.args[-1]
if (action.name == 'move') and (command.start_conf is
action.args[-2].positions):
command = command.reverse()
command.retime(start_time=action.start)
#print(action)
#print(action.start, get_end(action), action.duration)
#print(command.start_time, command.end_time, command.duration)
#for traj in command.trajectories:
# print(traj, traj.start_time, traj.end_time, traj.duration)
trajectories.extend(command.trajectories)
#print(sum(traj.duration for traj in trajectories))
num_motion = sum(action.name == 'move' for action in plan)
wait_if_gui('Begin?')
with VideoSaver(record):
for t in inclusive_range(0, makespan, time_step):
# if action.start <= t <= get_end(action):
executing = Counter(traj.robot for traj in trajectories
if traj.at(t) is not None)
print('t={:.3f}/{:.3f} | executing={}'.format(
t, makespan, executing))
for robot in robots:
num = executing.get(robot, 0)
if 2 <= num:
raise RuntimeError(
'Robot {} simultaneously executing {} trajectories'.
format(robot, num))
if (num_motion == 0) and (num == 0):
set_configuration(robot, DUAL_CONF)
#step_simulation()
wait_for_duration(time_step / speed_up)
wait_if_gui('Finish?')