def extract_sequence(level_from_node, remaining, ordered_pairs): planned = set() sequence = [] for key1, key2 in ordered_pairs[1:-1]: element1, node1 = key1 element2, node2 = key2 if (element1 == element2) and (element1 not in planned) and ( node1 in level_from_node): directed = reverse_element(element1) if is_end( node1, element1) else element1 planned.add(element1) sequence.append(directed) if remaining != planned: return None return sequence
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 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
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
def directed(self): return reverse_element(self.element) if self.reverse else self.element