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