def compute_euclidean_tree(node_points,
ground_nodes,
elements,
initial_position=None):
# remove printed elements from the tree
# TODO: directed version
start_time = time.time()
point_from_vertex, edges = embed_graph(elements, node_points, ground_nodes,
initial_position)
edge_weights = {(n1, n2): get_distance(point_from_vertex[n1],
point_from_vertex[n2])
for n1, n2 in edges}
components = get_connected_components(point_from_vertex, edge_weights)
tree = compute_spanning_tree(edge_weights)
from extrusion.visualization import draw_model
draw_model(tree, point_from_vertex, ground_nodes, color=BLUE)
draw_model(edges - tree, point_from_vertex, ground_nodes, color=RED)
weight = sum(edge_weights[e] for e in tree)
print(len(components), len(point_from_vertex), len(tree), weight,
elapsed_time(start_time))
wait_for_user()
return weight
def solve_serialized(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
trajectories=[],
post_process=False,
collisions=True,
disable=False,
max_time=INF,
**kwargs):
start_time = time.time()
saver = WorldSaver()
elements = set(element_bodies)
elements_from_layers = compute_elements_from_layer(elements, layer_from_n)
layers = sorted(elements_from_layers.keys())
print('Layers:', layers)
full_plan = []
makespan = 0.
removed = set()
for layer in reversed(layers):
print(SEPARATOR)
print('Layer: {}'.format(layer))
saver.restore()
remaining = elements_from_layers[layer]
printed = elements - remaining - removed
draw_model(remaining, node_points, ground_nodes, color=GREEN)
draw_model(printed, node_points, ground_nodes, color=RED)
problem = get_pddlstream(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
printed=printed,
removed=removed,
return_home=False,
trajectories=trajectories,
collisions=collisions,
disable=disable,
**kwargs)
layer_plan, certificate = solve_pddlstream(problem,
node_points,
element_bodies,
max_time=max_time -
elapsed_time(start_time))
remove_all_debug()
if layer_plan is None:
return None
if post_process:
print(SEPARATOR)
# Allows the planner to continue to check collisions
problem.init[:] = certificate.all_facts
#static_facts = extract_static_facts(layer_plan, ...)
#problem.init.extend(('Order',) + pair for pair in compute_total_orders(layer_plan))
for fact in [('print', ), ('move', )]:
if fact in problem.init:
problem.init.remove(fact)
new_layer_plan, _ = solve_pddlstream(problem,
node_points,
element_bodies,
planner=POSTPROCESS_PLANNER,
max_time=max_time -
elapsed_time(start_time))
if (new_layer_plan
is not None) and (compute_duration(new_layer_plan) <
compute_duration(layer_plan)):
layer_plan = new_layer_plan
user_input('{:.3f}->{:.3f}'.format(
compute_duration(layer_plan),
compute_duration(new_layer_plan)))
# TODO: replan in a cost sensitive way
layer_plan = apply_start(layer_plan, makespan)
duration = compute_duration(layer_plan)
makespan += duration
print(
'\nLength: {} | Start: {:.3f} | End: {:.3f} | Duration: {:.3f} | Makespan: {:.3f}'
.format(len(layer_plan), compute_start(layer_plan),
compute_end(layer_plan), duration, makespan))
full_plan.extend(layer_plan)
removed.update(remaining)
print(SEPARATOR)
print_plan(full_plan)
return full_plan, None
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 plan_extrusion(args_list,
viewer=False,
verify=False,
verbose=False,
watch=False):
results = []
if not args_list:
return results
# TODO: setCollisionFilterGroupMask
if not verbose:
sys.stdout = open(os.devnull, 'w')
problems = {args.problem for args in args_list}
assert len(problems) == 1
[problem] = problems
# TODO: change dir for pddlstream
extrusion_path = get_extrusion_path(problem)
if False:
extrusion_path = transform_json(problem)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path,
verbose=True)
elements = sorted(element_from_id.values())
#node_points = transform_model(problem, elements, node_points, ground_nodes)
connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR)
with LockRenderer(lock=True):
draw_pose(unit_pose(), length=1.)
json_data = read_json(extrusion_path)
draw_pose(parse_origin_pose(json_data))
draw_model(elements, node_points, ground_nodes)
obstacles, robot = load_world()
color = apply_alpha(BLACK, alpha=0) # 0, 1
#color = None
element_bodies = dict(
zip(elements,
create_elements_bodies(node_points, elements, color=color)))
set_extrusion_camera(node_points)
#if viewer:
# label_nodes(node_points)
saver = WorldSaver()
wait_if_gui()
#visualize_stiffness(extrusion_path)
#debug_elements(robot, node_points, node_order, elements)
initial_position = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
checker = None
plan = None
for args in args_list:
if args.stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
saver.restore()
#initial_conf = get_joint_positions(robot, get_movable_joints(robot))
with LockRenderer(lock=not viewer):
start_time = time.time()
plan, data = None, {}
with timeout(args.max_time):
with Profiler(num=10, cumulative=False):
plan, data = solve_extrusion(robot,
obstacles,
element_from_id,
node_points,
element_bodies,
extrusion_path,
ground_nodes,
args,
checker=checker)
runtime = elapsed_time(start_time)
sequence = recover_directed_sequence(plan)
if verify:
max_translation, max_rotation = compute_plan_deformation(
extrusion_path, recover_sequence(plan))
valid = check_plan(extrusion_path, sequence)
print('Valid:', valid)
safe = validate_trajectories(element_bodies, obstacles, plan)
print('Safe:', safe)
data.update({
'safe': safe,
'valid': valid,
'max_translation': max_translation,
'max_rotation': max_rotation,
})
data.update({
'runtime':
runtime,
'num_elements':
len(elements),
'ee_distance':
compute_sequence_distance(node_points,
sequence,
start=initial_position,
end=initial_position),
'print_distance':
get_print_distance(plan, teleport=True),
'distance':
get_print_distance(plan, teleport=False),
'sequence':
sequence,
'parameters':
get_global_parameters(),
'problem':
args.problem,
'algorithm':
args.algorithm,
'heuristic':
args.bias,
'plan_extrusions':
not args.disable,
'use_collisions':
not args.cfree,
'use_stiffness':
args.stiffness,
})
print(data)
results.append((args, data))
if True:
data.update({
'assembly': read_json(extrusion_path),
'plan': extract_plan_data(plan), # plan | trajectories
})
plan_file = '{}_solution.json'.format(args.problem)
plan_path = os.path.join('solutions', plan_file)
write_json(plan_path, data)
reset_simulation()
disconnect()
if watch and (plan is not None):
args = args_list[-1]
animate = not (args.disable or args.ee_only)
connect(use_gui=True, shadows=SHADOWS,
color=BACKGROUND_COLOR) # TODO: avoid reconnecting
obstacles, robot = load_world()
display_trajectories(
node_points,
ground_nodes,
plan, #time_step=None, video=True,
animate=animate)
reset_simulation()
disconnect()
if not verbose:
sys.stdout.close()
return results