def test_rrtstar():
general_obstacles_list = [
[
(1, 1),
(2, 1),
(2, 2),
(1, 2),
],
[(3, 4), (4, 4), (4, 5), (3, 5)],
]
poly = PolygonEnv(buffer_dist=0.1)
poly.update(general_obstacles_list)
sampler = UniformRectSampler(0, 6, 0, 6)
my_tree = RRTstar(
sampler=sampler,
expand_dis=0.1,
neighbourhood_radius=0.5,
goal_distance=0.5,
max_iter=2000,
)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(5, 5))
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_prm_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.1)
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = PRM(sampler=sampler, r=5, n=100)
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
if len(path.path) != 1:
assert poly.get_traj_status(path) is True
def test_dstar():
obstacles = [[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)]]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_tree = DStar(sampler=sampler, r=3, n=75)
poly.update(obstacles)
path = my_tree.plan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path, poly)
obstacles = [
[(8, 5), (7, 8), (2, 9), (3, 5)],
[(3, 3), (3, 5), (5, 5), (5, 3)],
[(10, 8), (12, 8), (11, 6)],
]
# obstacles = [[(10, 7), (9, 10), (4, 11), (5, 7)], [(5, 5), (5, 7), (7, 7), (7, 5)],[(9,8),(13,8),(11,5)]]
poly.update(obstacles)
path_new = my_tree.replan(start, goal, poly)
from gennav.envs.common import visualize_path
visualize_path(path_new, poly)
def test_rrt():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
poly = PolygonEnv(buffer_dist=0.5)
my_tree = RRT(sampler=sampler, expand_dis=0.1)
start = RobotState(position=Point(1, 1))
goal = RobotState(position=Point(10, 10))
for obstacles in general_obstacles_list:
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly)
# from gennav.utils.visualisation import visualize_node
# node_list = _['node_list']
# visualize_node(node_list, poly)
# from gennav.envs.common import visualize_path
# visualize_path(path, poly)
assert poly.get_traj_status(path) is True
def prmstar_plan():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
polygon = PolygonEnv()
start = RobotState(position=Point(0, 0))
goal = RobotState(position=Point(12, 10))
my_prmstar = PRMStar(sampler=sampler, c=30, n=100)
# Plan a path using the PRMStar.plan method for each obstacle:
for obstacles in general_obstacles_list:
polygon.update(obstacles) # updates the environment with the obstacle
path = my_prmstar.plan(start, goal, polygon)
visualize_path(path, polygon)
if len(path.path) != 1: # check if the path has only the start state
assert polygon.get_traj_status(path) is True
def test_uniform_rect_sampler():
min_x, max_x, min_y, max_y = -5, -5, 15, 15
sampler = UniformRectSampler(min_x, max_x, min_y, max_y)
for _ in range(100):
s = sampler()
p = s.position
assert min_x <= p.x and p.x <= max_x and min_y <= p.y and p.y <= max_y
def prm_construct():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, 15, -5, 15)
polygon = PolygonEnv()
my_prm = PRM(sampler=sampler, r=5, n=50)
# Construct a graph in the configuration space of each obstacle using the PRM.construct method:
for obstacles in general_obstacles_list:
polygon.update(obstacles) # updates the environment with the obstacle
graph = my_prm.construct(polygon) # noqa: F841
visualize_graph(graph, polygon)
def test_prm_construct():
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
sampler = UniformRectSampler(-5, -5, 15, 15)
poly = PolygonEnv()
my_tree = PRM(sampler=sampler, r=5, n=50)
for obstacles in general_obstacles_list:
poly.update(obstacles)
graph = my_tree.construct(poly) # noqa: F841
from gennav.utils.geometry import Point # parameter to Robostate
from gennav.utils.samplers import ( # importing sampler class for sampling points
UniformRectSampler, )
general_obstacles_list = [
[[(8, 5), (7, 8), (2, 9), (3, 5)], [(3, 3), (3, 5), (5, 5), (5, 3)]],
[
[(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)],
[(4, 0), (4, 5), (5, 5), (5, 0)],
[(8, 2), (8, 7), (10, 7), (10, 2)],
],
]
# A list of obstacles for the planner.
sampler = UniformRectSampler(-5, 15, -5, 15) # for uniformly sampling points
poly = PolygonEnv(
buffer_dist=1.0
) # intializing the environment object needed for updating the obstacles of the environment
my_tree = RRG(sampler=sampler, expand_dis=1.0,
max_iter=500) # initializing RRG planner object
start = RobotState(position=Point(0, 0)) # setting start point to (0,0)
goal = RobotState(position=Point(10, 10)) # setting end point to (0,0)
for obstacles in general_obstacles_list: # updating the environment with all obstacles
poly.update(obstacles)
path, _ = my_tree.plan(start, goal, poly) # planning the path