def sequential_from_temporal_plan(plan):
if plan is None:
return plan
over_actions = []
state_changes = [DurativeAction(None, [], 0, 0)]
for durative_action in plan:
args = durative_action.args
start, end = durative_action.start, get_end(durative_action)
start_action, over_action, end_action = [
SIMPLE_TEMPLATE.format(durative_action.name, i) for i in range(3)
]
state_changes.append(
DurativeAction(start_action, args, start, end - start))
#state_changes.append(DurativeAction(start_action, args, start, 0))
over_actions.append(
DurativeAction(over_action, args, start, end - start))
state_changes.append(DurativeAction(end_action, args, end, 0))
state_changes = sorted(state_changes, key=lambda a: a.start)
sequence = []
for i in range(1, len(state_changes)):
# Technically should check the state change points as well
start_action = state_changes[i - 1]
end_action = state_changes[i]
for over_action in over_actions:
if (over_action.start < end_action.start) and (
start_action.start < get_end(over_action)): # Exclusive
sequence.append(over_action)
sequence.append(end_action)
return sequence
def value_from_obj_plan(obj_plan):
if not is_plan(obj_plan):
return obj_plan
#return [(action,) + tuple(values_from_objects(args)) for action, args in obj_plan]
#return [(action, tuple(values_from_objects(args))) for action, args in obj_plan]
value_plan = []
for action in obj_plan:
if len(action) == 3:
name, inputs, outputs = action
new_inputs = params_from_objects(inputs) # values_from_objects
new_outputs = outputs
if isinstance(new_outputs, collections.Sequence):
new_outputs = params_from_objects(
new_outputs) # values_from_objects
new_action = (name, new_inputs, new_outputs)
elif isinstance(action, DurativeAction):
name, args, start, duration = action
name, index = name[:-2], int(name[-1])
if index != 0:
continue
new_action = DurativeAction(name,
tuple(map(param_from_object, args)),
start, duration)
else:
new_action = transform_action_args(
action, param_from_object) # values_from_objects
value_plan.append(new_action)
return value_plan
def retime_plan(plan, duration=1):
if plan is None:
return plan
return [
DurativeAction(name, args, i * duration, duration)
for i, (name, args) in enumerate(plan)
]
def value_from_obj_plan(obj_plan):
if not is_plan(obj_plan):
return obj_plan
value_plan = []
for action in obj_plan:
# TODO: I shouldn't need this decomposition any more, right?
if isinstance(action, StreamAction):
name, inputs, outputs = action
new_inputs = params_from_objects(inputs)
new_outputs = outputs
#if isinstance(new_outputs, collections.Sequence): # TODO: what was this for?
new_outputs = params_from_objects(new_outputs)
new_action = StreamAction(name, new_inputs, new_outputs)
elif isinstance(action, DurativeAction):
name, args, start, duration = action
name, index = name[:-2], int(name[-1])
if index != 0:
continue
new_action = DurativeAction(name,
tuple(map(param_from_object, args)),
start, duration)
elif isinstance(action, Action):
new_action = transform_action_args(
action, param_from_object) # values_from_objects
elif isinstance(action, Assignment):
new_action = transform_action_args(action, param_from_object)
else:
raise ValueError(action)
value_plan.append(new_action)
return value_plan
def apply_start(plan, new_start):
if not plan:
return plan
old_start = compute_start(plan)
delta_start = new_start - old_start
return [DurativeAction(name, args, start + delta_start, duration)
for name, args, start, duration in plan]
def reverse_plan(plan):
if plan is None:
return None
makespan = compute_duration(plan)
return [
DurativeAction(action.name, action.args, makespan - get_end(action),
action.duration) for action in plan
]
def temporal_from_sequential(action):
# TODO: clean this up
assert isinstance(action, DurativeAction)
name, args, start, duration = action
if name[-2] != '-':
return action
new_name, index = name[:-2], int(name[-1])
if index != 0: # Only keeps the start action
return None
return DurativeAction(new_name, args, start, duration)
def parse_temporal_solution(solution):
makespan = 0.0
plan = []
# TODO: this regex doesn't work for @
regex = r'(\d+.\d+):\s+\(\s*(\w+(?:\s\w+)*)\s*\)\s+\[(\d+.\d+)\]'
for start, action, duration in re.findall(regex, solution):
entries = action.lower().split(' ')
action = DurativeAction(entries[0], tuple(entries[1:]), float(start),
float(duration))
plan.append(action)
makespan = max(action.start + action.duration, makespan)
return plan, makespan
def transform_action_args(action, fn):
if isinstance(action, Action):
name, args = action
return Action(name, tuple(map(fn, args)))
elif isinstance(action, DurativeAction):
name, args, start, duration = action
return DurativeAction(name, tuple(map(fn, args)), start, duration)
elif isinstance(action, StreamAction):
name, inputs, outputs = action
return StreamAction(name, tuple(map(fn, inputs)),
tuple(map(fn, outputs)))
elif isinstance(action, Assignment):
args, = action
return Assignment(tuple(map(fn, args)))
raise NotImplementedError(action)
def transform_action_args(action, fn):
if isinstance(action, Action):
name, args = action
return Action(name, tuple(map(fn, args)))
elif isinstance(action, DurativeAction):
action = temporal_from_sequential(action)
if action is None:
return None
name, args, start, duration = action
return DurativeAction(name, tuple(map(fn, args)), start, duration)
elif isinstance(action, StreamAction):
name, inputs, outputs = action
return StreamAction(name, tuple(map(fn, inputs)),
tuple(map(fn, outputs)))
elif isinstance(action, FunctionAction):
name, inputs = action
return FunctionAction(name, tuple(map(fn, inputs)))
elif isinstance(action, Assignment):
args, = action
return Assignment(tuple(map(fn, args)))
raise NotImplementedError(action)
def transform_action_args(action, fn):
if isinstance(action, Action):
name, args = action
return Action(name, tuple(map(fn, args)))
elif isinstance(action, DurativeAction):
name, args, start, duration = action
#name, index = name[:-2], int(name[-1])
#if index != 0: # TODO: what was this for?
# return None
return DurativeAction(name, tuple(map(fn, args)), start, duration)
elif isinstance(action, StreamAction):
name, inputs, outputs = action
return StreamAction(name, tuple(map(fn, inputs)),
tuple(map(fn, outputs)))
elif isinstance(action, FunctionAction):
name, inputs = action
return FunctionAction(name, tuple(map(fn, inputs)))
elif isinstance(action, Assignment):
args, = action
return Assignment(tuple(map(fn, args)))
raise NotImplementedError(action)
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 transform_action_args(action, fn):
if isinstance(action, DurativeAction):
name, args, start, duration = action
return DurativeAction(name, tuple(map(fn, args)), start, duration)
name, args = action
return Action(name, tuple(map(fn, args)))