Ejemplo n.º 1
0
def rrt_test():
    cons = Box(np.array([[0, 10], [0, 10]]))
    x_init = np.array([0, 0])
    goal = Box(np.array([[9, 9.5], [9, 9.5]]))
    rrt = RRT(cons, [], 1)
    t, end = rrt.build_tree(x_init, goal)
    nt.assert_true(goal.contains(end.node))
Ejemplo n.º 2
0
def rrt_obs_test():
    cons = Box(np.array([[0, 10], [0, 10]]))
    x_init = np.array([0, 0])
    goal = Box(np.array([[9, 9.5], [9, 9.5]]))
    obstacles = [Polytope(np.array([[1, 1,1], [1, 1,2.5], [1, 2.5,2.5], [1, 2.5,1]]), False),
                 Polytope(np.array([[1, 4,6], [1, 5,6], [1, 5,7], [1, 4,7]]), False)]
    rrt = RRT(cons, obstacles, 1)
    t, end = rrt.build_tree(x_init, goal)
    nt.assert_true(goal.contains(end.node))
    nt.assert_true(np.all([[not obs.contains(n) for obs in obstacles]
                           for n in t.nodes()]))

    util.plot_casestudy(cons, goal, obstacles, t)