swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add path finding

Browse Source
This commit is contained in:
shibo
2025-01-25 16:55:59 -05:00
parent 317ffbb5d6
commit 1db0800285
13 changed files with 830 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
path_finding/graph_search.py Normal file
View File

@@ -0,0 +1,156 @@
from heapq import heappush, heappop # Recommended.
import numpy as np
from rotorpy.world import World
from path_finding.occupancy_map import OccupancyMap # Recommended.
def get_neighbors(index, occ_map):
x, y, z = index
neighbors = []
costs = []
for dx in [-1, 0, 1]:
for dy in [-1, 0, 1]:
for dz in [-1, 0, 1]:
# Skip the center point (0, 0, 0)
if dx == 0 and dy == 0 and dz == 0:
continue
nx, ny, nz = x + dx, y + dy, z + dz
# Check if the node is not occupied
if not occ_map.is_occupied_index((nx, ny, nz)):
neighbors.append((nx, ny, nz))
changes = abs(dx)+abs(dy)+abs(dz)
if changes == 1:
cost = 1
elif changes ==2:
cost = 1.41
else:
cost = 1.73
costs.append(cost)
return neighbors, costs
def dijkstra(start, goal, start_index, goal_index, occ_map):
frontier = []
heappush(frontier,(0,start_index))
previous_nodes = {start_index:None}
cost = {start_index:0}
nodes_expanded = 0
while frontier:
current = heappop(frontier)[1]
nodes_expanded += 1
if current == goal_index:
break
next_nodes, next_costs = get_neighbors(current, occ_map)
for next in next_nodes:
new_cost = cost[current]+next_costs[next_nodes.index(next)]
if next not in cost or new_cost < cost[next]:
cost[next] = new_cost
heappush(frontier,(new_cost, next))
previous_nodes[next] = current
if goal_index not in previous_nodes:
return None, nodes_expanded
path = []
current = goal_index
while current != start_index:
path.append(occ_map.index_to_metric_center(current))
current = previous_nodes[current]
path.append(start)
path.reverse()
path[-1] = goal
return np.array(path), nodes_expanded
def heuristic(next, goal):
euclidean_distance = np.sqrt((next[0] - goal[0])**2 + (next[1] - goal[1])**2 + (next[2] - goal[2])**2)
manhattan_distance = (goal[0]-next[0])+(goal[1]-next[1])+(goal[2]-next[2])
return euclidean_distance
def astar_algorithm(start, goal, start_index, goal_index, occ_map):
frontier = []
heappush(frontier,(0,start_index))
previous_nodes = {start_index:None}
cost = {start_index:0}
nodes_expanded = 0
while frontier:
current = heappop(frontier)[1]
nodes_expanded += 1
if current == goal_index:
break
next_nodes, next_costs = get_neighbors(current, occ_map)
for next in next_nodes:
new_cost = cost[current]+next_costs[next_nodes.index(next)]
if next not in cost or new_cost < cost[next]:
cost[next] = new_cost
total_cost = new_cost + heuristic(next, goal_index)
heappush(frontier,(total_cost, next))
previous_nodes[next] = current
if goal_index not in previous_nodes:
return None, nodes_expanded
path = []
current = goal_index
while current != start_index:
path.append(occ_map.index_to_metric_center(current))
current = previous_nodes[current]
path.append(start)
path.reverse()
path[-1] = goal
return np.array(path), nodes_expanded
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
return a tuple (path, nodes_expanded)
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
nodes_expanded, the number of nodes that have been expanded
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
# MY CODE STARTS HERE
if astar == False:
return dijkstra(start, goal, start_index, goal_index, occ_map)
if astar == True:
return astar_algorithm(start, goal, start_index, goal_index, occ_map)

222
path_finding/occupancy_map.py Normal file
View File

