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))