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