def construct_approximate_reeb_graph(environment, grid_x, grid_y):
"""Returns columns of points on a grid corresponding to the contracted level sets for a Reeb Graph.
:param environment: Environment to construct Reeb Graph of
:param grid_x: number of grid points in x direction
:param grid_y: number of grid points in y direction
:return: List[List[Node]]
"""
min_x, max_x, min_y, max_y = environment.bounds()
eps = 0.01 * environment.width
min_x += eps
min_y += eps
max_x -= eps
max_y -= eps
draw_problem_configuration(environment, None, None, None, draw_robot=False)
step_y = (max_y - min_y) / grid_y
columns = []
for x in np.linspace(min_x, max_x, grid_x):
current_column = []
y = min_y
while y < max_y:
start_y = y
while environment.clear_coords(x, y) and y < max_y:
y += step_y
end_y = y - step_y
if environment.clear_coords(
x, start_y) and environment.clear_coords(x, end_y):
current_column.append(
Node(np.array([x, (start_y + end_y) / 2])))
while not environment.clear_coords(x, y) and y < max_y:
y += step_y
columns.append(current_column)
redundant_columns = set()
for i in range(len(columns) - 1):
if {n.configuration[1]
for n in columns[i]
} == {n.configuration[1]
for n in columns[i + 1]}:
redundant_columns.add(i + 1)
columns = [
columns[i] for i in range(len(columns))
if i not in redundant_columns and len(columns[i]) > 0
]
return columns
from ProblemConfigurations.UniqueSolutionMaze import generate_maze_problem, generate_maze_reeb_graph
import matplotlib.pyplot as plt
from Drawing import draw_problem_configuration
from DRRRT import augment_reeb_graph
environment, robot, start, goal = generate_maze_problem()
reeb_graph = generate_maze_reeb_graph()
draw_problem_configuration(environment, robot, start, goal, draw_robot=False)
reeb_graph.draw("green")
plt.show()
reeb_graph, start_node, goal_node = augment_reeb_graph(
environment,
start,
goal,
link_step_size=0.001 * environment.width,
reeb_graph=reeb_graph,
connection_distance_threshold=0.15 * environment.width)
# reeb_graph = prune_reeb_graph(reeb_graph, goal_node)
draw_problem_configuration(environment, robot, start, goal, draw_robot=False)
reeb_graph.draw('green')
plt.show()
from performance_tests import generate_problem
from Drawing import draw_problem_configuration
import matplotlib.pyplot as plt
for name in ['barriers', 'hallway', 'narrow', 'split', 'maze']:
environment, robot, start, goal = generate_problem(name)
plt.close()
draw_problem_configuration(environment, robot, start, goal, title='')
plt.savefig("{}_problem.pdf".format(name))