def test_rewire_rand_rtrrt(empty_environment_info):
planner = RTRRTPlanner(empty_environment_info["space"], np.array([10,10]),
assisting_metric=DiffusionMetric(empty_environment_info["diffusion_map"]))
planner.expand()
costs = {}
for node in planner.tree.nodes:
costs[node] = planner.cost(node)
planner.rewire_rand()
for node in planner.tree.nodes:
assert planner.cost(node) <= costs[node]
def test_rewire_goal_amrrt(empty_environment_info):
planner = AMRRTPlanner(empty_environment_info["space"], np.array([10,10]),
assisting_metric=DiffusionMetric(empty_environment_info["diffusion_map"]))
planner.set_goal(np.array([20,20]))
while planner.goal not in planner.tree.nodes:
planner.expand()
costs = {}
for node in planner.tree.nodes:
costs[node] = planner.cost(node)
planner.rewire_goal()
for node in planner.tree.nodes:
assert planner.cost(node) <= costs[node]
def visualiser(space, planner_type, metric_type):
pygame.init()
grid_graph = GridGraph(space)
if metric_type == "euclidean" : assisting_metric = EuclideanMetric()
elif metric_type == "diffusion" : assisting_metric = DiffusionMetric(DiffusionMap(space, grid_graph=grid_graph))
else : assisting_metric = GeodesicMetric(grid_graph)
planner = None
agent_pos = None
image = space.display_image()
image_size = 800
scale = image_size/min(image.size[0],image.size[1])
width, height = int(image.size[0]*scale), int(image.size[1]*scale)
screen = pygame.display.set_mode((width, height))
image = image.resize((width, height), resample=Image.NEAREST)
image = pygame.image.frombuffer(image.tobytes(), image.size, image.mode)
while planner is None:
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
if event.type == pygame.MOUSEBUTTONDOWN:
pos = np.array(pygame.mouse.get_pos()) / scale
if planner_type == "rtrrt" : planner = RTRRTPlanner(space, pos, assisting_metric=assisting_metric)
else : planner = AMRRTPlanner(space, pos, assisting_metric=assisting_metric)
agent_pos = pos
screen.blit(image, (0, 0))
pygame.display.update()
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
if event.type == pygame.MOUSEBUTTONDOWN:
pos = np.array(pygame.mouse.get_pos()) / scale
if pygame.key.get_pressed()[pygame.K_d]:
planner.add_dynamic_obstacle(pos, 3)
else:
planner.set_goal(pos)
path = []
waypoint = planner.plan(agent_pos).pos.copy()
path = planner.goal_path()
agent_pos += min(1, 0.7/np.linalg.norm(waypoint-agent_pos)) * (waypoint-agent_pos)
display(screen, planner, agent_pos, path, image, scale)
def test_distance_diffusion(maze_environment_info):
space = maze_environment_info["space"]
a = space.create_state(np.array([10, 10]))
b = space.create_state(np.array([20, 20]))
diffusion_metric = DiffusionMetric(maze_environment_info["diffusion_map"])
assert diffusion_metric.distance(a, b) == diffusion_metric.distance(b, a)