Skip to content

Instantly share code, notes, and snippets.

@blzzua

blzzua/robot_navigation.py

Last active March 31, 2021 10:58
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save blzzua/e5717110d6c42f4cd3743187bcbeaf4d to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save blzzua/e5717110d6c42f4cd3743187bcbeaf4d to your computer and use it in GitHub Desktop.
Download ZIP
Raw
robot_navigation.py
# to run this module from command line typing
#
# > python robot_navigation.py robot_data.json
#
# it reads the starting and the finishing points as well as
# the list of obstacles from robot_data.json file and then
# runs find_path() function that has to return the path
#
import sys
import numpy as np
import json
### these functions may help you to check if your polyline does not intersect the obstacles
def check_polyline(polyline, obstacles):
"""this function returns True if the polyline does not intersect obstacles
Otherwise it returns False
You can use it to verify your algorithm
"""
for obstacle in obstacles:
for i in range(len(obstacle)):
obstacle_segment = (obstacle[i-1], obstacle[i])
for j in range(1, len(polyline)):
path_segment = (polyline[j-1], polyline[j])
if is_segments_intersect(obstacle_segment, path_segment):
print("segments intersect:", obstacle_segment, path_segment)
return False
return True
def is_segments_intersect(seg_1, seg_2):
## let's find two line coming through the two points of each segment
## v = a * v1 + (1 - a) * v2
## u = b * u1 + (1 - b) * u2
## lines intersect at u = v, => a * v1 + (1 - a) * v2 = b * u1 + (1 - b) * u2
## or (v1 - v2) * a + (u2 - u1) * b = u2 - v2
##
## if lines intersect within the given segments, a and b must be strictly between 0 and 1
v1, v2 = seg_1
u1, u2 = seg_2
M = np.array([v1 - v2, u2 - u1]).T
if np.linalg.matrix_rank(M) < 2:
return False
a, b = np.linalg.inv(M).dot(u2 - v2)
return (0 < a < 1) and (0 < b < 1)
def find_path(start, finish, obstacles=[]):
start = np.array(start)
finish = np.array(finish)
obstacles = [ np.array(ob) for ob in obstacles]
print("Start:", start)
print("Finish point:", finish)
print("Obstacles:", obstacles)
## YOUR CODE STARTS HERE #######################
def get_intercept_point(a,b):
if not is_segments_intersect(a,b):
return None
"a,b - lists of end-points of 2d-arrays"
# алгоритм из http://algolist.ru/maths/geom/intersect/lineline2d.php
(x1,y1),(x2,y2) = a
(x3,y3),(x4,y4) = b
denom = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1)
ua = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3)) / denom
#ub = ((x2-x1)*(x1-y3) - (y2-y1)*(x1-x3)) / denom
intercept = np.array((x1+ ua*(x2-x1), y1+ua*(y2-y1)))
return intercept
def around(from_, to_, n, direction = 1):
if direction not in (1,-1):
# print аргумент - направление обхода direction должен быть 1 или -1
return []
around_list = []
i=from_
c = 100 # workaround circutbreaker against infinite loop
if direction == -1 and from_ == to_:
#workaround
around_list = (list(range(from_,n,1)) + list(range(0,from_,1)) + [from_,])[::-1]
return around_list
while True:
if c <= 0:
break
c = c-1
around_list.append(i)
if i == to_:
break
else:
i = i + direction
if i == n:
i = 0
elif i <= 0:
i = n-1
return around_list
# реализация 4
path = [start, finish]
gap = 0.0001
cur_path_num = 0
flat_obtacles = []
for i, obstacle in enumerate(obstacles):
for obt_i in range(len(obstacle)):
obstacle_segment = (obstacle[obt_i-1], obstacle[obt_i])
flat_obtacles.append((i, obstacle_segment))
while cur_path_num < len(path)-1:
cur_path_segm = [path[cur_path_num], path[cur_path_num+1]]
#print(f"cur_path_num = {cur_path_num}, len =", len(path) )
closest_obt=dict()
closest_obt['point'], closest_obt['point'] = path[cur_path_num+1]
crosses = []
closest_obtacle = -1
need_get_around = False
direction = cur_path_segm
for i, obstacle_segment in flat_obtacles:
#print ("прооверяем ", i, "и", i-1," точками. в точке", cross_point, "на расстоянии", distance_from_begin )
if is_segments_intersect(direction, obstacle_segment):
need_get_around = True
cross_point = get_intercept_point(direction, obstacle_segment)
distance_from_begin = np.linalg.norm(cross_point-direction[0])
crosses.append((i, obstacle_segment, distance_from_begin))
if distance_from_begin < np.linalg.norm(closest_obt['point']-direction[0]):
closest_obt['point'] = cross_point
closest_obtacle=i
# print (cur_path_segm,"пересекается c ", i, " obtacle в точке", cross_point, "на расстоянии", distance_from_begin )
if need_get_around:
# вычисляем между какими сегментами препятствия необходимо совершить обход, и в каких точках
obstacle = obstacles[closest_obtacle]
first_cross=dict()
last_cross=dict()
first_cross['point'], last_cross['point'] = direction[::-1]
for j, seg in enumerate(obstacle):
obstacle_segment = (obstacle[j-1], obstacle[j])
if is_segments_intersect(direction, obstacle_segment):
cross_point = get_intercept_point(direction, obstacle_segment)
distance_from_begin = np.linalg.norm(cross_point-direction[0])
if distance_from_begin < np.linalg.norm(first_cross['point']-direction[0]):
first_cross['point'] = cross_point
first_cross['v1']=j-1
first_cross['v2']=j
if distance_from_begin > np.linalg.norm(last_cross['point']-direction[0]):
last_cross['point'] = cross_point
last_cross['v1']=j-1
last_cross['v2']=j
#print(f"обходим препятствие {obstacle} по периметру от точки {first_cross} до точки {last_cross}")
if first_cross['v2'] <= last_cross['v1']:
#print(f"обход по варианту1:")
direction1 = around(first_cross['v1'], last_cross['v2'], len(obstacle), direction = 1 )
direction2 = around(first_cross['v2'], last_cross['v1'], len(obstacle), direction = -1)
if first_cross['v2'] >= last_cross['v1']:
#print(f"обход по варианту2:")
direction1 = around(first_cross['v1'], last_cross['v2'], len(obstacle), direction = -1 )
direction2 = around(first_cross['v2'], last_cross['v1'], len(obstacle), direction = 1)
# установка зазора чтобы не происходила ошибка округления переносяшая точку внутрь препятствия
first_cross['point'] = first_cross['point'] - (first_cross['point'] - direction[0])/np.linalg.norm(first_cross['point'] - direction[0]) * gap
last_cross['point'] = last_cross['point'] - (last_cross['point'] - direction[1])/np.linalg.norm(last_cross['point'] - direction[1]) * gap
#print(f"строим обход вдоль {direction2}")
path1 = [first_cross['point']]
for i in direction1:
path1.append(obstacle[i])
path1.append(last_cross['point'])
path2 = [first_cross['point']]
for i in direction2:
path2.append(obstacle[i])
path2.append(last_cross['point'])
# тут бы разумно выбрать один из вариантов, который покороче.
sump1 = sum([np.linalg.norm(path1[_+1]-path1[_] ) for _ in range( len( path1 )-1) ])
sump2 = sum([np.linalg.norm(path2[_+1]-path2[_] ) for _ in range( len( path2 )-1) ])
if sump1 > sump2:
around_path = path2[:]
else:
around_path = path1[:]
path = path[:cur_path_num+1] + around_path + path[cur_path_num+1:]
cur_path_num = cur_path_num + len(around_path)
#print(f"path увеличен на {around_path} поменял path")
else:
cur_path_num = cur_path_num + 1
if cur_path_num > 30:
#circuitbreak
break
##################################################
return path
if __name__ == '__main__':
if len(sys.argv) != 2:
print("USAGE EXAMPLE:\n\n python robot_navigation.py robot_data.json\n")
exit(1)
data_file = sys.argv[1]
f = open(data_file)
data = json.load(f)
f.close()
find_path(data["start"], data["finish"], data["obstacles"])
Raw
robot_navigation_potiental_field.py
# to run this module from command line typing
#
# > python robot_navigation.py robot_data.json
#
# it reads the starting and the finishing points as well as
# the list of obstacles from robot_data.json file and then
# runs find_path() function that has to return the path
#
import sys
import numpy as np
import json
global center_mass_list
def normalize(v):
norm = np.linalg.norm(v)
if norm == 0:
return v
return v / norm
def quiet_check_polyline(polyline, obstacles):
"тот-же check_polyline только не принтует. используется для работы алгоритма."
for obstacle in obstacles:
for i in range(len(obstacle)):
obstacle_segment = (obstacle[i-1], obstacle[i])
for j in range(1, len(polyline)):
path_segment = (polyline[j-1], polyline[j])
if is_segments_intersect(obstacle_segment, path_segment):
return False
return True
def center_mass(data):
epsilon = 1 * 10**-4
metric = lambda v: np.linalg.norm(v)
x = data.mean(axis=0)
while True:
prev_x = x
denom = sum(1/metric(v - x) for v in data)
x = sum(v/metric(v - x) for v in data)/denom
if ( metric(prev_x - x) < epsilon ):
break
return x
def get_intercept_point(a,b):
"""Возвращает точку пересечения двух отрезков. аргументами дожны быть 2 списка, каждый из двух координат. """
if not is_segments_intersect(a,b):
return None
"a,b - lists of end-points of 2d-arrays"
# алгоритм из http://algolist.ru/maths/geom/intersect/lineline2d.php
(x1,y1),(x2,y2) = a
(x3,y3),(x4,y4) = b
denom = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1)
ua = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3)) / denom
#ub = ((x2-x1)*(x1-y3) - (y2-y1)*(x1-x3)) / denom
intercept = np.array((x1+ ua*(x2-x1), y1+ua*(y2-y1)))
return intercept
def find_path(start, finish, obstacles=[]):
obstacles = list(map(np.array, obstacles))
center_masses = {}
for ob in obstacles:
center = center_mass(ob)
str_ob = str(ob)
center_masses[str_ob] = center
def get_distance_for_obtacle(point, ob):
"""Возвращает расстояние от точки до препятствия. препятствие - список координат"""
#center = center_mass(ob)
nonlocal center_masses
center = center_masses[str(ob)]
for i in range(len(ob)):
segment = (ob[i-1], ob[i])
cross = get_intercept_point((point, center),segment)
if not cross is None :
return np.linalg.norm(cross-point)
print("ошибка. точка внутри препятствия", point, ob)
return 10**-10
def get_vector_from_obtacle(point, ob):
"возвращает вектор от центра масс препятствия. до точки пересечения. "
#center = center_mass(ob)
nonlocal center_masses
center = center_masses[str(ob)]
for i in range(len(ob)):
segment = (ob[i-1], ob[i])
cross = get_intercept_point((point, center),segment)
if not cross is None :
return point - cross
start = np.array(start)
finish = np.array(finish)
obstacles = [ np.array(ob) for ob in obstacles]
begin, end = start, finish
cur_point_list = [begin]
cur_point = begin
step = np.linalg.norm(end - begin)/20 # выбор и изменение длины шага.
min_step_coef = np.linalg.norm(end - begin)/100*(1+len(obstacles)) # минимальный шаг. коэфициент для тюнинга
max_step_coef = np.linalg.norm(end - begin)/2*(1+len(obstacles)) # максимальный шаг. коэфициент для тюнинга
circuit_breaker = 0
breath_factor = False
while circuit_breaker < 5000: # аварийный выход
circuit_breaker += 1
# сердце алгоритма
repulsion = [get_vector_from_obtacle(cur_point,ob) / (get_distance_for_obtacle(cur_point,ob) ** (4 if breath_factor else 2) )
for ob in obstacles]
#attractor = ( end - cur_point ) / (1 + np.linalg.norm(end - cur_point) )
attractor = ( end - cur_point )
directon = np.array(repulsion).sum(axis=0) + attractor
next_point = cur_point + normalize(directon) * step
if step < min_step_coef:
breath_factor = True
else:
breath_factor = False
if quiet_check_polyline([cur_point, next_point ], obstacles):
print(f"{circuit_breaker}: next_point = {next_point}" , f"step ={step} breath_factor={breath_factor}")
cur_point_list.append(next_point)
step = max(min(step * 2, max_step_coef), min_step_coef)
cur_point = next_point
else:
step = step / 2
continue
if quiet_check_polyline([cur_point,end], obstacles):
cur_point_list.append(end)
break
return cur_point_list
print("Start:", start)
print("Finish point:", finish)
print("Obstacles:", obstacles)
## YOUR CODE STARTS HERE #######################
# find polyline that do not intersect any obstacle
# and return it
## the list of vectors you have to return. It should contain the points that the robot
# has to follow avoiding the obstacles.
polyline = [start, finish]
##################################################
return polyline
### these functions may help you to check if your polyline does not intersect the obstacles
def check_polyline(polyline, obstacles):
"""this function returns True if the polyline does not intersect obstacles
Otherwise it returns False
You can use it to verify your algorithm
"""
for obstacle in obstacles:
for i in range(len(obstacle)):
obstacle_segment = (obstacle[i-1], obstacle[i])
for j in range(1, len(polyline)):
path_segment = (polyline[j-1], polyline[j])
if is_segments_intersect(obstacle_segment, path_segment):
print("segments intersect:", obstacle_segment, path_segment)
return False
return True
def is_segments_intersect(seg_1, seg_2):
## let's find two line coming through the two points of each segment
## v = a * v1 + (1 - a) * v2
## u = b * u1 + (1 - b) * u2
## lines intersect at u = v, => a * v1 + (1 - a) * v2 = b * u1 + (1 - b) * u2
## or (v1 - v2) * a + (u2 - u1) * b = u2 - v2
##
## if lines intersect within the given segments, a and b must be strictly between 0 and 1
v1, v2 = seg_1
u1, u2 = seg_2
M = np.array([v1 - v2, u2 - u1]).T
if np.linalg.matrix_rank(M) < 2:
return False
a, b = np.linalg.inv(M).dot(u2 - v2)
return (0 < a < 1) and (0 < b < 1)
## ORIGINAL MAIN # if __name__ == '__main__':
## ORIGINAL MAIN # if len(sys.argv) != 2:
## ORIGINAL MAIN # print("USAGE EXAMPLE:\n\n python robot_navigation.py robot_data.json\n")
## ORIGINAL MAIN # exit(1)
## ORIGINAL MAIN #
## ORIGINAL MAIN # data_file = sys.argv[1]
## ORIGINAL MAIN # f = open(data_file)
## ORIGINAL MAIN # data = json.load(f)
## ORIGINAL MAIN # f.close()
## ORIGINAL MAIN #
## ORIGINAL MAIN # find_path(data["start"], data["finish"], data["obstacles"])
if __name__ == '__main__':
if len(sys.argv) != 2:
print("USAGE EXAMPLE:\n\n python robot_navigation.py robot_data.json\n")
exit(1)
data_file = sys.argv[1]
f = open(data_file)
data = json.load(f)
f.close()
path = find_path(data["start"], data["finish"], data["obstacles"])
from matplotlib import pyplot as plt
plt.figure(figsize=(10,10))
plt.style.use('dark_background')
plt.axis('equal')
obstacles = list(map(np.array, data["obstacles"]))
for ob in obstacles:
ob = list(ob)
ob.append(ob[0])
x,y = zip(*ob)
plt.plot(x, y, c='red', linewidth=3)
x1, x2 = data["start"]
plt.scatter(x1, x2, c='green', s=5)
x1, x2 = data["finish"]
plt.scatter(x1, x2, c='green', s=5)
x1, x2 = np.array(path).T
plt.plot(x1, x2, c='yellow', linewidth=1, linestyle=':')
plt.show()
print (path)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment