def compute_distances(stream_plan):
stream_orders = get_partial_orders(stream_plan)
reversed_orders = {(s2, s1) for s1, s2 in stream_orders}
in_stream_orders, out_stream_orders = neighbors_from_orders(
reversed_orders)
sources = {
stream
for stream in stream_plan if not in_stream_orders[stream]
} # In the reversed DAG
output_sources = {
stream
for stream in sources if stream.external.has_outputs
}
test_sources = sources - output_sources
#visited = dijkstra(output_sources, reversed_orders)
#distances = {stream: node.g for stream, node in visited.items()}
distances = layer_sort(set(stream_plan) - test_sources, reversed_orders)
# TODO: take into account argument overlap
max_distance = max([0] + list(distances.values()))
for stream in stream_plan:
if stream not in distances:
distances[stream] = min(
[max_distance] +
[distances[s] - 1 for s in out_stream_orders[stream]])
#dump_layers(distances)
return distances
def combine_optimizers_greedy(evaluations, external_plan):
if not is_plan(external_plan):
return external_plan
# The key thing is that a variable must be grounded before it can used in a non-stream thing
# TODO: construct variables in order
# TODO: graph cut algorithm to minimize the number of constraints that are excluded
# TODO: reorder to ensure that constraints are done first since they are likely to fail as tests
incoming_edges, outgoing_edges = neighbors_from_orders(
get_partial_orders(external_plan))
queue = []
functions = []
for v in external_plan:
if not incoming_edges[v]:
(functions if isinstance(v, FunctionResult) else queue).append(v)
current = []
ordering = []
while queue:
optimizer = get_optimizer(current[-1]) if current else None
for v in queue:
if optimizer == get_optimizer(v):
current.append(v)
break
else:
ordering.extend(combine_optimizer_plan(current, functions))
current = [queue[0]]
v1 = current[-1]
queue.remove(v1)
for v2 in outgoing_edges[v1]:
incoming_edges[v2].remove(v1)
if not incoming_edges[v2]:
(functions
if isinstance(v2, FunctionResult) else queue).append(v2)
ordering.extend(combine_optimizer_plan(current, functions))
return ordering + functions
def dynamic_programming(store,
vertices,
valid_head_fn,
stats_fn,
prune=True,
greedy=False):
# 2^N rather than N!
# TODO: can just do on the infos themselves
dominates = lambda v1, v2: all(
s1 <= s2 for s1, s2 in zip(stats_fn(v1), stats_fn(v2)))
effort_orders = set()
if prune:
for i, v1 in enumerate(vertices):
for v2 in vertices[i + 1:]:
if dominates(v1, v2):
effort_orders.add((v1, v2)) # Includes equality
elif dominates(v2, v1):
effort_orders.add((v2, v1))
_, out_priority_orders = neighbors_from_orders(effort_orders)
priority_ordering = topological_sort(vertices, effort_orders)[::-1]
# TODO: could the greedy strategy lead to premature choices
# TODO: this starts to blow up
subset = frozenset()
queue = deque([subset]) # Acyclic because subsets
subproblems = {subset: Subproblem(0, None, None)}
while queue:
if store.is_terminated():
return vertices
subset = queue.popleft() # TODO: greedy version of this
applied = set()
for v in priority_ordering:
if greedy and applied:
break
if (v not in subset) and valid_head_fn(
v, subset) and not (out_priority_orders[v] & applied):
applied.add(v)
new_subset = frozenset([v]) | subset
p_success, overhead = stats_fn(v)
new_cost = overhead + p_success * subproblems[subset].cost
subproblem = Subproblem(
new_cost, v, subset) # Adds new element to the front
if new_subset not in subproblems:
queue.append(new_subset)
subproblems[new_subset] = subproblem
elif new_cost < subproblems[new_subset].cost:
subproblems[new_subset] = subproblem
ordering = []
subset = frozenset(vertices)
while True:
if subset not in subproblems:
print(vertices)
# TODO: some sort of bug where the problem isn't solved?
subproblem = subproblems[subset]
if subproblem.head is None:
break
ordering.append(subproblem.head)
subset = subproblem.subset
return ordering
def debug_elements(robot, node_points, node_order, elements):
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
for element in elements:
color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0,
0)
draw_element(node_points, element, color=color)
wait_for_interrupt('Continue?')
# TODO: topological sort
node = node_order[40]
node_neighbors = get_node_neighbors(elements)
for element in node_neighbors[node]:
color = (0, 1,
0) if element_supports(element, node, node_points) else (1, 0,
0)
draw_element(node_points, element, color)
element = elements[-1]
draw_element(node_points, element, (0, 1, 0))
incoming_edges, _ = neighbors_from_orders(
get_supported_orders(elements, node_points))
supporters = []
retrace_supporters(element, incoming_edges, supporters)
for e in supporters:
draw_element(node_points, e, (1, 0, 0))
wait_for_interrupt('Continue?')
def reorder_stream_plan(stream_plan, **kwargs):
if not is_plan(stream_plan):
return stream_plan
stream_orders = get_partial_orders(stream_plan)
in_stream_orders, out_stream_orders = neighbors_from_orders(stream_orders)
valid_combine = lambda v, subset: out_stream_orders[v] <= subset
#valid_combine = lambda v, subset: in_stream_orders[v] & subset
return dynamic_programming(stream_plan, valid_combine, get_stream_stats, **kwargs)
def convert_fluent_streams(stream_plan, real_states, action_plan,
step_from_fact, node_from_atom):
#return stream_plan
import pddl
assert len(real_states) == len(action_plan) + 1
steps_from_stream = get_steps_from_stream(stream_plan, step_from_fact,
node_from_atom)
# TODO: ensure that derived facts aren't in fluents?
# TODO: handle case where costs depend on the outputs
_, outgoing_edges = neighbors_from_orders(
get_partial_orders(stream_plan,
init_facts=map(
fact_from_fd,
filter(lambda f: isinstance(f, pddl.Atom),
real_states[0]))))
static_plan = []
fluent_plan = []
for result in stream_plan:
external = result.external
if isinstance(result, FunctionResult) or (result.opt_index != 0) or (
not external.is_fluent):
static_plan.append(result)
continue
if outgoing_edges[result]:
# No way of taking into account the binding of fluent inputs when preventing cycles
raise NotImplementedError(
'Fluent stream is required for another stream: {}'.format(
result))
#if (len(steps_from_stream[result]) != 1) and result.output_objects:
# raise NotImplementedError('Fluent stream required in multiple states: {}'.format(result))
for state_index in steps_from_stream[result]:
new_output_objects = [
#OptimisticObject.from_opt(out.value, object())
OptimisticObject.from_opt(
out.value, UniqueOptValue(result.instance, object(), name))
for name, out in safe_zip(result.external.outputs,
result.output_objects)
]
if new_output_objects and (state_index <= len(action_plan) - 1):
# TODO: check that the objects aren't used in any effects
instance = copy.copy(action_plan[state_index])
action_plan[state_index] = instance
output_mapping = get_mapping(
list(map(pddl_from_object, result.output_objects)),
list(map(pddl_from_object, new_output_objects)))
instance.var_mapping = {
p: output_mapping.get(v, v)
for p, v in instance.var_mapping.items()
}
new_instance = get_fluent_instance(external,
result.instance.input_objects,
real_states[state_index])
# TODO: handle optimistic here
new_result = new_instance.get_result(new_output_objects,
opt_index=result.opt_index)
fluent_plan.append(new_result)
return static_plan + fluent_plan
def get_future_p_successes(stream_plan):
# TODO: should I use this instead of p_success in some places?
# TODO: learn this instead. Can estimate conditional probabilities of certain sequences
orders = get_partial_orders(stream_plan)
incoming_edges, outgoing_edges = neighbors_from_orders(orders)
descendants_map = {}
for s1 in reversed(stream_plan):
descendants_map[s1] = s1.instance.get_p_success()
for s2 in outgoing_edges[s1]:
descendants_map[s1] *= descendants_map[s2]
return descendants_map
def reorder_combined_plan(evaluations, combined_plan, action_info, domain, **kwargs):
if not is_plan(combined_plan):
return combined_plan
stream_plan, action_plan = separate_plan(combined_plan)
orders = get_combined_orders(evaluations, stream_plan, action_plan, domain)
_, out_orders = neighbors_from_orders(orders)
valid_combine = lambda v, subset: out_orders[v] <= subset
def stats_fn(operator):
if isinstance(operator, Result):
return get_stream_stats(operator)
name, _ = operator
info = action_info[name]
return info.p_success, info.overhead
return dynamic_programming(combined_plan, valid_combine, stats_fn, **kwargs)
def get_synthetic_stream_plan(stream_plan, synthesizers):
# TODO: fix this implementation of this to be as follows:
# 1) Prune graph not related
# 2) Cluster
# 3) Try combinations of replacing on stream plan
if not is_plan(stream_plan) or (not synthesizers):
return stream_plan
orders = get_partial_orders(stream_plan)
for order in list(orders):
orders.add(order[::-1])
neighbors, _ = neighbors_from_orders(orders)
# TODO: what if many possibilities?
# TODO: cluster first and then plan using the macro and regular streams
processed = set()
new_stream_plan = []
for result in stream_plan: # Processing in order is important
if result in processed:
continue
processed.add(result)
# TODO: assert that it has at least one thing in it
for synthesizer in synthesizers:
# TODO: something could be an input and output of a cut...
if result.instance.external.name not in synthesizer.streams:
continue
# TODO: need to ensure all are covered I think?
# TODO: don't do if no streams within
cluster = expand_cluster(synthesizer, result, neighbors, processed)
counts = Counter(r.instance.external.name for r in cluster)
if not all(n <= counts[name]
for name, n in synthesizer.streams.items()):
continue
ordered_cluster = [r for r in stream_plan if r in cluster]
synthesizer_result = synthesizer.get_synth_stream(ordered_cluster)
if synthesizer_result is None:
continue
new_stream_plan.append(synthesizer_result)
new_stream_plan.extend(
filter(lambda s: isinstance(s, FunctionResult),
ordered_cluster))
break
else:
new_stream_plan.append(result)
return new_stream_plan
def get_connected_components(vertices, edges):
#return [vertices]
incoming, outgoing = neighbors_from_orders(edges)
clusters = []
processed = set()
for v0 in vertices:
if v0 in processed:
continue
processed.add(v0)
cluster = {v0}
queue = deque([v0])
while queue:
v1 = queue.popleft()
for v2 in (incoming[v1] | outgoing[v1]):
if v2 not in processed:
processed.add(v2)
cluster.add(v2)
queue.append(v2)
clusters.append([v for v in vertices if v in cluster])
return clusters
def opt_from_graph(names, orders, infos={}):
param_from_order = {order: PARAM_TEMPLATE.format(*order) for order in orders}
fact_from_order = {order: (PREDICATE, param_from_order[order]) for order in orders}
object_from_param = {param: parse_value(param) for param in param_from_order.values()}
incoming_from_edges, outgoing_from_edges = neighbors_from_orders(orders)
stream_plan = []
for i, n in enumerate(names):
#info = infos.get(n, StreamInfo(p_success=1, overhead=0, verbose=True))
inputs = [param_from_order[n2, n] for n2 in incoming_from_edges[n]]
outputs = [param_from_order[n, n2] for n2 in outgoing_from_edges[n]]
#gen = get_gen(outputs=outputs, p_success=info.p_success)
#gen = get_gen(infos[i], outputs=outputs)
stream = Stream(
name=n,
#gen_fn=DEBUG,
#gen_fn=from_gen(gen),
gen_fn=from_gen_fn(get_gen_fn(outputs=outputs, **infos[i])),
inputs=inputs,
domain=[fact_from_order[n2, n] for n2 in incoming_from_edges[n]],
fluents=[],
outputs=outputs,
certified=[fact_from_order[n, n2] for n2 in outgoing_from_edges[n]],
#info=info,
info=StreamInfo(),
)
# TODO: dump names
print()
print(stream)
input_objects = safe_apply_mapping(stream.inputs, object_from_param)
instance = stream.get_instance(input_objects)
print(instance)
output_objects = safe_apply_mapping(stream.outputs, object_from_param)
result = instance.get_result(output_objects)
print(result)
stream_plan.append(result)
opt_plan = OptPlan(action_plan=[], preimage_facts=[])
opt_solution = OptSolution(stream_plan, opt_plan, cost=1)
return opt_solution
def reorder_stream_plan(store, stream_plan, **kwargs):
if not is_plan(stream_plan):
return stream_plan
indices = range(len(stream_plan))
index_from_stream = dict(zip(stream_plan, indices))
stream_orders = get_partial_orders(stream_plan)
stream_orders = {(index_from_stream[s1], index_from_stream[s2])
for s1, s2 in stream_orders}
#nodes = stream_plan
nodes = indices
in_stream_orders, out_stream_orders = neighbors_from_orders(stream_orders)
valid_combine = lambda v, subset: out_stream_orders[v] <= subset
#valid_combine = lambda v, subset: in_stream_orders[v] & subset
#stats_fn = get_stream_stats
stats_fn = lambda idx: get_stream_stats(stream_plan[idx])
ordering = dynamic_programming(store, nodes, valid_combine, stats_fn,
**kwargs)
#gc.collect()
ordering = [stream_plan[index] for index in ordering]
return ordering
def optimal_reorder_stream_plan(store,
stream_plan,
stats_from_stream=None,
**kwargs):
if not stream_plan:
return stream_plan
if stats_from_stream is None:
stats_from_stream = compute_statistics(stream_plan)
# TODO: use the negative output (or overhead) as a bound
indices = range(len(stream_plan))
index_from_stream = dict(zip(stream_plan, indices))
stream_orders = get_partial_orders(stream_plan)
stream_orders = {(index_from_stream[s1], index_from_stream[s2])
for s1, s2 in stream_orders}
#nodes = stream_plan
nodes = indices # TODO: are indices actually much faster?
in_stream_orders, out_stream_orders = neighbors_from_orders(stream_orders)
valid_combine = lambda v, subset: out_stream_orders[v] <= subset
#valid_combine = lambda v, subset: in_stream_orders[v] & subset
# TODO: these are special because they don't enable any downstream access to another stream
#sources = {stream_plan[index] for index in indices if not in_stream_orders[index]}
#sinks = {stream_plan[index] for index in indices if not out_stream_orders[index]} # Contains collision checks
#print(dijkstra(sources, get_partial_orders(stream_plan)))
stats_fn = lambda idx: stats_from_stream[stream_plan[idx]]
#tiebreaker_fn = lambda *args: 0
#tiebreaker_fn = lambda *args: random.random() # TODO: introduces cycles
tiebreaker_fn = lambda idx: stream_plan[idx].stats_heuristic()
ordering = dynamic_programming(store,
nodes,
valid_combine,
stats_fn=stats_fn,
tiebreaker_fn=tiebreaker_fn,
**kwargs)
#import gc
#gc.collect()
return [stream_plan[index] for index in ordering]
def get_print_gen_fn(robot, fixed_obstacles, node_points, element_bodies,
ground_nodes):
max_attempts = 300 # 150 | 300
max_trajectories = 10
check_collisions = True
# 50 doesn't seem to be enough
movable_joints = get_movable_joints(robot)
disabled_collisions = get_disabled_collisions(robot)
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(
get_supported_orders(element_bodies, node_points))
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
# TODO: prioritize choices that don't collide with too many edges
def gen_fn(node1, element): # fluents=[]):
reverse = (node1 != element[0])
element_body = element_bodies[element]
n1, n2 = reversed(element) if reverse else element
delta = node_points[n2] - node_points[n1]
# if delta[2] < 0:
# continue
length = np.linalg.norm(delta) # 5cm
#supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)}
supporters = []
retrace_supporters(element, incoming_supporters, supporters)
elements_order = [
e for e in element_bodies
if (e != element) and (e not in supporters)
]
bodies_order = [element_bodies[e] for e in elements_order]
obstacles = fixed_obstacles + [element_bodies[e] for e in supporters]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits={}) # TODO: get_custom_limits
trajectories = []
for num in irange(max_trajectories):
for attempt in range(max_attempts):
path = sample_print_path(robot, length, reverse, element_body,
collision_fn)
if path is None:
continue
if check_collisions:
collisions = check_trajectory_collision(
robot, path, bodies_order)
colliding = {
e
for k, e in enumerate(elements_order)
if (element != e) and collisions[k]
}
else:
colliding = set()
if (node_neighbors[n1] <= colliding) and not any(
n in ground_nodes for n in element):
continue
print_traj = PrintTrajectory(robot, movable_joints, path,
element, reverse, colliding)
trajectories.append(print_traj)
# TODO: need to prune dominated trajectories
if print_traj not in trajectories:
continue
print('{}) {}->{} ({}) | {} | {} | {}'.format(
num, n1, n2, len(supporters), attempt, len(trajectories),
sorted(len(t.colliding) for t in trajectories)))
yield (print_traj, )
if not colliding:
return
else:
print('{}) {}->{} ({}) | {} | Max attempts exceeded!'.format(
num, len(supporters), n1, n2, max_attempts))
user_input('Continue?')
return
return gen_fn
def get_wild_move_gen_fn(robots,
static_obstacles,
element_bodies,
partial_orders=set(),
collisions=True,
**kwargs):
incoming_supporters, _ = neighbors_from_orders(partial_orders)
def wild_gen_fn(name, conf1, conf2, *args):
is_initial = (conf1.element is None) and (conf2.element is not None)
is_goal = (conf1.element is not None) and (conf2.element is None)
if is_initial:
supporters = []
elif is_goal:
supporters = list(element_bodies)
else:
supporters = [conf1.element
] # TODO: can also do according to levels
retrace_supporters(conf1.element, incoming_supporters, supporters)
element_obstacles = {element_bodies[e] for e in supporters}
obstacles = set(static_obstacles) | element_obstacles
if not collisions:
obstacles = set()
robot = index_from_name(robots, name)
conf1.assign()
joints = get_movable_joints(robot)
# TODO: break into pieces at the furthest part from the structure
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
#path = [conf1, conf2]
path = plan_joint_motion(robot,
joints,
conf2.positions,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=3,
iterations=100,
smooth=100)
if not path:
return
path = [conf1.positions] + path[1:-1] + [conf2.positions]
traj = MotionTrajectory(robot, joints, path)
command = Command([traj])
edges = [
(conf1, command, conf2),
(conf2, command, conf1), # TODO: reverse
]
outputs = []
#outputs = [(command,)]
facts = []
for q1, cmd, q2 in edges:
facts.extend([
('Traj', name, cmd),
('CTraj', name, cmd),
('MoveAction', name, q1, q2, cmd),
])
yield WildOutput(outputs, facts)
return wild_gen_fn
def dynamic_programming(store,
vertices,
valid_head_fn,
stats_fn=Performance.get_statistics,
prune=True,
greedy=False,
**kwargs):
# TODO: include context here as a weak constraint
# TODO: works in the absence of partial orders
# TODO: can also more manually reorder
# 2^N rather than N!
start_time = time.time()
effort_orders = set() # 1 cheaper than 2
if prune:
effort_orders.update(
compute_pruning_orders(vertices, stats_fn=stats_fn, **kwargs))
_, out_priority_orders = neighbors_from_orders(
effort_orders) # more expensive
priority_ordering = topological_sort(
vertices, effort_orders)[::-1] # most expensive to cheapest
# TODO: can break ties with index on action plan to prioritize doing the temporally first things
# TODO: could the greedy strategy lead to premature choices
# TODO: this starts to blow up - group together similar streams (e.g. collision streams) to decrease size
# TODO: key grouping concern are partial orders and ensuring feasibility (isomorphism)
# TODO: flood-fill cheapest as soon as something that has no future dependencies has been found
# TODO: do the forward version to take advantage of sink vertices
subset = frozenset()
queue = deque([subset]) # Acyclic because subsets
subproblems = {subset: Subproblem(cost=0, head=None, subset=None)}
while queue: # searches backward from last to first
if store.is_terminated():
return vertices
subset = queue.popleft(
) # TODO: greedy/weighted A* version of this (heuristic is next cheapest stream)
applied = set()
# TODO: roll-out more than one step to cut the horizon
# TODO: compute a heuristic that's the best case affordances from subsequent streams
for v in priority_ordering: # most expensive first
if greedy and applied:
break
if (v not in subset) and valid_head_fn(
v, subset) and not (out_priority_orders[v] & applied):
applied.add(v)
new_subset = frozenset([v]) | subset
p_success, overhead = stats_fn(v)
new_cost = overhead + p_success * subproblems[subset].cost
subproblem = Subproblem(
cost=new_cost, head=v,
subset=subset) # Adds new element to the front
if new_subset not in subproblems:
queue.append(new_subset)
subproblems[new_subset] = subproblem
elif new_cost < subproblems[new_subset].cost:
subproblems[new_subset] = subproblem
ordering = []
subset = frozenset(vertices)
while True:
if subset not in subproblems:
print(vertices)
# TODO: some sort of bug where the problem isn't solved?
subproblem = subproblems[subset]
if subproblem.head is None:
break
ordering.append(subproblem.head)
subset = subproblem.subset
#print('Streams: {} | Expected cost: {:.3f} | Time: {:.3f}'.format(
# len(ordering), compute_expected_cost(ordering, stats_fn=stats_fn), elapsed_time(start_time)))
return ordering
def get_print_gen_fn(robot, obstacles, node_points, element_bodies,
ground_nodes):
max_attempts = 150
max_trajectories = 10
check_collisions = True
# 50 doesn't seem to be enough
movable_joints = get_movable_joints(robot)
disabled_collisions = {
tuple(link_from_name(robot, link) for link in pair)
for pair in DISABLED_COLLISIONS
}
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(
get_supported_orders(element_bodies, node_points))
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
def gen_fn(node1, element, fluents=[]):
# TODO: sample collisions while making the trajectory
reverse = (node1 != element[0])
element_body = element_bodies[element]
n1, n2 = reversed(element) if reverse else element
delta = node_points[n2] - node_points[n1]
# if delta[2] < 0:
# continue
length = np.linalg.norm(delta) # 5cm
#supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)}
supporters = []
retrace_supporters(element, incoming_supporters, supporters)
elements_order = [
e for e in element_bodies
if (e != element) and (e not in supporters)
]
bodies_order = [element_bodies[e] for e in elements_order]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles + [element_bodies[e] for e in supporters],
attachments=[],
self_collisions=True,
disabled_collisions=disabled_collisions,
use_limits=True)
trajectories = []
for num in irange(max_trajectories):
for attempt in range(max_attempts):
path = sample_print_path(robot, length, reverse, element_body,
collision_fn)
if path is None:
continue
if check_collisions:
collisions = check_trajectory_collision(
robot, path, bodies_order)
colliding = {
e
for k, e in enumerate(elements_order)
if (element != e) and collisions[k]
}
else:
colliding = set()
if (node_neighbors[n1] <= colliding) and not any(
n in ground_nodes for n in element):
continue
print_traj = PrintTrajectory(robot, movable_joints, path,
element, reverse, colliding)
trajectories.append(print_traj)
if print_traj not in trajectories:
continue
print('{}) {}->{} ({}) | {} | {} | {}'.format(
num, n1, n2, len(supporters), attempt, len(trajectories),
sorted(len(t.colliding) for t in trajectories)))
yield print_traj,
if not colliding:
return
else:
print('{}) {}->{} ({}) | {} | Failure!'.format(
num, len(supporters), n1, n2, max_attempts))
break
return gen_fn
def get_print_gen_fn(robot,
fixed_obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
partial_orders=set(),
removed=set(),
collisions=True,
disable=False,
ee_only=False,
allow_failures=False,
p_nearby=0.,
max_directions=MAX_DIRECTIONS,
max_attempts=MAX_ATTEMPTS,
max_time=INF,
**kwargs):
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
if not collisions:
precompute_collisions = False
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(partial_orders)
initial_conf = get_configuration(robot)
end_effector = EndEffector(robot,
ee_link=link_from_name(robot, EE_LINK),
tool_link=link_from_name(robot, TOOL_LINK))
extrusions = {
reverse_element(element) if reverse else element:
Extrusion(end_effector, element_bodies, node_points, ground_nodes,
element, reverse)
for element in element_bodies for reverse in [False, True]
}
def gen_fn(node1, element, extruded=[], trajectories=[]): # fluents=[]):
start_time = time.time()
assert not is_reversed(element_bodies, element)
idle_time = 0
reverse = is_end(node1, element)
if disable or (len(extruded) < SKIP_PERCENTAGE *
len(element_bodies)): # For quick visualization
path, tool_path = [], []
traj = PrintTrajectory(end_effector, get_movable_joints(robot),
path, tool_path, element, reverse)
command = Command([traj])
yield (command, )
return
directed = reverse_element(element) if reverse else element
extrusion = extrusions[directed]
n1, n2 = reverse_element(element) if reverse else element
neighboring_elements = node_neighbors[n1] & node_neighbors[n2]
supporters = [] # TODO: can also do according to levels
retrace_supporters(element, incoming_supporters, supporters)
element_obstacles = {
element_bodies[e]
for e in supporters + list(extruded)
}
obstacles = set(fixed_obstacles) | element_obstacles
if not collisions:
obstacles = set()
#obstacles = set(fixed_obstacles)
elements_order = [
e for e in element_bodies if (e != element) and (
e not in removed) and (element_bodies[e] not in obstacles)
]
collision_fn = get_element_collision_fn(robot, obstacles)
#print(len(fixed_obstacles), len(element_obstacles), len(elements_order))
trajectories = list(trajectories)
for num in irange(INF):
for attempt, tool_traj in enumerate(
islice(extrusion.generator(), max_directions)):
# TODO: is this slower now for some reason?
#tool_traj.safe_from_body = {} # Disables caching
if not tool_traj.is_safe(obstacles):
continue
for _ in range(max_attempts):
if max_time <= elapsed_time(start_time):
return
nearby_conf = initial_conf if random.random(
) < p_nearby else None
command = compute_direction_path(tool_traj,
collision_fn,
ee_only=ee_only,
initial_conf=nearby_conf,
**kwargs)
if command is None:
continue
command.update_safe(extruded)
if precompute_collisions:
bodies_order = [
element_bodies[e] for e in elements_order
]
colliding = command_collision(end_effector, command,
bodies_order)
for element2, unsafe in zip(elements_order, colliding):
if unsafe:
command.set_unsafe(element2)
else:
command.set_safe(element2)
if not is_ground(element, ground_nodes) and (
neighboring_elements <= command.colliding):
continue # If all neighbors collide
trajectories.append(command)
if precompute_collisions:
prune_dominated(trajectories)
if command not in trajectories:
continue
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Trajectories: {} | Colliding: {}'
.format(num, n1, n2, ee_only, len(supporters), attempt,
len(trajectories),
sorted(len(t.colliding)
for t in trajectories)))
temp_time = time.time()
yield (command, )
idle_time += elapsed_time(temp_time)
if precompute_collisions:
if len(command.colliding) == 0:
#print('Reevaluated already non-colliding trajectory!')
return
elif len(command.colliding) == 1:
[colliding_element] = command.colliding
obstacles.add(element_bodies[colliding_element])
break
else:
if allow_failures:
yield None
else:
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Max attempts exceeded!'
.format(num, n1, n2, ee_only, len(supporters),
max_directions))
return
#yield None
return gen_fn
def convert_fluent_streams(stream_plan, real_states, action_plan,
step_from_fact, node_from_atom):
import pddl
assert len(real_states) == len(action_plan) + 1
steps_from_stream = {}
for result in reversed(stream_plan):
steps_from_stream[result] = set()
for fact in result.get_certified():
if (fact in step_from_fact) and (node_from_atom[fact].result
== result):
steps_from_stream[result].update(step_from_fact[fact])
for fact in result.instance.get_domain():
step_from_fact[fact] = step_from_fact.get(
fact, set()) | steps_from_stream[result]
# TODO: apply this recursively
# TODO: ensure that derived facts aren't in fluents?
# TODO: handle case where costs depend on the outputs
_, outgoing_edges = neighbors_from_orders(
get_partial_orders(stream_plan,
init_facts=map(
fact_from_fd,
filter(lambda f: isinstance(f, pddl.Atom),
real_states[0]))))
static_plan = []
fluent_plan = []
for result in stream_plan:
external = result.external
if (result.opt_index != 0) or (not external.is_fluent()):
static_plan.append(result)
continue
if outgoing_edges[result]:
# No way of taking into account the binding of fluent inputs when preventing cycles
raise NotImplementedError(
'Fluent stream is required for another stream: {}'.format(
result))
#if (len(steps_from_stream[result]) != 1) and result.output_objects:
# raise NotImplementedError('Fluent stream required in multiple states: {}'.format(result))
for state_index in steps_from_stream[result]:
new_output_objects = [ # OptimisticObject.from_opt(out.value, object())
OptimisticObject.from_opt(
out.value, UniqueOptValue(result.instance, object(), i))
for i, out in enumerate(result.output_objects)
]
if new_output_objects and (state_index < len(action_plan)):
# TODO: check that the objects aren't used in any effects
instance = copy.copy(action_plan[state_index])
action_plan[state_index] = instance
output_mapping = get_mapping(
map(pddl_from_object, result.output_objects),
map(pddl_from_object, new_output_objects))
instance.var_mapping = {
p: output_mapping.get(v, v)
for p, v in instance.var_mapping.items()
}
fluent_facts = list(
map(
fact_from_fd,
filter(
lambda f: isinstance(f, pddl.Atom) and
(f.predicate in external.fluents),
real_states[state_index])))
new_instance = external.get_instance(result.instance.input_objects,
fluent_facts=fluent_facts)
new_result = new_instance.get_result(new_output_objects,
opt_index=result.opt_index)
fluent_plan.append(new_result)
return static_plan + fluent_plan
def add_plan_constraints(constraints,
domain,
evaluations,
goal_exp,
internal=False):
if (constraints is None) or (constraints.skeletons is None):
return goal_exp
import pddl
# TODO: unify this with the constraint ordering
# TODO: can constrain to use a plan prefix
prefix = get_internal_prefix(internal)
assigned_predicate = ASSIGNED_PREDICATE.format(prefix)
bound_predicate = BOUND_PREDICATE.format(prefix)
group_predicate = GROUP_PREDICATE.format(prefix)
order_predicate = ORDER_PREDICATE.format(prefix)
new_facts = []
for group in constraints.groups:
for value in constraints.groups[group]:
# TODO: could make all constants groups (like an equality group)
fact = (group_predicate, to_obj(group), to_obj(value))
new_facts.append(fact)
new_actions = []
new_goals = []
for num, skeleton in enumerate(constraints.skeletons):
actions, orders = skeleton
incoming_orders, _ = neighbors_from_orders(orders)
order_facts = [(order_predicate, to_obj('n{}'.format(num)),
to_obj('t{}'.format(step)))
for step in range(len(actions))]
for step, (name, args) in enumerate(actions):
# TODO: could also just remove the free parameter from the action
new_action = deepcopy(
find_unique(lambda a: a.name == name, domain.actions))
local_from_global = {
a: p.name
for a, p in safe_zip(args, new_action.parameters)
if is_parameter(a)
}
ancestors, descendants = get_ancestors(step,
orders), get_descendants(
step, orders)
parallel = set(range(
len(actions))) - ancestors - descendants - {step}
parameters = set(filter(is_parameter, args))
ancestor_parameters = parameters & set(
filter(is_parameter,
(p for idx in ancestors for p in actions[idx][1])))
#descendant_parameters = parameters & set(filter(is_parameter, (p for idx in descendants for p in actions[idx][1])))
parallel_parameters = parameters & set(
filter(is_parameter,
(p for idx in parallel for p in actions[idx][1])))
#bound_preconditions = [Imply(bound, assigned) for bound, assigned in safe_zip(bound_facts, assigned_facts)]
bound_condition = pddl.Conjunction([
pddl.Disjunction(
map(fd_from_fact, [
Not((bound_predicate, to_constant(p))),
(assigned_predicate, to_constant(p),
local_from_global[p])
])) for p in parallel_parameters
])
existing_preconditions = [(assigned_predicate, to_constant(p),
local_from_global[p])
for p in ancestor_parameters]
constant_pairs = [(a, p.name)
for a, p in safe_zip(args, new_action.parameters)
if is_constant(a)]
group_preconditions = [
(group_predicate if is_hashable(a) and
(a in constraints.groups) else EQ, to_obj(a), p)
for a, p in constant_pairs
]
order_preconditions = [
order_facts[idx] for idx in incoming_orders[step]
]
new_preconditions = existing_preconditions + group_preconditions + order_preconditions + [
Not(order_facts[step])
]
new_action.precondition = pddl.Conjunction([
new_action.precondition, bound_condition,
make_preconditions(new_preconditions)
]).simplified()
new_parameters = parameters - ancestors
bound_facts = [(bound_predicate, to_constant(p))
for p in new_parameters]
assigned_facts = [(assigned_predicate, to_constant(p),
local_from_global[p]) for p in new_parameters]
new_effects = bound_facts + assigned_facts + [order_facts[step]]
new_action.effects.extend(make_effects(new_effects))
# TODO: should also negate the effects of all other sequences here
new_actions.append(new_action)
#new_action.dump()
new_goals.append(
And(*[order_facts[idx] for idx in incoming_orders[GOAL_INDEX]]))
add_predicate(domain, make_predicate(order_predicate, ['?num', '?step']))
if constraints.exact:
domain.actions[:] = []
domain.actions.extend(new_actions)
new_goal_exp = And(goal_exp, Or(*new_goals))
for fact in new_facts:
add_fact(evaluations, fact, result=INTERNAL_EVALUATION)
return new_goal_exp
