예제 #1
0
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
예제 #2
0
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
예제 #3
0
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)
예제 #4
0
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
예제 #5
0
파일: prm_star.py 프로젝트: veds12/gennav
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
예제 #6
0
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
예제 #7
0
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)
예제 #8
0
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
예제 #9
0
파일: rrg.py 프로젝트: veds12/gennav
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