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 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(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?')
def stripstream(robot1,
obstacles,
node_points,
element_bodies,
ground_nodes,
dual=True,
serialize=False,
hierarchy=False,
**kwargs):
robots = mirror_robot(robot1, node_points) if dual else [robot1]
elements = set(element_bodies)
initial_confs = {
ROBOT_TEMPLATE.format(i): Conf(robot)
for i, robot in enumerate(robots)
}
saver = WorldSaver()
layer_from_n = compute_layer_from_vertex(elements, node_points,
ground_nodes)
#layer_from_n = cluster_vertices(elements, node_points, ground_nodes) # TODO: increase resolution for small structures
# TODO: compute directions from first, layer from second
max_layer = max(layer_from_n.values())
print('Max layer: {}'.format(max_layer))
data = {}
if serialize:
plan, certificate = solve_serialized(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
**kwargs)
else:
plan, certificate = solve_joint(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
**kwargs)
if plan is None:
return None, data
if hierarchy:
print(SEPARATOR)
static_facts = extract_static_facts(plan, certificate, initial_confs)
partial_orders = compute_total_orders(plan)
plan, certificate = solve_joint(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs=initial_confs,
can_print=False,
can_transit=True,
additional_init=static_facts,
additional_orders=partial_orders,
**kwargs)
if plan is None:
return None, data
if plan and not isinstance(plan[0], DurativeAction):
time_from_start = 0.
retimed_plan = []
for name, args in plan:
command = args[-1]
command.retime(start_time=time_from_start)
retimed_plan.append(
DurativeAction(name, args, time_from_start, command.duration))
time_from_start += command.duration
plan = retimed_plan
plan = reverse_plan(plan)
print('\nLength: {} | Makespan: {:.3f}'.format(len(plan),
compute_duration(plan)))
# TODO: retime using the TFD duration
# TODO: attempt to resolve once without any optimistic facts to see if a solution exists
# TODO: choose a better initial config
# TODO: decompose into layers hierarchically
#planned_elements = [args[2] for name, args, _, _ in sorted(plan, key=lambda a: get_end(a))] # TODO: remove approach
#if not check_plan(extrusion_path, planned_elements):
# return None, data
if has_gui():
saver.restore()
#label_nodes(node_points)
# commands = [action.args[-1] for action in reversed(plan) if action.name == 'print']
# trajectories = flatten_commands(commands)
# elements = recover_sequence(trajectories)
# draw_ordered(elements, node_points)
# wait_if_gui('Continue?')
#simulate_printing(node_points, trajectories)
#display_trajectories(node_points, ground_nodes, trajectories)
simulate_parallel(robots, plan)
return None, data
def solve_serialized(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
trajectories=[],
post_process=False,
collisions=True,
disable=False,
max_time=INF,
**kwargs):
start_time = time.time()
saver = WorldSaver()
elements = set(element_bodies)
elements_from_layers = compute_elements_from_layer(elements, layer_from_n)
layers = sorted(elements_from_layers.keys())
print('Layers:', layers)
full_plan = []
makespan = 0.
removed = set()
for layer in reversed(layers):
print(SEPARATOR)
print('Layer: {}'.format(layer))
saver.restore()
remaining = elements_from_layers[layer]
printed = elements - remaining - removed
draw_model(remaining, node_points, ground_nodes, color=GREEN)
draw_model(printed, node_points, ground_nodes, color=RED)
problem = get_pddlstream(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
printed=printed,
removed=removed,
return_home=False,
trajectories=trajectories,
collisions=collisions,
disable=disable,
**kwargs)
layer_plan, certificate = solve_pddlstream(problem,
node_points,
element_bodies,
max_time=max_time -
elapsed_time(start_time))
remove_all_debug()
if layer_plan is None:
return None
if post_process:
print(SEPARATOR)
# Allows the planner to continue to check collisions
problem.init[:] = certificate.all_facts
#static_facts = extract_static_facts(layer_plan, ...)
#problem.init.extend(('Order',) + pair for pair in compute_total_orders(layer_plan))
for fact in [('print', ), ('move', )]:
if fact in problem.init:
problem.init.remove(fact)
new_layer_plan, _ = solve_pddlstream(problem,
node_points,
element_bodies,
planner=POSTPROCESS_PLANNER,
max_time=max_time -
elapsed_time(start_time))
if (new_layer_plan
is not None) and (compute_duration(new_layer_plan) <
compute_duration(layer_plan)):
layer_plan = new_layer_plan
user_input('{:.3f}->{:.3f}'.format(
compute_duration(layer_plan),
compute_duration(new_layer_plan)))
# TODO: replan in a cost sensitive way
layer_plan = apply_start(layer_plan, makespan)
duration = compute_duration(layer_plan)
makespan += duration
print(
'\nLength: {} | Start: {:.3f} | End: {:.3f} | Duration: {:.3f} | Makespan: {:.3f}'
.format(len(layer_plan), compute_start(layer_plan),
compute_end(layer_plan), duration, makespan))
full_plan.extend(layer_plan)
removed.update(remaining)
print(SEPARATOR)
print_plan(full_plan)
return full_plan, None