@@ -0,0 +1,222 @@
import heapq
import numpy as np
from scipy.spatial import Rectangle
from scipy.spatial.transform import Rotation
from rotorpy.world import World
from rotorpy.utils import shapes
class OccupancyMap:
def __init__(self, world=World.empty((0, 2, 0, 2, 0, 2)), resolution=(.1, .1, .1), margin=.2):
"""
This class creates a 3D voxel occupancy map of the configuration space from a flightsim World object.
Parameters:
world, a flightsim World object
resolution, the discretization of the occupancy grid in x,y,z
margin, the inflation radius used to create the configuration space (assuming a spherical drone)
"""
self.world = world
self.resolution = np.array(resolution)
self.margin = margin
self._init_map_from_world()
def index_to_metric_negative_corner(self, index):
"""
Return the metric position of the most negative corner of a voxel, given its index in the occupancy grid
"""
return index*np.array(self.resolution) + self.origin
def index_to_metric_center(self, index):
"""
Return the metric position of the center of a voxel, given its index in the occupancy grid
"""
return self.index_to_metric_negative_corner(index) + self.resolution/2.0
def metric_to_index(self, metric):
"""
Returns the index of the voxel containing a metric point.
Remember that this and index_to_metric and not inverses of each other!
If the metric point lies on a voxel boundary along some coordinate,
the returned index is the lesser index.
"""
return np.floor((metric - self.origin)/self.resolution).astype('int')
def _metric_block_to_index_range(self, bounds, outer_bound=True):
"""
A fast test that returns the closed index range intervals of voxels
intercepting a rectangular bound. If outer_bound is true the returned
index range is conservatively large, if outer_bound is false the index
range is conservatively small.
"""
# Implementation note: The original intended resolution may not be
# exactly representable as a floating point number. For example, the
# floating point value for "0.1" is actually bigger than 0.1. This can
# cause surprising results on large maps. The solution used here is to
# slightly inflate or deflate the resolution by the smallest
# representative unit to achieve either an upper or lower bound result.
sign = 1 if outer_bound else -1
min_index_res = np.nextafter(self.resolution, sign * np.inf) # Use for lower corner.
max_index_res = np.nextafter(self.resolution, -sign * np.inf) # Use for upper corner.
bounds = np.asarray(bounds)
# Find minimum included index range.
min_corner = bounds[0::2]
min_frac_index = (min_corner - self.origin)/min_index_res
min_index = np.floor(min_frac_index).astype('int')
min_index[min_index == min_frac_index] -= 1
min_index = np.maximum(0, min_index)
# Find maximum included index range.
max_corner = bounds[1::2]
max_frac_index = (max_corner - self.origin)/max_index_res
max_index = np.floor(max_frac_index).astype('int')
max_index = np.minimum(max_index, np.asarray(self.map.shape)-1)
return (min_index, max_index)
def _init_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is
occupied, False is unoccupied. This function is called during
initialization of the object.
"""
# Initialize the occupancy map, marking all free.
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(abs(bounds[1+i*2]-bounds[i*2]))
voxel_dimensions_indices.append(int(np.ceil(voxel_dimensions_metric[i]/self.resolution[i])))
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array(bounds[0::2])
# Iterate through each block obstacle.
for block in self.world.world.get('blocks', []):
extent = block['extents']
block_rect = Rectangle([extent[1], extent[3], extent[5]], [extent[0], extent[2], extent[4]])
# Get index range that is definitely occupied by this block.
(inner_min_index, inner_max_index) = self._metric_block_to_index_range(extent, outer_bound=False)
a, b = inner_min_index, inner_max_index
self.map[a[0]:(b[0]+1), a[1]:(b[1]+1), a[2]:(b[2]+1)] = True
# Get index range that is definitely not occupied by this block.
outer_extent = extent + self.margin * np.array([-1, 1, -1, 1, -1, 1])
(outer_min_index, outer_max_index) = self._metric_block_to_index_range(outer_extent, outer_bound=True)
# Iterate over uncertain voxels with rect-rect distance check.
for i in range(outer_min_index[0], outer_max_index[0]+1):
for j in range(outer_min_index[1], outer_max_index[1]+1):
for k in range(outer_min_index[2], outer_max_index[2]+1):
# If map is not already occupied, check for collision.
if not self.map[i,j,k]:
metric_loc = self.index_to_metric_negative_corner((i,j,k))
voxel_rect = Rectangle(metric_loc+self.resolution, metric_loc)
rect_distance = voxel_rect.min_distance_rectangle(block_rect)
self.map[i,j,k] = rect_distance <= self.margin
def draw_filled(self, ax):
"""
Visualize the occupancy grid (mostly for debugging)
Warning: may be slow with O(10^3) occupied voxels or more
Parameters:
ax, an Axes3D object
"""
self.world.draw_empty_world(ax)
it = np.nditer(self.map, flags=['multi_index'])
while not it.finished:
if self.map[it.multi_index] == True:
metric_loc = self.index_to_metric_negative_corner(it.multi_index)
xmin, ymin, zmin = metric_loc
xmax, ymax, zmax = metric_loc + self.resolution
c = shapes.Cuboid(ax, xmax-xmin, ymax-ymin, zmax-zmin, alpha=0.1, linewidth=1, edgecolors='k', facecolors='b')
c.transform(position=(xmin, ymin, zmin))
it.iternext()
def _draw_voxel_face(self, ax, index, direction):
# Normalized coordinates of the top face.
face = np.array([(1,1,1), (-1,1,1), (-1,-1,1), (1,-1,1)])
# Rotate to find normalized coordinates of target face.
if direction[0] != 0:
axis = np.array([0, 1, 0]) * np.pi/2 * direction[0]
elif direction[1] != 0:
axis = np.array([-1, 0, 0]) * np.pi/2 * direction[1]
elif direction[2] != 0:
axis = np.array([1, 0, 0]) * np.pi/2 * (1-direction[2])
face = (Rotation.from_rotvec(axis).as_matrix() @ face.T).T
# Scale, position, and draw using Face object.
face = 0.5 * face * np.reshape(self.resolution, (1,3))
f = shapes.Face(ax, face, alpha=0.1, linewidth=1, edgecolors='k', facecolors='b')
f.transform(position=(self.index_to_metric_center(index)))
def draw_shell(self, ax):
self.world.draw_empty_world(ax)
it = np.nditer(self.map, flags=['multi_index'])
while not it.finished:
idx = it.multi_index
if self.map[idx] == True:
for d in [(0,0,-1), (0,0,1), (0,-1,0), (0,1,0), (-1,0,0), (1,0,0)]:
neigh_idx = (idx[0]+d[0], idx[1]+d[1], idx[2]+d[2])
neigh_exists = self.is_valid_index(neigh_idx)
if not neigh_exists or (neigh_exists and not self.map[neigh_idx]):
self._draw_voxel_face(ax, idx, d)
it.iternext()
def draw(self, ax):
self.draw_shell(ax)
def is_valid_index(self, voxel_index):
"""
Test if a voxel index is within the map.
Returns True if it is inside the map, False otherwise.
"""
for i in range(3):
if voxel_index[i] >= self.map.shape[i] or voxel_index[i] < 0:
return False
return True
def is_valid_metric(self, metric):
"""
Test if a metric point is within the world.
Returns True if it is inside the world, False otherwise.
"""
bounds = self.world.world['bounds']['extents']
for i in range(3):
if metric[i] <= bounds[i*2] or metric[i] >= bounds[i*2+1]:
return False
return True
def is_occupied_index(self, voxel_index):
"""
Test if a voxel index is occupied.
Returns True if occupied or outside the map, False otherwise.
"""
return (not self.is_valid_index(voxel_index)) or self.map[tuple(voxel_index)]
def is_occupied_metric(self, voxel_metric):
"""
Test if a metric point is within an occupied voxel.
Returns True if occupied or outside the map, False otherwise.
"""
ind = self.metric_to_index(voxel_metric)
return (not self.is_valid_index(ind)) or self.is_occupied_index(ind)
if __name__ == "__main__":
from flightsim.axes3ds import Axes3Ds
import matplotlib.pyplot as plt
# Create a world object first
world = World.random_forest(world_dims=(5, 5, 5), tree_width=.1, tree_height=5, num_trees=10)
# Create a figure
fig = plt.figure()
ax = Axes3Ds(fig)
# Draw the world
world.draw(ax)
# Create an Occupancy map
oc = OccupancyMap(world, (.2, .2, .5), .1)
# Draw the occupancy map (may be slow for many voxels; will look weird if plotted on top of a world.draw)
oc.draw(ax)
plt.show()