def add_successors(queue, all_elements, node_points, ground_nodes, heuristic_fn, printed, position, conf, partial_orders=[], visualize=False): incoming_from_element = incoming_from_edges(partial_orders) remaining = all_elements - printed num_remaining = len(remaining) - 1 #assert 0 <= num_remaining #bias_from_element = {} # TODO: print ground first for directed in randomize( compute_printable_directed(all_elements, ground_nodes, printed)): element = get_undirected(all_elements, directed) if not (incoming_from_element[element] <= printed): continue bias = heuristic_fn(printed, directed, position, conf) priority = (num_remaining, bias, random.random()) visits = 0 heapq.heappush(queue, (visits, priority, printed, directed, conf))
def regression(robot, obstacles, element_bodies, extrusion_path, partial_orders=[], heuristic='z', max_time=INF, max_memory=INF, backtrack_limit=INF, revisit=False, stiffness_attempts=1, collisions=True, stiffness=True, motions=True, lazy=LAZY, checker=None, **kwargs): # Focused has the benefit of reusing prior work # Greedy has the benefit of conditioning on previous choices # TODO: max branching factor # TODO: be more careful when near the end # TODO: max time spent evaluating successors (less expensive when few left) # TODO: tree rollouts # TODO: best-first search with a minimizing path distance cost # TODO: immediately select if becomes more stable # TODO: focus branching factor on most stable regions start_time = time.time() initial_conf = get_configuration(robot) initial_position = get_tool_position(robot) element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path) id_from_element = get_id_from_element(element_from_id) all_elements = frozenset(element_bodies) ground_elements = get_ground_elements(all_elements, ground_nodes) if stiffness and (checker is None): checker = create_stiffness_checker(extrusion_path, verbose=False) print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes, precompute_collisions=False, max_directions=MAX_DIRECTIONS, max_attempts=MAX_ATTEMPTS, collisions=collisions, **kwargs) heuristic_fn = get_heuristic_fn(robot, extrusion_path, heuristic, checker=checker, forward=False) queue = [] outgoing_from_element = outgoing_from_edges(partial_orders) def add_successors(printed, position, conf): only_ground = printed <= ground_elements num_remaining = len(printed) - 1 #assert 0 <= num_remaining for element in randomize(printed): if not (outgoing_from_element[element] & printed) and implies( is_ground(element, ground_nodes), only_ground): for directed in get_directions(element): visits = 0 bias = heuristic_fn(printed, directed, position, conf) priority = (num_remaining, bias, random.random()) heapq.heappush(queue, (visits, priority, printed, directed, conf)) final_conf = initial_conf # TODO: allow choice of final config final_position = initial_position final_printed = all_elements visited = {final_printed: Node(None, None)} if check_connected(ground_nodes, final_printed) and \ (not stiffness or test_stiffness(extrusion_path, element_from_id, final_printed, checker=checker)): add_successors(final_printed, final_position, final_conf) # if has_gui(): # sequence = sorted(final_printed, key=lambda e: heuristic_fn(final_printed, e, conf=None), reverse=True) # remove_all_debug() # draw_ordered(sequence, node_points) # wait_for_user() plan = None min_remaining = len(all_elements) num_evaluated = max_backtrack = extrusion_failures = transit_failures = stiffness_failures = 0 while queue and (elapsed_time(start_time) < max_time) and check_memory(): #max_memory): visits, priority, printed, directed, current_conf = heapq.heappop( queue) element = get_undirected(all_elements, directed) num_remaining = len(printed) backtrack = num_remaining - min_remaining max_backtrack = max(max_backtrack, backtrack) if backtrack_limit < backtrack: break # continue num_evaluated += 1 print( 'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}' .format(num_evaluated, min_remaining, len(printed), element, id_from_element[element], elapsed_time(start_time))) next_printed = printed - {element} next_nodes = compute_printed_nodes(ground_nodes, next_printed) #draw_action(node_points, next_printed, element) #if 3 < backtrack + 1: # remove_all_debug() # set_renderer(enable=True) # draw_model(next_printed, node_points, ground_nodes) # wait_for_user() node1, node2 = directed if (next_printed in visited) or (node1 not in next_nodes) or not check_connected( ground_nodes, next_printed): continue # TODO: stiffness plan lazily here possibly with reuse if stiffness and not test_stiffness( extrusion_path, element_from_id, next_printed, checker=checker): stiffness_failures += 1 continue # TODO: stronger condition for this procedure # if plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, next_printed, # checker=checker, max_backtrack=0) is None: # # TODO: cache and reuse prior stiffness plans # print('Failed stiffness plan') # TODO: require just a short horizon # continue if revisit: heapq.heappush( queue, (visits + 1, priority, printed, directed, current_conf)) command, = next(print_gen_fn(node1, element, extruded=next_printed), (None, )) if command is None: extrusion_failures += 1 continue if motions and not lazy: motion_traj = compute_motion( robot, obstacles, element_bodies, printed, command.end_conf, current_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) # TODO: smooth=...) if motion_traj is None: transit_failures += 1 continue command.trajectories.append(motion_traj) if num_remaining < min_remaining: min_remaining = num_remaining #print('New best: {}'.format(num_remaining)) #if has_gui(): # # TODO: change link transparency # remove_all_debug() # draw_model(next_printed, node_points, ground_nodes) # wait_for_duration(0.5) visited[next_printed] = Node( command, printed) # TODO: be careful when multiple trajs if not next_printed: min_remaining = 0 commands = retrace_commands(visited, next_printed, reverse=True) if OPTIMIZE: commands = optimize_commands(robot, obstacles, element_bodies, extrusion_path, initial_conf, commands, motions=motions, collisions=collisions) plan = flatten_commands(commands) if motions and not lazy: motion_traj = compute_motion(robot, obstacles, element_bodies, frozenset(), initial_conf, plan[0].start_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) if motion_traj is None: plan = None transit_failures += 1 else: plan.insert(0, motion_traj) if motions and lazy: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan, collisions=collisions, max_time=max_time - elapsed_time(start_time)) break # if plan is not None: # break add_successors(next_printed, node_points[node1], command.start_conf) #del checker data = { #'memory': get_memory_in_kb(), # May need to update instead 'num_evaluated': num_evaluated, 'min_remaining': min_remaining, 'max_backtrack': max_backtrack, 'stiffness_failures': stiffness_failures, 'extrusion_failures': extrusion_failures, 'transit_failures': transit_failures, } return plan, data
def fn(printed, directed, position, conf): # Queue minimizes the statistic element = get_undirected(all_elements, directed) n1, n2 = directed # forward adds, backward removes structure = printed | {element} if forward else printed - {element} structure_ids = get_extructed_ids(element_from_id, structure) normalizer = 1 #normalizer = len(structure) #normalizer = compute_element_distance(node_points, all_elements) reduce_op = sum # sum | max | average reaction_fn = force_from_reaction # force_from_reaction | torque_from_reaction first_node, second_node = directed if forward else reverse_element(directed) layer = sign * layer_from_edge[element] #layer = sign * layer_from_directed.get(directed, INF) tool_point = position tool_distance = 0. if heuristic in COST_HEURISTICS: if conf is not None: if hash_or_id(conf) not in ee_cache: with BodySaver(robot): set_configuration(robot, conf) ee_cache[hash_or_id(conf)] = get_tool_position(robot) tool_point = ee_cache[hash_or_id(conf)] tool_distance = get_distance(tool_point, node_points[first_node]) # TODO: weighted average to balance cost and bias if heuristic == 'none': return 0 if heuristic == 'random': return random.random() elif heuristic == 'degree': # TODO: other graph statistics #printed_nodes = {n for e in printed for n in e} #node = n1 if n2 in printed_nodes else n2 #if node in ground_nodes: # return 0 raise NotImplementedError() elif heuristic == 'length': # Equivalent to mass if uniform density return get_element_length(element, node_points) elif heuristic == 'distance': return tool_distance elif heuristic == 'layered-distance': return (layer, tool_distance) # elif heuristic == 'components': # # Ground nodes intentionally omitted # # TODO: this is broken # remaining = all_elements - printed if forward else printed - {element} # vertices = nodes_from_elements(remaining) # components = get_connected_components(vertices, remaining) # #print('Components: {} | Distance: {:.3f}'.format(len(components), tool_distance)) # return (len(components), tool_distance) elif heuristic == 'fixed-tsp': # TODO: layer_from_edge[element] # TODO: score based on current distance from the plan in the tour # TODO: factor in the distance to the next element in a more effective way if order is None: return (INF, tool_distance) return (sign*order[element], tool_distance) # Chooses least expensive direction elif heuristic == 'tsp': if printed not in tsp_cache: # not last_plan and # TODO: seed with the previous solution #remaining = all_elements - printed if forward else printed #assert element in remaining #printed_nodes = compute_printed_nodes(ground_nodes, printed) if forward else ground_nodes tsp_cache[printed] = solve_tsp(all_elements, ground_nodes, node_points, printed, tool_point, initial_point, bidirectional=True, layers=True, max_time=30, visualize=False, verbose=True) #print(tsp_cache[printed]) if not last_plan: last_plan[:] = tsp_cache[printed][0] plan, cost = tsp_cache[printed] #plan = last_plan[len(printed):] if plan is None: #return tool_distance return (layer, INF) transit = compute_transit_distance(node_points, plan, start=tool_point, end=initial_point) assert forward first = plan[0] == directed #return not first # No info if the best isn't possible index = None for i, directed2 in enumerate(plan): undirected2 = get_undirected(all_elements, directed2) if element == undirected2: assert index is None index = i assert index is not None # Could also break ties by other in the plan # Two plans might have the same cost but the swap might be detrimental new_plan = [directed] + plan[:index] + plan[index+1:] assert len(plan) == len(new_plan) new_transit = compute_transit_distance(node_points, new_plan, start=tool_point, end=initial_point) #print(layer, cost, transit + compute_element_distance(node_points, plan), # new_transit + compute_element_distance(node_points, plan)) #return new_transit return (layer, not first, new_transit) # Layer important otherwise it shortcuts elif heuristic == 'online-tsp': if forward: _, tsp_distance = solve_tsp(all_elements-structure, ground_nodes, node_points, printed, node_points[second_node], initial_point, visualize=False) else: _, tsp_distance = solve_tsp(structure, ground_nodes, node_points, printed, initial_point, node_points[second_node], visualize=False) total = tool_distance + tsp_distance return total # elif heuristic == 'mst': # # TODO: this is broken # mst_distance = compute_component_mst(node_points, ground_nodes, remaining, # initial_position=node_points[second_node]) # return tool_distance + mst_distance elif heuristic == 'x': return sign * get_midpoint(node_points, element)[0] elif heuristic == 'z': return sign * compute_z_distance(node_points, element) elif heuristic == 'pitch': #delta = node_points[second_node] - node_points[first_node] delta = node_points[n2] - node_points[n1] return get_pitch(delta) elif heuristic == 'dijkstra': # offline # TODO: sum of all element path distances return sign*np.average([distance_from_node[node].cost for node in element]) # min, max, average elif heuristic == 'online-dijkstra': if printed not in distance_cache: distance_cache[printed] = compute_distance_from_node(printed, node_points, ground_nodes) return sign*min(distance_cache[printed][node].cost if node in distance_cache[printed] else INF for node in element) elif heuristic == 'plan-stiffness': if order is None: return None return (sign*order[element], directed not in order) elif heuristic == 'load': nodal_loads = checker.get_nodal_loads(existing_ids=structure_ids, dof_flattened=False) # get_self_weight_loads return reduce_op(np.linalg.norm(force_from_reaction(reaction)) for reaction in nodal_loads.values()) elif heuristic == 'fixed-forces': #printed = all_elements # disable to use most up-to-date # TODO: relative to the load introduced if printed not in reaction_cache: reaction_cache[printed] = compute_all_reactions(extrusion_path, all_elements, checker=checker) force = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reaction in reaction_cache[printed].reactions[element]) return force / normalizer elif heuristic == 'forces': reactions_from_nodes = compute_node_reactions(extrusion_path, structure, checker=checker) #torque = sum(np.linalg.norm(np.sum([torque_from_reaction(reaction) for reaction in reactions], axis=0)) # for reactions in reactions_from_nodes.values()) #return torque / normalizer total = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reactions in reactions_from_nodes.values() for reaction in reactions) return total / normalizer #return max(sum(np.linalg.norm(reaction_fn(reaction)) for reaction in reactions) # for reactions in reactions_from_nodes.values()) elif heuristic == 'stiffness': # TODO: add different variations # TODO: normalize by initial stiffness, length, or degree # Most unstable or least unstable first # Gets faster with fewer all_elements #old_stiffness = score_stiffness(extrusion_path, element_from_id, printed, checker=checker) stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better return stiffness / normalizer #return stiffness / old_stiffness elif heuristic == 'fixed-stiffness': # TODO: invert the sign for regression/progression? # TODO: sort FastDownward by the (fixed) action cost return stiffness_cache[element] / normalizer elif heuristic == 'relative-stiffness': stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better if normalizer == 0: return 0 return stiffness / normalizer #return stiffness / stiffness_cache[element] raise ValueError(heuristic)
def plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, elements, initial_position=None, checker=None, stiffness=True, heuristic='z', max_time=INF, max_backtrack=0): start_time = time.time() if stiffness and checker is None: checker = create_stiffness_checker(extrusion_path) remaining_elements = frozenset(elements) min_remaining = len(remaining_elements) queue = [(None, frozenset(), initial_position, [])] while queue and (elapsed_time(start_time) < max_time): _, printed, position, sequence = heapq.heappop(queue) num_remaining = len(remaining_elements) - len(printed) backtrack = num_remaining - min_remaining if max_backtrack < backtrack: break # continue if stiffness and not test_stiffness(extrusion_path, element_from_id, printed, checker=checker, verbose=False): continue if printed == remaining_elements: #from extrusion.visualization import draw_ordered distance = compute_sequence_distance(node_points, sequence, start=initial_position, end=initial_position) print('Success! Elements: {}, Distance: {:.3f}m, Time: {:.3f}sec'. format(len(sequence), distance, elapsed_time(start_time))) #local_search(extrusion_path, element_from_id, node_points, ground_nodes, checker, sequence, # initial_position=initial_position, stiffness=stiffness, max_time=INF) #draw_ordered(sequence, node_points) #wait_for_user() return sequence for directed in compute_printable_directed(remaining_elements, ground_nodes, printed): node1, node2 = directed element = get_undirected(elements, directed) new_printed = printed | {element} new_sequence = sequence + [directed] num_remaining = len(remaining_elements) - len(new_printed) min_remaining = min(min_remaining, num_remaining) # Don't count edge length distance = get_distance( position, node_points[node1]) if position is not None else None # distance = compute_sequence_distance(node_points, new_sequence) if heuristic == 'none': bias = None elif heuristic == 'random': bias = random.random() elif heuristic == 'z': bias = compute_z_distance(node_points, element) elif heuristic == 'distance': bias = distance else: raise ValueError(heuristic) #bias = heuristic_fn(printed, element, conf=None) # TODO: experiment with other biases priority = (num_remaining, bias, random.random()) heapq.heappush( queue, (priority, new_printed, node_points[node2], new_sequence)) print( 'Failed to find stiffness plan! Elements: {}, Min remaining {}, Time: {:.3f}sec' .format(len(remaining_elements), min_remaining, elapsed_time(start_time))) return None
def progression(robot, obstacles, element_bodies, extrusion_path, partial_orders=[], heuristic='z', max_time=INF, backtrack_limit=INF, revisit=False, stiffness=True, motions=True, collisions=True, lazy=LAZY, checker=None, **kwargs): start_time = time.time() initial_conf = get_configuration(robot) initial_position = get_tool_position(robot) element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path) if checker is None: checker = create_stiffness_checker(extrusion_path, verbose=False) print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes, precompute_collisions=False, collisions=collisions, **kwargs) id_from_element = get_id_from_element(element_from_id) all_elements = frozenset(element_bodies) heuristic_fn = get_heuristic_fn(robot, extrusion_path, heuristic, checker=checker, forward=True) initial_printed = frozenset() queue = [] visited = {initial_printed: Node(None, None)} if check_connected(ground_nodes, all_elements) and \ test_stiffness(extrusion_path, element_from_id, all_elements): add_successors(queue, all_elements, node_points, ground_nodes, heuristic_fn, initial_printed, initial_position, initial_conf, partial_orders=partial_orders) plan = None min_remaining = len(all_elements) num_evaluated = max_backtrack = stiffness_failures = extrusion_failures = transit_failures = 0 while queue and (elapsed_time(start_time) < max_time): num_evaluated += 1 visits, priority, printed, directed, current_conf = heapq.heappop( queue) element = get_undirected(all_elements, directed) num_remaining = len(all_elements) - len(printed) backtrack = num_remaining - min_remaining max_backtrack = max(max_backtrack, backtrack) if backtrack_limit < backtrack: break # continue num_evaluated += 1 if num_remaining < min_remaining: min_remaining = num_remaining print( 'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}' .format(num_evaluated, min_remaining, len(printed), element, id_from_element[element], elapsed_time(start_time))) next_printed = printed | {element} assert check_connected(ground_nodes, next_printed) if (next_printed in visited) or (stiffness and not test_stiffness( extrusion_path, element_from_id, next_printed, checker=checker) ): stiffness_failures += 1 continue if revisit: # could also prevent revisiting if command is not None heapq.heappush( queue, (visits + 1, priority, printed, directed, current_conf)) node1, node2 = directed command, = next(print_gen_fn(node1, element, extruded=printed), (None, )) if command is None: extrusion_failures += 1 continue if motions and not lazy: # TODO: test reachability from initial_conf motion_traj = compute_motion(robot, obstacles, element_bodies, printed, current_conf, command.start_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) if motion_traj is None: transit_failures += 1 continue command.trajectories.insert(0, motion_traj) visited[next_printed] = Node(command, printed) if all_elements <= next_printed: min_remaining = 0 commands = retrace_commands(visited, next_printed) if OPTIMIZE: commands = optimize_commands(robot, obstacles, element_bodies, extrusion_path, initial_conf, commands, motions=motions, collisions=collisions) plan = flatten_commands(commands) if motions and not lazy: motion_traj = compute_motion(robot, obstacles, element_bodies, frozenset(), initial_conf, plan[0].start_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) if motion_traj is None: plan = None transit_failures += 1 else: plan.append(motion_traj) if motions and lazy: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan, collisions=collisions, max_time=max_time - elapsed_time(start_time)) break # if plan is not None: # break add_successors(queue, all_elements, node_points, ground_nodes, heuristic_fn, next_printed, node_points[node2], command.end_conf, partial_orders=partial_orders) data = { 'num_evaluated': num_evaluated, 'min_remaining': min_remaining, 'max_backtrack': max_backtrack, 'stiffness_failures': stiffness_failures, 'extrusion_failures': extrusion_failures, 'transit_failures': transit_failures, } return plan, data
def lookahead(robot, obstacles, element_bodies, extrusion_path, partial_orders=[], num_ee=0, num_arm=1, plan_all=False, use_conflicts=False, use_replan=False, heuristic='z', max_time=INF, backtrack_limit=INF, revisit=False, ee_only=False, collisions=True, stiffness=True, motions=True, lazy=LAZY, checker=None, **kwargs): if not use_conflicts: num_ee, num_arm = min(num_ee, 1), min(num_arm, 1) if ee_only: num_ee, num_arm = max(num_arm, num_ee), 0 print('#EE: {} | #Arm: {}'.format(num_ee, num_arm)) # TODO: only check nearby remaining_elements # TODO: only check collisions conditioned on current decisions start_time = time.time() initial_conf = get_configuration(robot) initial_position = get_tool_position(robot) element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path) if checker is None: checker = create_stiffness_checker(extrusion_path, verbose=False) #print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes, # precompute_collisions=False, supports=False, ee_only=ee_only, # max_directions=MAX_DIRECTIONS, max_attempts=MAX_ATTEMPTS, collisions=collisions, **kwargs) full_print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes, precompute_collisions=False, ee_only=ee_only, allow_failures=True, collisions=collisions, **kwargs) # TODO: could just check environment collisions & kinematics instead of element collisions ee_print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes, precompute_collisions=False, ee_only=True, allow_failures=True, collisions=collisions, **kwargs) id_from_element = get_id_from_element(element_from_id) all_elements = frozenset(element_bodies) heuristic_fn = get_heuristic_fn(robot, extrusion_path, heuristic, checker=checker, forward=True) #distance_fn = get_distance_fn(robot, joints, weights=JOINT_WEIGHTS) # TODO: 2-step lookahead based on neighbors or spatial proximity full_sample_traj, full_trajs_from_element = get_sample_traj( ground_nodes, element_bodies, full_print_gen_fn, collisions=collisions) ee_sample_traj, ee_trajs_from_element = get_sample_traj( ground_nodes, element_bodies, ee_print_gen_fn, collisions=collisions) if ee_only: full_sample_traj = ee_sample_traj #ee_sample_traj, ee_trajs_from_element = full_sample_traj, full_trajs_from_element #heuristic_trajs_from_element = full_trajs_from_element if (num_ee == 0) else ee_trajs_from_element heuristic_trajs_from_element = full_trajs_from_element if ( num_arm != 0) else ee_trajs_from_element ######################### def sample_remaining(printed, next_printed, sample_fn, num=1, **kwargs): if num == 0: return True remaining_elements = (all_elements - next_printed) if plan_all else \ compute_printable_elements(all_elements, ground_nodes, next_printed) # TODO: could just consider nodes in printed (connected=True) return all( sample_fn(printed, next_printed, element, connected=False, num=num, **kwargs) for element in randomize(remaining_elements)) def conflict_fn(printed, element, conf): # Dead-end detection without stability performs reasonably well # TODO: could add element if desired order = retrace_elements(visited, printed) printed = frozenset( order[:-1]) # Remove last element (to ensure at least one traj) if use_replan: remaining = list(all_elements - printed) requires_replan = [ all(element in traj.colliding for traj in ee_trajs_from_element[e2] if not (traj.colliding & printed)) for e2 in remaining if e2 != element ] return len(requires_replan) else: safe_trajectories = [ traj for traj in heuristic_trajs_from_element[element] if not (traj.colliding & printed) ] assert safe_trajectories best_traj = max(safe_trajectories, key=lambda traj: len(traj.colliding)) num_colliding = len(best_traj.colliding) return -num_colliding #distance = distance_fn(conf, best_traj.start_conf) # TODO: ee distance vs conf distance # TODO: l0 distance based on whether we remain at the same node # TODO: minimize instability while printing (dynamic programming) #return (-num_colliding, distance) if use_conflicts: priority_fn = lambda *args: (conflict_fn(*args), heuristic_fn(*args)) else: priority_fn = heuristic_fn ######################### initial_printed = frozenset() queue = [] visited = {initial_printed: Node(None, None)} if check_connected(ground_nodes, all_elements) and \ test_stiffness(extrusion_path, element_from_id, all_elements) and \ sample_remaining(initial_printed, initial_printed, ee_sample_traj, num=num_ee) and \ sample_remaining(initial_printed, initial_printed, full_sample_traj, num=num_arm): add_successors(queue, all_elements, node_points, ground_nodes, priority_fn, initial_printed, initial_position, initial_conf, partial_orders=partial_orders) plan = None min_remaining = INF num_evaluated = worst_backtrack = num_deadends = stiffness_failures = extrusion_failures = transit_failures = 0 while queue and (elapsed_time(start_time) < max_time): num_evaluated += 1 visits, priority, printed, directed, current_conf = heapq.heappop( queue) element = get_undirected(all_elements, directed) # TODO: use the correct direction num_remaining = len(all_elements) - len(printed) backtrack = num_remaining - min_remaining worst_backtrack = max(worst_backtrack, backtrack) if backtrack_limit < backtrack: break # continue num_evaluated += 1 if num_remaining < min_remaining: min_remaining = num_remaining print( 'Iteration: {} | Best: {} | Backtrack: {} | Deadends: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}' .format(num_evaluated, min_remaining, worst_backtrack, num_deadends, len(printed), element, id_from_element[element], elapsed_time(start_time))) if has_gui(): color_structure(element_bodies, printed, element) next_printed = printed | {element} if next_printed in visited: continue assert check_connected(ground_nodes, next_printed) if stiffness and not test_stiffness(extrusion_path, element_from_id, next_printed, checker=checker, verbose=False): # Hard dead-end #num_deadends += 1 stiffness_failures += 1 print('Partial structure is not stiff!') continue if revisit: heapq.heappush( queue, (visits + 1, priority, printed, element, current_conf)) #condition = frozenset() #condition = set(retrace_elements(visited, printed, horizon=2)) #condition = printed # horizon=1 condition = next_printed if not sample_remaining( condition, next_printed, ee_sample_traj, num=num_ee): num_deadends += 1 print('An end-effector successor could not be sampled!') continue print('Sampling transition') #command = sample_extrusion(print_gen_fn, ground_nodes, printed, element) command = next( iter(full_sample_traj(printed, printed, element, connected=True)), None) if command is None: # Soft dead-end print('The transition could not be sampled!') extrusion_failures += 1 continue print('Sampling successors') if not sample_remaining( condition, next_printed, full_sample_traj, num=num_arm): num_deadends += 1 print('A successor could not be sampled!') continue start_conf = end_conf = None if not ee_only: start_conf, end_conf = command.start_conf, command.end_conf if (start_conf is not None) and motions and not lazy: motion_traj = compute_motion(robot, obstacles, element_bodies, printed, current_conf, start_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) if motion_traj is None: transit_failures += 1 continue command.trajectories.insert(0, motion_traj) visited[next_printed] = Node(command, printed) if all_elements <= next_printed: # TODO: anytime mode min_remaining = 0 plan = retrace_trajectories(visited, next_printed) if motions and not lazy: motion_traj = compute_motion(robot, obstacles, element_bodies, frozenset(), initial_conf, plan[0].start_conf, collisions=collisions, max_time=max_time - elapsed_time(start_time)) if motion_traj is None: plan = None transit_failures += 1 else: plan.append(motion_traj) if motions and lazy: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan, collisions=collisions, max_time=max_time - elapsed_time(start_time)) break # if plan is not None: # break add_successors(queue, all_elements, node_points, ground_nodes, priority_fn, next_printed, node_points[directed[1]], end_conf, partial_orders=partial_orders) data = { 'num_evaluated': num_evaluated, 'min_remaining': min_remaining, 'max_backtrack': worst_backtrack, 'stiffness_failures': stiffness_failures, 'extrusion_failures': extrusion_failures, 'transit_failures': transit_failures, 'num_deadends': num_deadends, } return plan, data
def solve_tsp(all_elements, ground_nodes, node_points, printed, initial_point, final_point, bidirectional=False, layers=True, max_time=30, visualize=True, verbose=False): # https://developers.google.com/optimization/routing/tsp # https://developers.google.com/optimization/reference/constraint_solver/routing/RoutingModel # http://www.math.uwaterloo.ca/tsp/concorde.html # https://developers.google.com/optimization/reference/python/constraint_solver/pywrapcp # AddDisjunction # TODO: pick up and delivery # TODO: time window for skipping elements # TODO: Minimum Spanning Tree (MST) bias # TODO: time window constraint to ensure connected # TODO: reuse by simply swapping out the first vertex # arc-routing from ortools.constraint_solver import routing_enums_pb2, pywrapcp from extrusion.visualization import draw_ordered, draw_model start_time = time.time() assert initial_point is not None remaining = all_elements - printed #printed_nodes = compute_printed_nodes(ground_nodes, printed) if not remaining: cost = get_distance(initial_point, final_point) return [], cost # TODO: use as a lower bound total_distance = compute_element_distance(node_points, remaining) # TODO: some of these are invalid still level_from_node, cost_from_edge, sequence = greedily_plan( all_elements, node_points, ground_nodes, remaining, initial_point) if sequence is None: return None, INF extrusion_edges = set() point_from_vertex = {INITIAL_NODE: initial_point, FINAL_NODE: final_point} frame_nodes = nodes_from_elements(remaining) min_level = min(level_from_node[n] for n in frame_nodes) max_level = max(level_from_node[n] for n in frame_nodes) frame_keys = set() keys_from_node = defaultdict(set) for element in remaining: mid = (element, element) point_from_vertex[mid] = get_midpoint(node_points, element) for node in element: key = (element, node) point_from_vertex[key] = node_points[node] frame_keys.add(key) keys_from_node[node].add(key) for reverse in [True, False]: directed = reverse_element(element) if reverse else element node1, node2 = directed #delta = node_points[node2] - node_points[node1] #pitch = get_pitch(delta) #upward = -SUPPORT_THETA <= pitch # theta = angle_between(delta, [0, 0, -1]) # upward = theta < (np.pi / 2 - SUPPORT_THETA) #if (directed in tree_elements): # or upward: if bidirectional or (directed in cost_from_edge): # Add edges from anything that is roughly the correct cost start = (element, node1) end = (element, node2) extrusion_edges.update({(start, mid), (mid, end)}) for node in keys_from_node: for edge in product(keys_from_node[node], repeat=2): extrusion_edges.add(edge) # Key thing is partial order on buckets of elements to adhere to height # Connect v2 to v1 if v2 is the same level # Traversing an edge might move to a prior level (but at most one) transit_edges = set() for directed in product(frame_keys, repeat=2): key1, key2 = directed element1, node1 = key1 #level1 = min(level_from_node[n] for n in element1) level1 = level_from_node[get_other_node(node1, element1)] _, node2 = key2 level2 = level_from_node[node2] if level2 in [level1, level1 + 1]: # TODO: could bucket more coarsely transit_edges.add(directed) for key in frame_keys: _, node = key if level_from_node[node] == min_level: transit_edges.add((INITIAL_NODE, key)) if level_from_node[node] in [max_level, max_level - 1]: transit_edges.add((key, FINAL_NODE)) # TODO: can also remove restriction that elements are printed in a single direction if not layers: transit_edges.update( product(frame_keys | {INITIAL_NODE, FINAL_NODE}, repeat=2)) # TODO: apply to greedy as well key_from_index = list( {k for pair in extrusion_edges | transit_edges for k in pair}) edge_weights = { pair: INVALID for pair in product(key_from_index, repeat=2) } for k1, k2 in transit_edges | extrusion_edges: p1, p2 = point_from_vertex[k1], point_from_vertex[k2] edge_weights[k1, k2] = get_distance(p1, p2) #edge_weights.update({e: 0. for e in extrusion_edges}) # frame edges are free edge_weights[FINAL_NODE, INITIAL_NODE] = 0. edge_weights[INITIAL_NODE, FINAL_NODE] = INVALID # Otherwise might be backward print( 'Elements: {} | Vertices: {} | Edges: {} | Structure: {:.3f} | Min Level {} | Max Level: {}' .format(len(remaining), len(key_from_index), len(edge_weights), total_distance, min_level, max_level)) index_from_key = dict(map(reversed, enumerate(key_from_index))) num_vehicles, depot = 1, index_from_key[INITIAL_NODE] manager = pywrapcp.RoutingIndexManager(len(key_from_index), num_vehicles, depot) #[depot], [depot]) solver = pywrapcp.RoutingModel(manager) cost_from_index = {} for (k1, k2), weight in edge_weights.items(): i1, i2 = index_from_key[k1], index_from_key[k2] cost_from_index[i1, i2] = int(math.ceil(SCALE * weight)) solver.SetArcCostEvaluatorOfAllVehicles( solver.RegisterTransitCallback( lambda i1, i2: cost_from_index[manager.IndexToNode( i1), manager.IndexToNode(i2)])) # from -> to # sequence = plan_stiffness(None, None, node_points, ground_nodes, elements, # initial_position=initial_point, stiffness=False, max_backtrack=INF) initial_order = [] #initial_order = [INITIAL_NODE] # Start and end automatically included for directed in sequence: node1, node2 = directed element = get_undirected(remaining, directed) initial_order.extend([ (element, node1), (element, element), (element, node2), ]) initial_order.append(FINAL_NODE) #initial_order.append(INITIAL_NODE) initial_route = [index_from_key[key] for key in initial_order] #index = initial_route.index(0) #initial_route = initial_route[index:] + initial_route[:index] + [0] initial_solution = solver.ReadAssignmentFromRoutes( [initial_route], ignore_inactive_indices=True) assert initial_solution is not None #print_solution(manager, solver, initial_solution) #print(solver.GetAllDimensionNames()) #print(solver.ComputeLowerBound()) objective = initial_solution.ObjectiveValue() / SCALE invalid = int(objective / INVALID) order = parse_solution(solver, manager, key_from_index, initial_solution)[:-1] ordered_pairs = get_pairs(order) cost = sum(edge_weights[pair] for pair in ordered_pairs) #print('Initial solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s'.format( # invalid, objective, cost, elapsed_time(start_time))) if False and visualize: # and invalid remove_all_debug() draw_model(printed, node_points, None, color=BLACK) draw_point(initial_point, color=BLACK) draw_point(final_point, color=GREEN) for pair in ordered_pairs: if edge_weights[pair] == INVALID: for key in pair: draw_point(point_from_vertex[key], color=RED) draw_ordered(ordered_pairs, point_from_vertex) wait_for_user() # TODO: pause only if viewer #sequence = extract_sequence(level_from_node, remaining, ordered_pairs) #print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point), total_distance+cost) #print([cost_from_edge[edge] for edge in sequence]) #return sequence, cost start_time = time.time() search_parameters = pywrapcp.DefaultRoutingSearchParameters() #search_parameters.solution_limit = 1 search_parameters.time_limit.seconds = int(max_time) search_parameters.log_search = verbose # AUTOMATIC | PATH_CHEAPEST_ARC | LOCAL_CHEAPEST_ARC | GLOBAL_CHEAPEST_ARC | LOCAL_CHEAPEST_INSERTION #search_parameters.first_solution_strategy = ( # routing_enums_pb2.FirstSolutionStrategy.AUTOMATIC) # AUTOMATIC | GREEDY_DESCENT | GUIDED_LOCAL_SEARCH | SIMULATED_ANNEALING | TABU_SEARCH | OBJECTIVE_TABU_SEARCH #search_parameters.local_search_metaheuristic = ( # routing_enums_pb2.LocalSearchMetaheuristic.GREEDY_DESCENT) #solution = solver.SolveWithParameters(search_parameters) solution = solver.SolveFromAssignmentWithParameters( initial_solution, search_parameters) #print_solution(manager, solver, solution) print('Status: {} | Text: {}'.format(solver.status(), STATUS[solver.status()])) if not solution: print('Failure! Duration: {:.3f}s'.format(elapsed_time(start_time))) return None, INF objective = solution.ObjectiveValue() / SCALE invalid = int(objective / INVALID) order = parse_solution(solver, manager, key_from_index, solution)[:-1] ordered_pairs = get_pairs(order) # + [(order[-1], order[0])] #cost = compute_element_distance(point_from_vertex, ordered_pairs) cost = sum(edge_weights[pair] for pair in ordered_pairs) print( 'Final solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s' .format(invalid, objective, cost, elapsed_time(start_time))) sequence = extract_sequence(level_from_node, remaining, ordered_pairs) #print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point)) #, total_distance+cost) #print(sequence) violations = 0 printed_nodes = compute_printed_nodes(ground_nodes, printed) for directed in sequence: if directed[0] not in printed_nodes: #print(directed) violations += 1 printed_nodes.update(directed) print('Violations:', violations) if visualize: # TODO: visualize by weight remove_all_debug() draw_model(printed, node_points, None, color=BLACK) draw_point(initial_point, color=BLACK) draw_point(final_point, color=GREEN) #tour_pairs = ordered_pairs + get_pairs(list(reversed(order))) #draw_model(extrusion_edges - set(tour_pairs), point_from_vertex, ground_nodes, color=BLACK) draw_ordered(ordered_pairs, point_from_vertex) wait_for_user() if sequence is None: return None, INF #print([cost_from_edge[edge] for edge in sequence]) return sequence, cost