def guess_edges(self, nodes, neighbor_len=20) -> List:
'''
Given a bunch of nodes in 3d space, guess some edges for each node.
The optimal way would be to generate a complete graph (where every node
is connected to every other node), but the minimum spanning aborescence
causes tons of page faults and thrashes my disk.
'''
edges = []
for node in nodes:
closest_nodes = sorted(
nodes,
key=lambda x: euclidean_distance(node, x))[:neighbor_len]
edges += [(node, neighbor) for neighbor in closest_nodes]
return edges
def build_edges(self, nodes, risk_weight=1):
'''
Given a list of nodes, builds as complete euclidean graph.
Returns the aborescence (directed analog of euclidean minimum
spanning tree) of the graph.
'''
print('Building edges...')
print(len(nodes))
with phase_timer.Timer():
g = nx.DiGraph()
g.add_nodes_from(nodes)
edges = self.guess_edges(nodes)
# three-ple of node, node, weight
ebunch = [(edge[0], edge[1], compute_cost(*edge))
for edge in edges]
g.add_weighted_edges_from(ebunch)
# Prepopulate edge distance
for a, b in g.edges:
g[a][b]['dist'] = euclidean_distance(a, b)
self.g = g
def generate_graph(path: str):
'''
Generate a graph data structure from a mesh polygon file
'''
p = plyfile.PlyData.read(path)
# format is x, y, z, nx, ny, nz
nodes = [Node(index, vertex) for index, vertex in enumerate(p['vertex'])]
# convert faces to edges
faces = [face[0] for face in p['face']]
edges = []
for face in faces:
edges += face_to_edge(face)
G = nx.Graph()
G.add_nodes_from(nodes)
edges = associate_edges(edges)
G.add_edges_from(edges)
# prepopulate edge distance
for a, b in G.edges:
G[a][b]['dist'] = euclidean_distance(a, b)
#nx.set_edge_attributes(G, 'dist', edge_dists)
return G
def multi_bot(bots=4, cache=True):
'''
Have multiple tethered robots attempt to get to
the goal node. Boths can't occupy the same space
at the same time, or stray more than
tether_distance from the other bots.help
'''
print('Loading graph...')
g = nx.read_gpickle(DATA_DIR + 'graph.gpickle')
print('Loading handholds...')
with open(DATA_DIR + 'handhold.pickle', 'rb') as f:
h = pickle.load(f)
## Generate XYZ file for pics
with open(DATA_DIR + 'handholds.xyz', 'w') as f:
for n in h.g.nodes:
f.write('{} {} {}\n'.format(n.x, n.y, n.z))
# These look like good node positions
START_NODE_POS = [
# planar
(362.98, -747.74, -33.73),
(365.12, -747.02, -34.31),
(355.47, -732.96, -31.34),
(370.75, -724.73, -32.03)
# Old
# (-249.14, 27.9, -34.4),
# (543.18, -696.065, -28.9262),
# (518.1, -687.459, -23.527),
# (524.652, -667.315, -20.868),
# (550.481, -675.239, -23.726)
]
END_NODE_POS = (-139.15, 877.49, 212.1)
# old
#END_NODE_POS = (-63.16, -427.12, 69.33)
if ITOKAWA:
START_NODE_POS = [
# MAXIMA
#(-87.43, 50.58, 111.15),
#(-90.38, 41, 108.55),
#(-81.549, 39.78, 110.72),
#(-70.25, 62.9, 106.7),
# PLANAR
(-159.38, -119.26, 15.17),
(-162.14, -118.72, 14.44),
(-162.10, -118.41, 15.70),
(-159.33, -118.92, 16.42)
]
# MAXIMA
#END_NODE_POS = (140.88, -22.55, 113.61)
# PLANAR
#END_NODE_POS = (94.06, 89.07, 74.53)
#END_NODE_POS = (-69.78, -7.15, -105.94)
# Works
#END_NODE_POS = (-161.405, -115.7, 25.83)
# Works
#END_NODE_POS = (-135.54, -56.34, 77.56)
END_NODE_POS = (-61.57, 21.41, 116.34)
#sorted_nodes = sorted(h.g.nodes(), key=lambda n: n.x ** 2 + n.y ** 2 + n.z ** 2)
print(h.g.number_of_nodes())
print(h.g.number_of_edges())
start_nodes = [get_node(h.g, pos) for pos in START_NODE_POS]
#start_nodes = sorted_nodes[:4]
end_node = get_node(h.g, END_NODE_POS)
#end_node = sorted_nodes[250]
# Make sure that we actually find the nodes...
assert(any(start_nodes))
# Run a simulation with bots tethered to each other
# they can't occupy the same space, and cannot go further
# than Bot.tether_length from the other bots
bots = [Bot(i, node) for i, node in enumerate(start_nodes)]
# init occupied nodes
for b in bots:
b.node.occupied = b
print('bots', [b.node for b in bots])
turn = 0
try:
while True:
if not any([b.moved for b in bots]):
raise Exception('No bots moved last turn')
for bot in bots:
turn += 1
bot.path.append((turn, bot.node.x, bot.node.y, bot.node.z))
try:
path_nodes = bounded_leg_astar(
h.g,
bot.node,
end_node,
heuristic=compute_cost,
bots=[b for b in bots if b.id != bot.id]
)
bot.moved = True
except:
print(bot.id, 'did not move')
bot.moved = False
bot.node.occupied = bot
continue
print('{}: {}: {} -> {}'.format(len(bot.path), bot.id, bot.node, path_nodes[1]))
bot.node.occupied = False
bot.total_dist += euclidean_distance(bot.node, path_nodes[1])
bot.node = path_nodes[1]
path_nodes[1].occupied = bot
if end_node.occupied:
raise Found
except Found:
print('Bot {} reached end node'.format(end_node.occupied.id))
print('final pos: ', [bot.node for bot in bots])
for n in h.g.nodes:
if n.occupied:
print(n, 'is occupied')
for bot in bots:
import compute_path
x, y, z = compute_path.compute_hub_pos(bots)
print('bot {} is {} units away from hub'.format(
bot.id, euclidean_distance_c(bot.node, (x, y, z))))
print('Saving paths...')
for bot in bots:
# Print stats
print('{} moved {} total distance units'.format(bot.id, bot.total_dist))
# Save paths for analysis
with open(DATA_DIR + 'bot{}-path'.format(bot.id), 'w+') as f:
for move in bot.path:
f.write('{} {} {} {}\n'.format(bot.id, move[1], move[2], move[3]))
with open(DATA_DIR + 'bot{}-path.xyz'.format(bot.id), 'w+') as f:
for move in bot.path:
f.write('{} {} {}\n'.format(move[1], move[2], move[3]))
def score_local_maximum(graph, node):
'''
Given a local maxima, return a score denoting how 'big' of a local maxima it is
'''
dists = [euclidean_distance(node, n) for n in graph.neighbors(node)]
return 0.1 * sum(dists)