path_cost = branch[n][0]
while branch[n][1] != start:
# path.append(branch[n][2])
path.append(branch[n][1])
n = branch[n][1]
# path.append(branch[n][2])
path.append(branch[n][1])
# return path[::-1], path_cost
return path, path_cost
if __name__ == "__main__":
filename = 'data/colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
grid = create_grid(data, 7, 6)
start, goal = (315, 444), (8, 500)
path, cost = a_star(grid, heuristic, start, goal)
print(path, cost)
fig = plt.figure()
plt.imshow(grid, cmap='Greys', origin='lower')
# draw points
all_pts = np.array(path)
north_vals = all_pts[:, 0]
east_vals = all_pts[:, 1]
plt.scatter(east_vals, north_vals, c='red')
plt.ylabel('NORTH')
plt.xlabel('EAST')
# 3) write a method "create_graph()" that:
# defines a networkx graph as g = Graph()
# defines a tree = KDTree(nodes)
# test for connectivity between each node and
# k of it's nearest neighbors
# if nodes are connectable, add an edge to graph
# Iterate through all candidate nodes!
g = create_graph(nodes, 10, polygons)
# Create a grid map of the world
# This will create a grid map at 1 m above ground level
grid = create_grid(data, 1, 1)
fig = plt.figure()
plt.imshow(grid, cmap='Greys', origin='lower')
nmin = np.min(data[:, 0])
emin = np.min(data[:, 1])
# If you have a graph called "g" these plots should work
# Draw edges
for (n1, n2) in g.edges:
plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'black' , alpha=0.5)
# Draw all nodes connected or not in blue
for n1 in nodes:
(position[1] - goal_position[1])**2)
return h
if __name__ == "__main__":
filename = 'data/colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
print(data)
start_ne = (25, 100)
goal_ne = (650, 500)
# Static drone altitude (meters)
drone_altitude = 5
safety_distance = 2
grid = create_grid(data, drone_altitude, safety_distance)
skeleton = medial_axis(invert(grid))
# equivalent to
# plt.imshow(np.flip(grid, 0))
plt.imshow(grid, cmap='Greys', origin='lower')
plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7)
plt.plot(start_ne[1], start_ne[0], 'rx')
plt.plot(goal_ne[1], goal_ne[0], 'rx')
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.show()
yvals = np.random.uniform(ymin, ymax, num_samples)
zvals = np.random.uniform(zmin, zmax, num_samples)
samples = list(zip(xvals, yvals, zvals))
samples[:10]
t0 = time.time()
to_keep = []
for point in samples:
if not collides(polygons, point):
to_keep.append(point)
time_taken = time.time() - t0
print("Time taken {0} seconds ...", time_taken)
print(len(to_keep))
grid = create_grid(data, zmax, 1)
fig = plt.figure()
plt.imshow(grid, cmap='Greys', origin='lower')
nmin = np.min(data[:, 0])
emin = np.min(data[:, 1])
# draw points
all_pts = np.array(to_keep)
north_vals = all_pts[:, 0]
east_vals = all_pts[:, 1]
plt.scatter(east_vals - emin, north_vals - nmin, c='red')
plt.ylabel('NORTH')
plt.xlabel('EAST')
# Load the colliders data
# Discretize your search space into a grid or graph
# Define a start and goal location
# Find a coarse 2D plan from start to goal
# Choose a location along that plan and discretize a local volume around that location (for example, you might try
# a 40x40 m area that is 10 m high discretized into 1m^3 voxels)
# Define your goal in the local volume to a a node or voxel at the edge of the volume in the direction of the next
# waypoint in your coarse global plan.
# Plan a path through your 3D grid or graph to that node or voxel at the edge of the local volume.
# We'll import some of the routines from previous exercises that you might find useful here.
if __name__ == "__main__":
# This is the same obstacle data from the previous lesson.
filename = 'data/colliders.csv'
data = np.loadtxt(filename, delimiter=',', dtype='Float64', skiprows=2)
print(data)
flight_altitude = 3
safety_distance = 3
grid = create_grid(data, flight_altitude, safety_distance)
fig = plt.figure()
plt.imshow(grid, cmap='Greys', origin='lower')
plt.xlabel('NORTH')
plt.ylabel('EAST')
plt.show()