def debug_elements(robot, node_points, node_order, elements):
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
for element in elements:
color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0,
0)
draw_element(node_points, element, color=color)
wait_for_user('Continue?')
# TODO: topological sort
node = node_order[40]
node_neighbors = get_node_neighbors(elements)
for element in node_neighbors[node]:
color = (0, 1,
0) if element_supports(element, node, node_points) else (1, 0,
0)
draw_element(node_points, element, color)
element = elements[-1]
draw_element(node_points, element, (0, 1, 0))
incoming_edges, _ = neighbors_from_orders(
get_supported_orders(elements, node_points))
supporters = []
retrace_supporters(element, incoming_edges, supporters)
for e in supporters:
draw_element(node_points, e, (1, 0, 0))
wait_for_user('Continue?')
def compute_local_orders(elements, layer_from_n):
# TODO: could make level objects
# Could update whether a node is connected, but it's slightly tricky
partial_orders = set()
for n1, neighbors in get_node_neighbors(elements).items():
below, equal, above = [], [], [] # wrt n1
for e in neighbors: # Directed version of this (likely wouldn't need directions then)
n2 = get_other_node(n1, e)
if layer_from_n[n1] < layer_from_n[n2]:
above.append(e)
elif layer_from_n[n1] > layer_from_n[n2]:
below.append(e)
else:
equal.append(e)
partial_orders.update(product(below, equal + above))
partial_orders.update(product(equal, above))
return partial_orders
def get_supported_orders(elements, node_points):
node_neighbors = get_node_neighbors(elements)
orders = set()
for node in node_neighbors:
supporters = {
e
for e in node_neighbors[node]
if element_supports(e, node, node_points)
}
printers = {
e
for e in node_neighbors[node]
if is_start_node(node, e, node_points)
and not doubly_printable(e, node_points)
}
orders.update((e1, e2) for e1 in supporters for e2 in printers)
return orders
def visualize_stiffness(extrusion_path):
if not has_gui():
return
#label_elements(element_bodies)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
elements = list(element_from_id.values())
#draw_model(elements, node_points, ground_nodes)
# Freeform Assembly Planning
# TODO: https://arxiv.org/pdf/1801.00527.pdf
# Though assembly sequencing is often done by finding a disassembly sequence and reversing it, we will use a forward search.
# Thus a low-cost state will usually be correctly identified by considering only the deflection of the cantilevered beam path
# and approximating the rest of the beams as being infinitely stiff
reaction_from_node = compute_node_reactions(extrusion_path, elements)
#reaction_from_node = deformation.displacements # For visualizing displacements
#test_node_forces(node_points, reaction_from_node)
force_from_node = {
node: sum(
np.linalg.norm(force_from_reaction(reaction))
for reaction in reactions)
for node, reactions in reaction_from_node.items()
}
sorted_nodes = sorted(reaction_from_node,
key=lambda n: force_from_node[n],
reverse=True)
for i, node in enumerate(sorted_nodes):
print('{}) node={}, point={}, magnitude={:.3E}'.format(
i, node, node_points[node], force_from_node[node]))
#max_force = max(force_from_node.values())
max_force = max(
np.linalg.norm(reaction[:3])
for reactions in reaction_from_node.values() for reaction in reactions)
print('Max force:', max_force)
neighbors_from_node = get_node_neighbors(elements)
colors = sample_colors(len(sorted_nodes))
handles = []
for node, color in zip(sorted_nodes, colors):
#color = (0, 0, 0)
reactions = reaction_from_node[node]
#print(np.array(reactions))
start = node_points[node]
handles.extend(draw_point(start, color=color))
for reaction in reactions[:1]:
handles.append(
draw_reaction(start, reaction, max_force=max_force, color=RED))
for reaction in reactions[1:]:
handles.append(
draw_reaction(start,
reaction,
max_force=max_force,
color=GREEN))
print(
'Node: {} | Ground: {} | Neighbors: {} | Reactions: {} | Magnitude: {:.3E}'
.format(node,
(node in ground_nodes), len(neighbors_from_node[node]),
len(reactions), force_from_node[node]))
print('Total:', np.sum(reactions, axis=0))
wait_for_user()
#for handle in handles:
# remove_debug(handle)
#handles = []
#remove_all_debug()
# TODO: could compute the least balanced node with respect to original forces
# TODO: sum the norms of all the forces in the structure
#draw_sequence(sequence, node_points)
wait_for_user()
def get_print_gen_fn(robot,
fixed_obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
partial_orders=set(),
removed=set(),
collisions=True,
disable=False,
ee_only=False,
allow_failures=False,
p_nearby=0.,
max_directions=MAX_DIRECTIONS,
max_attempts=MAX_ATTEMPTS,
max_time=INF,
**kwargs):
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
if not collisions:
precompute_collisions = False
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(partial_orders)
initial_conf = get_configuration(robot)
end_effector = EndEffector(robot,
ee_link=link_from_name(robot, EE_LINK),
tool_link=link_from_name(robot, TOOL_LINK))
extrusions = {
reverse_element(element) if reverse else element:
Extrusion(end_effector, element_bodies, node_points, ground_nodes,
element, reverse)
for element in element_bodies for reverse in [False, True]
}
def gen_fn(node1, element, extruded=[], trajectories=[]): # fluents=[]):
start_time = time.time()
assert not is_reversed(element_bodies, element)
idle_time = 0
reverse = is_end(node1, element)
if disable or (len(extruded) < SKIP_PERCENTAGE *
len(element_bodies)): # For quick visualization
path, tool_path = [], []
traj = PrintTrajectory(end_effector, get_movable_joints(robot),
path, tool_path, element, reverse)
command = Command([traj])
yield (command, )
return
directed = reverse_element(element) if reverse else element
extrusion = extrusions[directed]
n1, n2 = reverse_element(element) if reverse else element
neighboring_elements = node_neighbors[n1] & node_neighbors[n2]
supporters = [] # TODO: can also do according to levels
retrace_supporters(element, incoming_supporters, supporters)
element_obstacles = {
element_bodies[e]
for e in supporters + list(extruded)
}
obstacles = set(fixed_obstacles) | element_obstacles
if not collisions:
obstacles = set()
#obstacles = set(fixed_obstacles)
elements_order = [
e for e in element_bodies if (e != element) and (
e not in removed) and (element_bodies[e] not in obstacles)
]
collision_fn = get_element_collision_fn(robot, obstacles)
#print(len(fixed_obstacles), len(element_obstacles), len(elements_order))
trajectories = list(trajectories)
for num in irange(INF):
for attempt, tool_traj in enumerate(
islice(extrusion.generator(), max_directions)):
# TODO: is this slower now for some reason?
#tool_traj.safe_from_body = {} # Disables caching
if not tool_traj.is_safe(obstacles):
continue
for _ in range(max_attempts):
if max_time <= elapsed_time(start_time):
return
nearby_conf = initial_conf if random.random(
) < p_nearby else None
command = compute_direction_path(tool_traj,
collision_fn,
ee_only=ee_only,
initial_conf=nearby_conf,
**kwargs)
if command is None:
continue
command.update_safe(extruded)
if precompute_collisions:
bodies_order = [
element_bodies[e] for e in elements_order
]
colliding = command_collision(end_effector, command,
bodies_order)
for element2, unsafe in zip(elements_order, colliding):
if unsafe:
command.set_unsafe(element2)
else:
command.set_safe(element2)
if not is_ground(element, ground_nodes) and (
neighboring_elements <= command.colliding):
continue # If all neighbors collide
trajectories.append(command)
if precompute_collisions:
prune_dominated(trajectories)
if command not in trajectories:
continue
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Trajectories: {} | Colliding: {}'
.format(num, n1, n2, ee_only, len(supporters), attempt,
len(trajectories),
sorted(len(t.colliding)
for t in trajectories)))
temp_time = time.time()
yield (command, )
idle_time += elapsed_time(temp_time)
if precompute_collisions:
if len(command.colliding) == 0:
#print('Reevaluated already non-colliding trajectory!')
return
elif len(command.colliding) == 1:
[colliding_element] = command.colliding
obstacles.add(element_bodies[colliding_element])
break
else:
if allow_failures:
yield None
else:
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Max attempts exceeded!'
.format(num, n1, n2, ee_only, len(supporters),
max_directions))
return
#yield None
return gen_fn