def compute_plan_deformation(problem, plan):
# TODO: absence of entry means ignore
problem = extrusion_name_from_path(problem)
problem_path = get_extrusion_path(problem)
checker = create_stiffness_checker(problem_path, verbose=False)
trans_tol, rot_tol = checker.get_nodal_deformation_tol()
if plan is None:
return trans_tol, rot_tol
element_from_id, _, _ = load_extrusion(problem_path)
printed = []
translations = []
rotations = []
for element in plan:
printed.append(element)
deformation = evaluate_stiffness(problem,
element_from_id,
printed,
checker=checker,
verbose=False)
trans, rot, _, _ = checker.get_max_nodal_deformation()
translations.append(trans)
rotations.append(rot)
# TODO: could return full history
return max(translations), max(rotations)
def optimize_commands(robot, obstacles, element_bodies, extrusion_path, initial_conf, commands,
max_iterations=INF, max_time=60,
motions=True, collisions=True, **kwargs):
if commands is None:
return None
start_time = time.time()
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes,
precompute_collisions=False, collisions=collisions, **kwargs)
commands = list(commands)
sequence = [command.elements[0] for command in commands]
directed_sequence = [command.directed_elements[0] for command in commands]
indices = list(range(len(sequence)))
#trajectories = flatten_commands(commands)
#print(len(trajectories), get_print_distance(trajectories, teleport=False))
total_distance = sum(get_command_distance(robot, initial_conf, commands, index, command)
for index, command in enumerate(commands))
# TODO: bias sampling towards far apart relative to neighoors
# TODO: bias IK solutions to be the best of several
iterations = extrusion_failures = 0
while (iterations < max_iterations) and (elapsed_time(start_time) < max_time):
iterations += 1
index = random.choice(indices)
printed = sequence[:index-1]
element = sequence[index]
directed = directed_sequence[index]
print(commands[index])
distance = get_command_distance(robot, initial_conf, commands, index, commands[index])
print('Iteration {} | Failures: {} | Distance: {:.3f} | Index: {}/{} | Time: {:.3f}'.format(
iterations, extrusion_failures, total_distance, index, len(indices), elapsed_time(start_time)))
new_command, = next(print_gen_fn(directed[0], element, extruded=printed), (None,))
if new_command is None:
extrusion_failures += 1
continue
new_distance = get_command_distance(robot, initial_conf, commands, index, new_command)
if new_distance < distance:
commands.pop(index)
commands.insert(index, new_command)
total_distance -= (distance - new_distance)
# data = {
# 'extrusion_failures': extrusion_failures,
# }
return commands #, data
def compute_node_reactions(extrusion_path, elements, **kwargs):
# https://github.com/yijiangh/conmech/blob/master/tests/test_stiffness_checker.py#L407
# https://github.com/yijiangh/conmech/blob/master/src/pyconmech/frame_analysis/stiffness_checker.py
element_from_id, _, ground_nodes = load_extrusion(extrusion_path)
reaction_forces = compute_all_reactions(extrusion_path, elements, **kwargs)
loads, fixities, reactions = reaction_forces
#partial_forces(reaction_forces, elements)
reaction_from_node = {}
for node, wrench in loads.items():
reaction_from_node.setdefault(node, []).append(wrench)
# The fixities are global. The reaction forces are local
for node, reaction in fixities.items(
): # Fixities are like the ground force to resist the structure?
reaction_from_node.setdefault(node, []).append(reaction)
reaction_from_node = add_reactions(reactions, reaction_from_node)
#nodes = nodes_from_elements(elements) # | ground_nodes
#assert set(reaction_from_node) == nodes
# TODO: check that they are balanced
return reaction_from_node
def compute_all_reactions(extrusion_path, elements, checker=None):
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
extruded_ids = get_extructed_ids(element_from_id, elements)
#if checker is None:
# TODO: some strange issue when reusing here
checker = create_stiffness_checker(extrusion_path, verbose=False)
deformation = evaluate_stiffness(extrusion_path,
element_from_id,
elements,
checker=checker,
verbose=False)
# TODO: slight torque due to the load
#nodal_loads = checker.get_nodal_loads(existing_ids=extruded_ids, dof_flattened=False)
nodal_loads = checker.get_self_weight_loads(existing_ids=extruded_ids,
dof_flattened=False)
#total_load = np.linalg.norm(force_from_reaction(sum(nodal_loads.values())))
reactions = compute_global_reactions(element_from_id, checker, deformation)
#distance = compute_element_distance(node_points, elements)
#density = total_load / distance
#nodes = nodes_from_elements(elements) | ground_nodes
#print(len(elements), total_load, density)
return ReactionForces(nodal_loads, deformation.fixities, reactions)
def check_plan(extrusion_path, planned_elements, verbose=False):
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
#checker = create_stiffness_checker(extrusion_name)
connected_nodes = set(ground_nodes)
handles = []
all_connected = True
all_stiff = True
extruded_elements = set()
for element in planned_elements:
extruded_elements.add(element)
n1, n2 = element
#is_connected &= any(n in connected_nodes for n in element)
is_connected = (n1 in connected_nodes)
connected_nodes.update(element)
#is_connected = check_connected(ground_nodes, extruded_elements)
#all_connected &= is_connected
is_stiff = test_stiffness(extrusion_path,
element_from_id,
extruded_elements,
verbose=verbose)
all_stiff &= is_stiff
if verbose:
structures = get_connected_structures(extruded_elements)
print('Elements: {} | Structures: {} | Connected: {} | Stiff: {}'.
format(len(extruded_elements), len(structures), is_connected,
is_stiff))
if has_gui():
is_stable = is_connected and is_stiff
color = BLACK if is_stable else RED
handles.append(draw_element(node_points, element, color))
wait_for_duration(0.1)
if not is_stable:
wait_for_user()
# Make these counts instead
print('Connected: {} | Stiff: {}'.format(all_connected, all_stiff))
return all_connected and all_stiff
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 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 get_heuristic_fn(robot, extrusion_path, heuristic, forward, checker=None):
initial_point = get_tool_position(robot)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
all_elements = frozenset(element_from_id.values())
sign = +1 if forward else -1
distance_from_node = compute_distance_from_node(all_elements, node_points, ground_nodes)
layer_from_edge = compute_layer_from_element(all_elements, node_points, ground_nodes)
_, layer_from_directed = compute_layer_from_directed(all_elements, node_points, ground_nodes)
tsp_cache = {} # Technically influenced by the current position as well
plan = None
if heuristic == 'fixed-tsp':
plan, _ = solve_tsp(all_elements, ground_nodes, node_points, set(), initial_point, initial_point, visualize=False)
#tsp_cache[all_elements] = plan
elif heuristic == 'plan-stiffness':
plan = plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, all_elements,
initial_position=initial_point, checker=checker, max_backtrack=INF)
order = None
if plan is not None:
order = {get_undirected(all_elements, directed): i for i, directed in enumerate(plan)}
stiffness_cache = {}
if heuristic in ('fixed-stiffness', 'relative-stiffness'):
stiffness_cache.update({element: score_stiffness(extrusion_path, element_from_id, all_elements - {element},
checker=checker) for element in all_elements})
last_plan = []
reaction_cache = {}
distance_cache = {}
ee_cache = {}
# TODO: round values for more tie-breaking opportunities
# TODO: compute for all all_elements up front, sort, and bucket for the score (more general than rounding)
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)
return fn
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 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)
#extrusion_path = rotate_problem(extrusion_path)
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.)
obstacles, robot = load_world()
#draw_model(elements, node_points, ground_nodes)
#wait_for_user()
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()
#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)
#plan_path = '{}_solution.json'.format(args.problem)
#with open(plan_path, 'w') as f:
# json.dump(plan_data, f, indent=2, sort_keys=True)
results.append((args, 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
def load_experiment(filename, overall=False, failed_runtimes=True):
# TODO: maybe just pass the random seed as a separate arg
# TODO: aggregate over all problems and score using IPC rules
# https://ipc2018-classical.bitbucket.io/
max_time = 0
data_from_problem = OrderedDict()
data = read_pickle(filename)
for config, result in data:
#config.problem = extrusion_name_from_path(config.problem)
if config.problem in EXCLUDE:
continue
problem = ALL if overall else config.problem
plan = result.get('sequence', None)
result[SUCCESS] = (plan is not None)
result[SUCCESS] *= 100
result[RUNTIME] = min(result[RUNTIME], config.max_time)
result['length'] = len(plan) if result[SUCCESS] else INF
#max_trans, max_rot = max_plan_deformation(config.problem, plan)
#result['max_trans'] = max_trans
#result['max_rot'] = max_rot
result.pop('sequence', None)
max_time = max(max_time, result[RUNTIME])
if not result[SUCCESS] and not failed_runtimes:
result.pop(RUNTIME, None)
data_from_problem.setdefault(problem, []).append((config, result))
for p_idx, problem in enumerate(sorted(data_from_problem)):
print()
problem_name = os.path.basename(
os.path.abspath(problem)) # TODO: this isn't a path...
print('{}) Problem: {}'.format(p_idx, problem_name))
if problem != ALL:
extrusion_path = get_extrusion_path(problem)
element_from_id, node_points, ground_nodes = load_extrusion(
extrusion_path, verbose=False)
print('Nodes: {} | Ground: {} | Elements: {}'.format(
len(node_points), len(ground_nodes), len(element_from_id)))
data_from_config = OrderedDict()
value_per_field = {}
for config, result in data_from_problem[problem]:
new_config = Configuration(None, None, *config[2:])
#print(config._asdict()) # config.__dict__
for field, value in config._asdict().items():
value_per_field.setdefault(field, set()).add(value)
data_from_config.setdefault(new_config, []).append(result)
print('Attributes:', str_from_object(value_per_field))
print('Configs:', len(data_from_config))
all_results = {}
for c_idx, config in enumerate(sorted(data_from_config, key=str)):
results = data_from_config[config]
accumulated_result = {}
for result in results:
for name, value in result.items():
#if result[SUCCESS] or (name == SUCCESS):
if is_number(value):
accumulated_result.setdefault(name, []).append(value)
mean_result = {
name: round(np.average(values), 3)
for name, values in accumulated_result.items()
}
key = {
field: value
for field, value in config._asdict().items()
if (value is not None) and (field in ['algorithm', 'bias'] or
(2 <= len(value_per_field[field])))
}
all_results[frozenset(key.items())] = {
name: values
for name, values in accumulated_result.items()
if name in SCORES
}
score = score_result(mean_result)
print('{}) {} ({}): {}'.format(c_idx, str_from_object(key),
len(results),
str_from_object(score)))
if problem == ALL:
for attribute in SCORES:
bar_graph(all_results, attribute)
scatter_plot(data)
print('Max time: {:.3f} sec'.format(max_time))