def generatepath(source_loc: Tuple[float, float],
target_loc: Tuple[float, float],
other_rects: List[List[Dict[str, float]]],
agent_width: float = PERFORMER_WIDTH) \
-> Optional[List[Tuple[float, float]]]:
"""Boundary has to be CCW, Holes CW"""
# dilate all the polygons based on the agent width
dilation = agent_width / 2.0
polygons = _dilate_polygons(other_rects, dilation, Point(*source_loc), Point(*target_loc))
if polygons is None:
return None
# they may intersect now, so unify any that do
poly_coords = _unify_and_clean_polygons(polygons)
environment = Environment()
boundary_coordinates = [(MAX_X - dilation, MAX_Z - dilation),
(MIN_X + dilation, MAX_Z - dilation),
(MIN_X + dilation, MIN_Z + dilation),
(MAX_X - dilation, MIN_Z + dilation)]
try:
environment.store(boundary_coordinates, poly_coords, validate=True)
environment.prepare()
path, length = environment.find_shortest_path(source_loc, target_loc)
# Work around a bug in the DirectedHeuristicGraph class
# defined by the extremitypathfinder module. It causes the
# graph to keep growing across successive Environments. See
# https://github.com/MrMinimal64/extremitypathfinder/issues/10
environment.graph.all_nodes.clear()
except TypeError as e:
# We sometimes get "TypeError: unsupported operand type(s) for
# +: 'NoneType' and 'float'" and "'<' not supported between
# instances of 'generator' and 'generator'", but I don't know
# why.
logging.info(f'path finding failed (maybe impossible?): {e}')
return None
except Exception:
# We shouldn't get other exceptions from the path finder, but
# just in case...
logging.warning('unexpected error in path finding')
traceback.print_exc()
logObject = {
'boundary': boundary_coordinates,
'holes': poly_coords,
'start': source_loc,
'target': target_loc
}
logging.warning(logObject)
return None
return path
def _generate_shortest_path_position_list(
starting_position: Tuple[float, float], goal_position: Tuple[float, float],
pathfinding_environment: Environment
) -> Optional[List[Tuple[float, float]]]:
"""Generate and return the postion list for the shortest path from the
given starting position to the given goal position."""
try:
if not pathfinding_environment.within_map(starting_position):
logging.debug('Starting position not in pathfinding environment.')
return None
if not pathfinding_environment.within_map(goal_position):
logging.debug('Goal position not in pathfinding environment.')
return None
path, length = pathfinding_environment.find_shortest_path(
starting_position, goal_position)
except Exception as e:
logging.error('UNEXPECTED ERROR IN PATHFINDING')
logging.error(e)
return None
return path if len(path) > 0 else None
class GlobalLocalPlanner():
def __init__(self):
self.environment = Environment()
# counter clockwise vertex numbering!
boundary_coordinates = [(0 + ROBOT_SIZE, 0 + ROBOT_SIZE),
(3.25 - ROBOT_SIZE, 0 + ROBOT_SIZE),
(3.25 - ROBOT_SIZE, 1.0 + ROBOT_SIZE),
(3.5 + ROBOT_SIZE, 1.0 + ROBOT_SIZE),
(3.5 + ROBOT_SIZE, 0 + ROBOT_SIZE),
(8.0 - ROBOT_SIZE, 0 + ROBOT_SIZE),
(8.0 - ROBOT_SIZE, 5.0 - ROBOT_SIZE),
(8.0 - 3.25 + ROBOT_SIZE, 5.0 - ROBOT_SIZE),
(8.0 - 3.25 + ROBOT_SIZE, 4.0 - ROBOT_SIZE),
(8.0 - 3.5 - ROBOT_SIZE, 4.0 - ROBOT_SIZE),
(8.0 - 3.5 - ROBOT_SIZE, 5.0 - ROBOT_SIZE),
(0 + ROBOT_SIZE, 5.0 - ROBOT_SIZE)]
# clockwise numbering!
list_of_holes = []
for (x, y), (w, h) in zip(BORDER_POS[:], BORDER_BOX[:]):
#x, y, w, h = x*10, y*10, w*10, h*10
list_of_holes.append([
((x - w - ROBOT_SIZE), (y - h - ROBOT_SIZE)),
((x - w - ROBOT_SIZE), (y + h + ROBOT_SIZE)),
((x + w + ROBOT_SIZE), (y + h + ROBOT_SIZE)),
((x + w + ROBOT_SIZE), (y - h - ROBOT_SIZE)),
])
self.environment.store(boundary_coordinates,
list_of_holes,
validate=False)
self.environment.prepare()
self.done = True
self.dynamic = DynamicWindow()
def plot(self):
draw_prepared_map(self.environment)
def findPath(self, start, goal):
path, length = self.environment.find_shortest_path(start, goal)
return path
def setGoal(self, start, goal, angle=0):
#print(start, goal)
self.path = self.findPath(start, goal)
self.index = 1
self.angle_path = []
if len(self.path) > 1:
self.next_target = self.path[1]
#print("new target: {}".format(self.next_target))
for i, j in zip(self.path[:-1], self.path[1:]):
self.angle_path.append(math.atan2(j[1] - i[1], j[0] - i[0]))
self.angle_path.append(angle)
self.next_angle = self.angle_path[0]
self.done = False
else:
self.done = True
def moveTo(self, pos, vel, angle, angular, action):
if self.done:
return action
if self.distance(pos, self.next_target) < 0.4:
self.index += 1
if self.index < len(self.path):
self.next_target = self.path[self.index]
self.next_angle = self.angle_path[self.index - 1]
print("new target: {}".format(self.next_target))
else:
self.done = True
action[0], action[1], action[2] = 0, 0, 0
return action
#tic = time.time()
tmp_angle = math.atan2(self.next_target[1] - pos[1],
self.next_target[0] - pos[0])
tmp_length = (self.next_target[0]**2 + self.next_target[1]**2)
tmp_target = np.array([math.cos(tmp_angle),
math.sin(tmp_angle)]) * min(5, tmp_length)
action = self.dynamic.moveTo(action, pos, vel, angle, angular,
tmp_target, self.next_angle)
#print(time.time() - tic)
'''
u = np.array([
self.next_target[0]-pos[0],
self.next_target[1]-pos[1]])
target_angle = math.atan2(u[1], u[0])
u = u.reshape([2, 1])
mat_angle = np.array([
[math.cos(angle), math.sin(angle)],
[math.sin(angle), -math.cos(angle)]])
v = np.matmul(np.linalg.inv(mat_angle), u) / 10
#print(u, v, angle)
v /= v.max()
action[0] = v[0][0]
action[1] = target_angle - angle
action[2] = v[1][0]
'''
return action
def distance(self, p1, p2):
return ((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)**0.5
def find_path(thymioCoord, goal, obsListOrig, plotFlag=True):
"""Given an inital position, a goal and a set of obstacle, will create a Visibility Graph of the map,
then perform an A* search on the given graph, and return the shortest path
Librairy used for visiblity Graph generation and Graph search:https://github.com/MrMinimal64/extremitypathfinder
"""
obsList = obsListOrig.copy()
MARGE_BORD = 5
EXTREME_DIST = 400
# if obstacle are near the map border, make them bigger so we are surefound path dosn't go trough the small gap
for iObs in range(0, len(obsList)):
for iVert in range(0, len(obsList[iObs].vertexExpanded)):
listV = list(obsList[iObs].vertexExpanded[iVert])
if listV[0] > MAP_MAX_X_AXIS - MARGE_BORD:
listV[0] = EXTREME_DIST
obsList[iObs].vertexExpanded[iVert] = listV
if listV[0] < 0 + MARGE_BORD:
listV[0] = -EXTREME_DIST
obsList[iObs].vertexExpanded[iVert] = listV
if listV[1] > MAP_MAX_Y_AXIS - MARGE_BORD:
listV[1] = EXTREME_DIST
obsList[iObs].vertexExpanded[iVert] = listV
if listV[1] < 0 + MARGE_BORD:
listV[1] = -EXTREME_DIST
obsList[iObs].vertexExpanded[iVert] = listV
if plotFlag:
for obs in obsList:
unzippedList = list(zip(*obs.vertex))
unzippedList = [list(elem) for elem in unzippedList]
unzippedList[0].append(unzippedList[0][0])
unzippedList[1].append(unzippedList[1][0])
plt.plot(unzippedList[0], unzippedList[1])
if obs.expandEnabled:
unzippedList = list(zip(*obs.vertexExpanded))
unzippedList = [list(elem) for elem in unzippedList]
unzippedList[0].append(unzippedList[0][0])
unzippedList[1].append(unzippedList[1][0])
plt.plot(unzippedList[0], unzippedList[1], '--')
plt.plot(thymioCoord[0], thymioCoord[1], 'o', color='green')
plt.plot(goal[0], goal[1], 'o', color='red')
map = Environment()
# boundary of the map, anti clockwise vertex numbering!
boundary_coordinates = [(0.0, 0.0), (MAP_MAX_X_AXIS, 0.0),
(MAP_MAX_X_AXIS, MAP_MAX_Y_AXIS),
(0.0, MAP_MAX_Y_AXIS)]
list_of_obstacle = []
for obs in obsList:
if obs.expandEnabled:
list_of_obstacle.append(obs.vertexExpanded)
map.store(boundary_coordinates, list_of_obstacle, validate=False)
map.prepare()
path, length = map.find_shortest_path(thymioCoord, goal)
if plotFlag:
unzippedPath = list(zip(*path))
plt.plot(unzippedPath[0], unzippedPath[1], '--', color='black')
plt.xlim(0, MAP_MAX_X_AXIS)
plt.ylim(0, MAP_MAX_Y_AXIS)
plt.gca().set_aspect('equal', adjustable='box')
plt.show()
return path
(4.5, 7.0),
(5.0, 4.0),
]]
# environment.store(boundary_coordinates, list_of_holes, validate=True, export_plots=True)
environment.store(boundary_coordinates, list_of_holes, validate=False)
environment.prepare()
# environment.export_pickle()
# from extremitypathfinder.extremitypathfinder import load_pickle
# environment = load_pickle()
start_coordinates = (4.5, 1.0)
goal_coordinates = (4.0, 8.5)
path, length = environment.find_shortest_path(start_coordinates,
goal_coordinates)
print(path, length)
# grid world
size_x, size_y = 19, 10
obstacle_iter = [
# obstacles changing boundary
# (x,y),
(0, 1),
(1, 1),
(2, 1),
(3, 1),
(17, 9),
(17, 8),
(17, 7),
(17, 5),
def test_fct(self):
environment = PolygonEnvironment()
size_x, size_y = 19, 10
obstacle_iter = [
# (x,y),
# obstacles changing boundary
(0, 1),
(1, 1),
(2, 1),
(3, 1),
(17, 9),
(17, 8),
(17, 7),
(17, 5),
(17, 4),
(17, 3),
(17, 2),
(17, 1),
(17, 0),
# hole 1
(5, 5),
(5, 6),
(6, 6),
(6, 7),
(7, 7),
# hole 2
(7, 5),
]
environment.store_grid_world(size_x,
size_y,
obstacle_iter,
simplify=False,
validate=False)
assert len(
environment.all_extremities) == 17 # should detect all extremities
environment.prepare()
assert len(environment.graph.all_nodes
) == 16 # identical nodes should get joined in the graph!
# test if points outside the map are being rejected
for start_coordinates, goal_coordinates in [
# outside of map region
((-1, 5.0), (17, 0.5)),
((17, 0.5), (-1, 5.0)),
((20, 5.0), (17, 0.5)),
((17, 0.5), (20, 5.0)),
((1, -5.0), (17, 0.5)),
((17, 0.5), (1, -5.0)),
((1, 11.0), (17, 0.5)),
((17, 0.5), (1, 11.0)),
# outside boundary polygon
((17.5, 5.0), (17, 0.5)),
((17, 0.5), (17.5, 5.0)),
((1, 1.5), (17, 0.5)),
((17, 0.5), (1, 1.5)),
# inside hole
((6.5, 6.5), (17, 0.5)),
((17, 0.5), (6.5, 6.5)),
]:
with pytest.raises(ValueError):
environment.find_shortest_path(start_coordinates,
goal_coordinates)
for ((start_coordinates, goal_coordinates), expected_output) in [
# ((start,goal),(path,distance))
# identical nodes
(((15, 5), (15, 5)), ([(15, 5), (15, 5)], 0.0)),
# directly reachable
(((15, 5), (15, 6)), ([(15, 5), (15, 6)], 1.0)),
(((15, 6), (15, 5)), ([(15, 6), (15, 5)], 1.0)),
(((15, 5), (16, 6)), ([(15, 5), (16, 6)], sqrt(2))),
(((16, 6), (15, 5)), ([(16, 6), (15, 5)], sqrt(2))),
# points on the polygon edges (vertices) should be accepted!
# on edge
(((15, 0), (15, 6)), ([(15, 0), (15, 6)], 6.0)),
(((15, 6), (15, 0)), ([(15, 6), (15, 0)], 6.0)),
(((17, 5), (16, 5)), ([(17, 5), (16, 5)], 1.0)),
(((16, 5), (17, 5)), ([(16, 5), (17, 5)], 1.0)),
# on edge of hole
(((7, 8), (7, 9)), ([(7, 8), (7, 9)], 1.0)),
(((7, 9), (7, 8)), ([(7, 9), (7, 8)], 1.0)),
# on vertex
(((4, 2), (4, 3)), ([(4, 2), (4, 3)], 1.0)),
(((4, 3), (4, 2)), ([(4, 3), (4, 2)], 1.0)),
# on vertex of hole
(((6, 8), (6, 9)), ([(6, 8), (6, 9)], 1.0)),
(((6, 9), (6, 8)), ([(6, 9), (6, 8)], 1.0)),
# on two vertices
# coinciding with edge (direct neighbour)
(((4, 2), (4, 1)), ([(4, 2), (4, 1)], 1.0)),
(((5, 5), (5, 7)), ([(5, 5), (5, 7)], 2.0)),
# should have direct connection to all visible extremities! connected in graph
(((6, 8), (5, 7)), ([(6, 8), (5, 7)], sqrt(2))),
(((4, 1), (5, 7)), ([(4, 1), (5, 7)], sqrt(1**2 + 6**2))),
# should have direct connection to all visible extremities! even if not connected in graph!
(((4, 2), (5, 7)), ([(4, 2), (5, 7)], sqrt(1**2 + 5**2))),
# mix of edges and vertices, directly visible
(((2, 2), (5, 7)), ([(2, 2), (5, 7)], sqrt(3**2 + 5**2))),
# also regular points should have direct connection to all visible extremities!
(((10, 3), (17, 6)), ([(10, 3), (17, 6)], sqrt(7**2 + 3**2))),
(((10, 3), (8, 8)), ([(10, 3), (8, 8)], sqrt(2**2 + 5**2))),
# even if the query point lies in front of an extremity! (test if new query vertices are being created!)
(((10, 3), (8, 5)), ([(10, 3), (8, 5)], sqrt(2**2 + 2**2))),
# using a* graph search:
# directly reachable through a single vertex (does not change distance!)
(((5, 1), (3, 3)), ([(5, 1), (4, 2), (3, 3)], sqrt(2**2 + 2**2))),
# If two Polygons have vertices with identical coordinates (this is allowed),
# paths through these vertices are theoretically possible!
(((6.5, 5.5), (7.5, 6.5)), ([(6.5, 5.5), (7, 6),
(7.5, 6.5)], sqrt(1**2 + 1**2))),
# distance should stay the same even if multiple extremities lie on direct path
# test if path is skipping passed extremities
(((8, 4), (8, 8)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8)], 4)),
(((8, 4), (8, 9)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8),
(8, 9)], 5)),
# regular examples
(((0.5, 6), (18.5, 0.5)), ([(0.5, 6.0), (5, 5), (6, 5), (7, 5),
(8, 5), (17, 6), (18, 6),
(18.5, 0.5)], 23.18783787537749)),
(((0.5, 6), (9, 6)), ([(0.5, 6.0), (5, 5), (6, 5), (7, 6), (8, 6),
(9, 6)], 9.023985791019538)),
(((0.5, 6), (18.5, 9)), ([(0.5, 6.0), (5, 5),
(6, 5), (7, 5), (8, 5), (18, 7),
(18.5, 9.0)], 19.869364068640845)),
# 19.86936406864084
(((6.9, 4), (7, 9)), ([(6.9, 4.0), (7, 6), (8, 7), (8, 8),
(7, 9)], 5.830925564196269)),
(((6.5, 4), (7, 9)), ([(6.5, 4.0), (7, 6), (8, 7), (8, 8),
(7, 9)], 5.889979937555021)),
]:
# test if path and distance are correct
# print(input, expected_output, fct(input))
actual_output = environment.find_shortest_path(
start_coordinates, goal_coordinates)
if actual_output != expected_output:
print('input: {} expected: {} got: {}'.format(
(start_coordinates, goal_coordinates), expected_output,
actual_output))
assert actual_output == expected_output
# when the deep copy mechanism works correctly
# even after many queries the internal graph should have the same structure as before
# otherwise the temporarily added vertices during a query stay stored
assert len(environment.graph.all_nodes) == 16
class PathPreProcessor:
def __init__(self, config, plotting = False):
self.config = config
self.inflator = pyclipper.PyclipperOffset()
# Define environment based on wheter or not to plot
if plotting:
self.env = PlottingEnvironment(plotting_dir='plotted_paths')
else:
self.env = PolygonEnvironment()
def prepare(self, graph_map):
self.dyn_obs_list = graph_map.dyn_obs_list.copy()
# Obstacles
self.original_obstacle_list = graph_map.obstacle_list.copy()
# Pre-proccess obstacles
self.processed_obstacle_list = self.preprocess_obstacles(graph_map.obstacle_list)
# Boundary
self.original_boundary_coordinates = graph_map.boundary_coordinates.copy()
# Pre-process boundaries
self.processed_boundary_coordinates = self.preprocess_obstacle(
pyclipper.scale_to_clipper(graph_map.boundary_coordinates),
pyclipper.scale_to_clipper(-self.config.vehicle_width))
# Give obstacles and boundaries to environment
self.env.store(self.processed_boundary_coordinates, self.processed_obstacle_list)
# Prepare the visibility graph
self.env.prepare()
def get_initial_guess(self, start_pos, end_pos):
"""Method that generates the initially guessed path based on obstacles and boundaries specified during creation
Args:
start_pos (tuple): tuple containing x and y position for starting position
end_pos (tuple): tuple containing x and y position for end position
Returns:
path ([tuples]) : a list of coordinates that together becomes the inital path. These points lies on extremities of the padded obstacles
vertices ([tuples]) : a list of the vertices on the original (unpadded) obstacles corresponding to the vertices in path
"""
path, distance = self.env.find_shortest_path(start_pos, end_pos)
vertices = self.find_original_vertices(path)
self.path = path
self.vert = vertices
self.vert_copy = vertices.copy()
return path, vertices
def preprocess_obstacle(self, obstacle, inflation, boundary = False):
self.inflator.Clear()
self.inflator.AddPath(obstacle, pyclipper.JT_MITER, pyclipper.ET_CLOSEDPOLYGON)
inflated_obstacle = pyclipper.scale_from_clipper(self.inflator.Execute(inflation))[0]
return inflated_obstacle
def preprocess_obstacles(self, obstacle_list):
inflation = pyclipper.scale_to_clipper(self.config.vehicle_width)
inflated_obstacles = []
for obs in obstacle_list:
obstacle = pyclipper.scale_to_clipper(obs)
inflated_obstacle = self.preprocess_obstacle(obstacle, inflation, boundary=False)
inflated_obstacle.reverse() # obstacles are ordered clockwise
inflated_obstacles.append(inflated_obstacle)
return inflated_obstacles
@staticmethod
def dist_between_points(p1, p2):
return np.linalg.norm((np.asarray(p1) - np.asarray(p2)), ord = 2)
def get_closest_vert(self, vert, list_to_compare):
#best_vert = ()
#best_dist = 10e3
#best_idx = 0
distances = map(lambda x: self.dist_between_points(vert, x), list_to_compare)
best_idx = np.argmin(list(distances))
'''for idx, ref_vert in enumerate(list_to_compare):
dist = self.dist_between_points(vert, ref_vert)
if dist < best_dist:
best_dist = dist
best_idx = idx
best_vert = ref_vert'''
return list_to_compare[best_idx], best_idx
def find_original_vertices(self, path):
vertices = []
if not (len(path) > 2):
print('[INFO] Path is only one line. No vertices to find.')
return vertices
all_vert = self.original_obstacle_list + [self.original_boundary_coordinates]
all_vert_flat = [item for sublist in all_vert for item in sublist]
for vert in path[1:-1]: # dont use start and final positions as contstraints
closest_vert, _ = self.get_closest_vert(vert, all_vert_flat)
vertices.append(closest_vert)
return vertices
def find_closest_vertices(self, current_pos, n_vertices = 10, N_STEPS_LOOK_BACK = 2):
if n_vertices >= len(self.vert):
return self.vert
_, idx = self.get_closest_vert(current_pos, self.vert)
lb = max(0, idx - N_STEPS_LOOK_BACK) # look two objects behind
ub = min(len(self.vert), n_vertices - N_STEPS_LOOK_BACK)
return self.vert[lb:ub]
###### If one wants to use euclidian distance instead.
# self.vert_copy.sort(key = lambda x: self.dist_between_points(current_pos, x))
# return self.vert.copy()[:n_vertices]
######
@staticmethod
def generate_obstacle(p1, p2, freq, time):
p1 = np.array(p1)
p2 = np.array(p2)
time = np.array(time)
t = abs(np.sin(freq * time))
if type(t) == np.ndarray:
t = np.expand_dims(t,1)
p3 = t*p1 + (1-t)*p2
return p3
@staticmethod
def rotate(origin, point, angle):
ox, oy = origin
px, py = point
qx = math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
qy = math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
return np.array([qx, qy])
def rotate_and_add(self, p1,p2, angle, addition):
rot = self.rotate(p1, p2, angle)
rot[1] += addition
rot = self.rotate(np.array([0,0]), rot, -angle)
return rot + p1
def generate_sinus_obstacle(self, p1, p2, freq, time, ampl=1.5):
p1 = np.array(p1)
p2 = np.array(p2)
dp = p2 - p1
angle = np.arctan2(dp[1],dp[0])
time = np.array(time)
t = abs(np.sin(freq * time))
if type(t) == np.ndarray:
t = np.expand_dims(t,1)
p3 = t*p1 + (1-t)*p2
add = ampl*np.cos(10*freq*time)
return self.rotate_and_add(p1, p3, angle, add)
def get_dyn_obstacle(self, t, horizon, sinus_object=False):
# TODO: allow simulation to start from previously calculated positionss
if len(self.dyn_obs_list) == 0:
return []
time = np.linspace(t, t+horizon*self.config.ts, horizon)
obs_list = []
for i, obs in enumerate(self.dyn_obs_list):
p1, p2, freq, x_radius, y_radius, angle = obs
x_radius = x_radius+self.config.vehicle_width/2+self.config.vehicle_margin
y_radius = y_radius+self.config.vehicle_width/2+self.config.vehicle_margin
if sinus_object and i == 2:
q = [(*self.generate_sinus_obstacle(p1, p2, freq, t), x_radius, y_radius, angle) for t in time]
obs_list.append(q)
else:
obs_list.append([(*self.generate_obstacle(p1, p2, freq, t), x_radius, y_radius, angle) for t in time])
return obs_list
def plot_all(self, ax):
plot_boundaries(self.original_boundary_coordinates, ax, c='k', label = 'Original Boundary')
plot_boundaries(self.processed_boundary_coordinates, ax, c='g', label = 'Padded Boundary')
plot_obstacles(self.original_obstacle_list, c='r', ax = ax, label = 'Original Obstacles')
plot_obstacles(self.processed_obstacle_list,c='y', ax = ax, label = 'Padded Obstacles')
plot_path(self.path, c='--k', ax = ax)
plot_vertices(self.vert, radius = self.config.vehicle_width, ax = ax)