# possible_euler_pts = euler.euler_method()
possible_pts = runge_kutta.runge_kutta()
# print "straight , left , Right ",possible_euler_pts
# finds the nearest of the three points to the random point
newpt = nearest(possible_pts, randompt)
#Collision check whether the newpoint generated lies inside any obstacle or not
if polygons.contains(Point((newpt[0], newpt[1]))):
continue
#Check for new point not crossing the boundary of environment
if newpt[0] > 100 or newpt[0] < 0 or newpt[1] < 0 or newpt[1] > 100:
continue
#generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment
line = [(x_y[0], x_y[1]), (newpt[0], newpt[1])]
if polygons.crosses(LineString(line)):
continue
lines.append(line)
# color.append((0, 1, 1, 1))
# Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt"
graph.addEdge(index1, x_y, index2, newpt)
index1 += 2
index2 += 2
# Appends the newpt(new point) generated in each iteration to the "pts" (points) list.
pts.append(newpt)
# print(pts)
# print graph.vertexMap
# calls the shortest path method using the instance of the class Graph by passing source point as an argument. This finds the short routes from source to every other point in the tree
graph.shortestPath(source)
# possible_euler_pts = euler.euler_method()
possible_pts = runge_kutta.runge_kutta()
# print "straight , left , Right ",possible_euler_pts
# finds the nearest of the three points to the random point
newpt = nearest(possible_pts,randompt)
#Collision check whether the newpoint generated lies inside any obstacle or not
if polygons.contains(Point((newpt[0],newpt[1]))):
continue
#Check for new point not crossing the boundary of environment
if newpt[0]>100 or newpt[0]<0 or newpt[1]<0 or newpt[1]>100:
continue
#generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment
line = [(x_y[0],x_y[1]),(newpt[0],newpt[1])]
if polygons.crosses(LineString(line)):
continue
lines.append(line)
# color.append((0, 1, 1, 1))
# Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt"
graph.addEdge(index1,x_y,index2,newpt)
index1 += 2
index2 += 2
# Appends the newpt(new point) generated in each iteration to the "pts" (points) list.
pts.append(newpt)
# print(pts)
# print graph.vertexMap
# calls the shortest path method using the instance of the class Graph by passing source point as an argument. This finds the short routes from source to every other point in the tree
class Environment:
def __init__(self, input_file, factor=1):
self.plot_obstacles_polygon = []
self.obs_list = []
self.obs_polygon = MultiPolygon() # Shapely object to store all polygons
self.initial_state, self.goal_state = [], []
self.resolution = 0 # Dimension of the plane.
self.read_env_from_file(input_file)
def read_env_from_file(self, input_file):
# Read json input
try:
print(input_file)
with open(input_file, mode='r', encoding='utf-8') as a_file:
environment = json.loads(a_file.read())
except FileNotFoundError as fl:
print("File not found for JSON ", fl)
exit(1)
except ValueError:
print("Invalid JSON")
exit(1)
except Exception:
print("Unable to process input file")
exit(1)
try:
# Making sure the required entities are defined in the input json file.
environment['resolution'] and environment['obstacles']
environment['initial_state'] and environment['goal_state']
except KeyError:
print("Invalid Environment definition")
exit(1)
self.initial_state, self.goal_state = environment['initial_state'], environment['goal_state']
self.resolution = environment['resolution']
temp_polygon_list = []
for obs in environment['obstacles']:
if not obs.get('shape') and obs.get('property') and obs['property'].get('vertices'):
print("Shape element not present for the obstacles")
continue
if obs['shape'] == 'polygon':
# print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100))
polygon = mPolygon(np.array(obs['property']['vertices']))
temp_polygon_list.append(Polygon(obs['property']['vertices']))
self.plot_obstacles_polygon.append(polygon)
self.obs_list.append(obs['property']['vertices'])
else:
print("Undefined shape")
break
self.obs_polygon = MultiPolygon(temp_polygon_list)
def is_point_inside(self, xy):
"""
:param xy: tuple with x coordinate as first element and y coordinate as second element
:return: True if the point is inside the obstacles and False if it isn't
"""
return Point(xy[0], xy[1]).within(self.obs_polygon)
def is_line_inside(self, xy_start, xy_end):
# xy_start is tuple of (x, y) coordinate of one end of the line.
# xy_end is tuple of (x, y) coordinate of the other end of line.
line = LineString([xy_start, xy_end])
return self.obs_polygon.contains(line) or self.obs_polygon.touches(line) or self.obs_polygon.crosses(line)
def draw_env(self, path, key_xy, k_value):
# Method to draw an arrow in the environment
# path is a list of state objects. index 0 maps from-state and index 1 maps to-state.
fig, ax = plt.subplots()
x_path, y_path = [], []
for ls in path:
x_path.append(key_xy(ls)[0])
y_path.append(key_xy(ls)[1])
colors = 100*np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
plt.arrow(x_path[0], y_path[0], x_path[1]-x_path[0], y_path[1]-y_path[0], fc="k", ec="k", head_width=1.55, head_length=1.1)
plt.title("figure" + str(k_value)+".png")
fig.savefig("figure" + str(k_value)+".png", format='png', dpi=fig.dpi)
def animate_path(self, path, key_xy):
fig, ax = plt.subplots()
colors = 100*np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1]
x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1]
dx, dy = x_1 - x_0, y_0 - y_1
qv = ax.quiver(x_0, y_0, dx, dy, angles='xy',scale_units='xy',scale=1)
def animate(i):
x_init, y_init =key_xy(path[i])[0], key_xy(path[i])[1]
x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1]
dx, dy = x_f - x_init, y_f - y_init
qv.set_UVC(np.array(dx), np.array(dy))
qv.set_offsets((x_init, y_init))
return qv
anim = animation.FuncAnimation(fig, animate, frames=range(0, len(path)-1), interval=500)
plt.show()
def get_apprx_visible_vertices(self, xy_robot):
# To get visible vertices from robot point
# xy_robot should be a tuple of (x, y) coordinate
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, visible_lines = [], []
for obj in pool:
for vertex in obj:
vertex = tuple(vertex)
if vertex == xy_robot:
continue
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
visible_lines.append(line)
visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in visible_lines)
return visible_vertices
def get_actual_visible_vertices(self, xy_robot):
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, line_robot_vertices = [], {}
def line_slope(xy1, xy2):
return (xy2[1] - xy1[1])/(xy2[0] - xy1[0]) if (xy2[0] - xy1[0]) != 0 else sys.maxsize
for obj in pool:
for vertex in obj:
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
if line_slope(xy_robot, vertex) in line_robot_vertices:
if line.length < line_robot_vertices[line_slope(xy_robot, vertex)].length:
line_robot_vertices[line_slope(xy_robot, vertex)] = line
else:
line_robot_vertices[line_slope(xy_robot, vertex)] = line
visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in line_robot_vertices.values())
return visible_vertices
def visibility_line(self, xy_start, xy_end):
# Helper Method to check if the line is intersected by any obstacles.
line = LineString([xy_start, xy_end])
return self.obs_polygon.crosses(line) or self.obs_polygon.contains(line), line
def __str__(self):
return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \
% ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)
class Environment:
def __init__(self, input_file, factor=1):
self.plot_obstacles_polygon = []
self.obs_list = []
self.obs_polygon = MultiPolygon(
) # Shapely object to store all polygons
self.initial_state, self.goal_state = [], []
self.resolution = 0 # Dimension of the plane.
self.read_env_from_file(input_file)
def read_env_from_file(self, input_file):
# Read json input
try:
print(input_file)
with open(input_file, mode='r', encoding='utf-8') as a_file:
environment = json.loads(a_file.read())
except FileNotFoundError as fl:
print("File not found for JSON ", fl)
exit(1)
except ValueError:
print("Invalid JSON")
exit(1)
except Exception:
print("Unable to process input file")
exit(1)
try:
# Making sure the required entities are defined in the input json file.
environment['resolution'] and environment['obstacles']
environment['initial_state'] and environment['goal_state']
except KeyError:
print("Invalid Environment definition")
exit(1)
self.initial_state, self.goal_state = environment[
'initial_state'], environment['goal_state']
self.resolution = environment['resolution']
temp_polygon_list = []
for obs in environment['obstacles']:
if not obs.get('shape') and obs.get(
'property') and obs['property'].get('vertices'):
print("Shape element not present for the obstacles")
continue
if obs['shape'] == 'polygon':
# print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100))
polygon = mPolygon(np.array(obs['property']['vertices']))
temp_polygon_list.append(Polygon(obs['property']['vertices']))
self.plot_obstacles_polygon.append(polygon)
self.obs_list.append(obs['property']['vertices'])
else:
print("Undefined shape")
break
self.obs_polygon = MultiPolygon(temp_polygon_list)
def is_point_inside(self, xy):
"""
:param xy: tuple with x coordinate as first element and y coordinate as second element
:return: True if the point is inside the obstacles and False if it isn't
"""
return Point(xy[0], xy[1]).within(self.obs_polygon)
def is_line_inside(self, xy_start, xy_end):
# xy_start is tuple of (x, y) coordinate of one end of the line.
# xy_end is tuple of (x, y) coordinate of the other end of line.
line = LineString([xy_start, xy_end])
return self.obs_polygon.contains(line) or self.obs_polygon.touches(
line) or self.obs_polygon.crosses(line)
def draw_env(self, path, key_xy, k_value):
# Method to draw an arrow in the environment
# path is a list of state objects. index 0 maps from-state and index 1 maps to-state.
fig, ax = plt.subplots()
x_path, y_path = [], []
for ls in path:
x_path.append(key_xy(ls)[0])
y_path.append(key_xy(ls)[1])
colors = 100 * np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon,
cmap=matplotlib.cm.jet,
alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs',
self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
plt.arrow(x_path[0],
y_path[0],
x_path[1] - x_path[0],
y_path[1] - y_path[0],
fc="k",
ec="k",
head_width=1.55,
head_length=1.1)
plt.title("figure" + str(k_value) + ".png")
fig.savefig("figure" + str(k_value) + ".png",
format='png',
dpi=fig.dpi)
def animate_path(self, path, key_xy):
fig, ax = plt.subplots()
colors = 100 * np.random.rand(len(self.plot_obstacles_polygon))
p = PatchCollection(self.plot_obstacles_polygon,
cmap=matplotlib.cm.jet,
alpha=0.4)
p.set_array(np.array(colors))
ax.add_collection(p)
plt.colorbar(p)
plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs',
self.goal_state[0], self.goal_state[1], 'g^')
plt.axis([0, self.resolution, 0, self.resolution])
x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1]
x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1]
dx, dy = x_1 - x_0, y_0 - y_1
qv = ax.quiver(x_0,
y_0,
dx,
dy,
angles='xy',
scale_units='xy',
scale=1)
def animate(i):
x_init, y_init = key_xy(path[i])[0], key_xy(path[i])[1]
x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1]
dx, dy = x_f - x_init, y_f - y_init
qv.set_UVC(np.array(dx), np.array(dy))
qv.set_offsets((x_init, y_init))
return qv
anim = animation.FuncAnimation(fig,
animate,
frames=range(0,
len(path) - 1),
interval=500)
plt.show()
def get_apprx_visible_vertices(self, xy_robot):
# To get visible vertices from robot point
# xy_robot should be a tuple of (x, y) coordinate
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, visible_lines = [], []
for obj in pool:
for vertex in obj:
vertex = tuple(vertex)
if vertex == xy_robot:
continue
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
visible_lines.append(line)
visible_vertices.extend([x.xy[0][1], x.xy[1][1]]
for x in visible_lines)
return visible_vertices
def get_actual_visible_vertices(self, xy_robot):
if self.is_point_inside(xy_robot):
print("Invalid robot position")
return None
pool = copy.deepcopy(self.obs_list)
pool.append([self.goal_state])
visible_vertices, line_robot_vertices = [], {}
def line_slope(xy1, xy2):
return (xy2[1] - xy1[1]) / (xy2[0] - xy1[0]) if (
xy2[0] - xy1[0]) != 0 else sys.maxsize
for obj in pool:
for vertex in obj:
crosses, line = self.visibility_line(xy_robot, vertex)
if not crosses:
if line_slope(xy_robot, vertex) in line_robot_vertices:
if line.length < line_robot_vertices[line_slope(
xy_robot, vertex)].length:
line_robot_vertices[line_slope(xy_robot,
vertex)] = line
else:
line_robot_vertices[line_slope(xy_robot,
vertex)] = line
visible_vertices.extend([x.xy[0][1], x.xy[1][1]]
for x in line_robot_vertices.values())
return visible_vertices
def visibility_line(self, xy_start, xy_end):
# Helper Method to check if the line is intersected by any obstacles.
line = LineString([xy_start, xy_end])
return self.obs_polygon.crosses(line) or self.obs_polygon.contains(
line), line
def __str__(self):
return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \
% ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)
