Esempio n. 1
0
def main2():
    map = Map("maps/rss_offset.json")
    planner = RRT(map)
    start = [.5, .5]
    goal = [3.5, 2.5]
    path = np.array(planner.getPath(start, goal))
    r = Controller(start[0], start[1], 0, map, path)
    a = input("presse a key to start controller: ")
    x, y, v, w, d = r.followPath()
Esempio n. 2
0
def test6():
    obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]),
                Polygon([(4,4), (4,8), (7,1)]),
                Polygon([(2,2), (1,8), (2,4), (3,3)])]
    workspace = Workspace(dim=(12,12), obstacles=obstacles)
    car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=4, max_control_time=1, step_threshold=.05)
    results = car_rrt.build_rrt()
    print(results)
    ax = workspace.graph_results(results, car_rrt, plot_tree=False)
    plt.show()
Esempio n. 3
0
def test22():
    obstacles = [Polygon([(2,0), (8, 0), (8, 1), (2, 1)]),
                Polygon([(2,10), (8,10), (8,8), (2,8)]),
                Polygon([(2,1), (2,8), (3,8), (3,1)])]
    workspace = Workspace(dim=(12,12), obstacles=obstacles)
    car_rrt = RRT(workspace, (1, 6, pi), (6, 6, pi), 10000, num_rand_actions=6, max_control_time=1, step_threshold=.05)
    results = car_rrt.build_rrt()
    print(results)
    anim = workspace.animate_results(results, car_rrt, plot_tree=True)
    plt.show()
Esempio n. 4
0
 def generate_paths(self, algo="a-star"):
     if algo == "a-star":
         for robot in self.robots:
             astar = Astar(robot.pose, robot.goal, self.obstacles)
             robot.path = astar.generate_path()
     elif algo == "rrt":
         for robot in self.robots:
             rrt = RRT(robot.pose, robot.goal, self.obstacles)
             robot.path = rrt.generate_path()
     else:
         print "No such algo currently exists."
         return
Esempio n. 5
0
    def plan_cb(self, goal):

        # Wait for map to be created
        while not rospy.is_shutdown():
            if self.map is not None:
                break
            rospy.loginfo_throttle(5, 'Planning: Waiting for map...')
            self.wait_rate.sleep()
        rospy.loginfo('Planning: map recieved!')

        # Create waypoints for the planner to pass through
        waypoints = self.create_waypoints(self.map.gates)
        start_wp = waypoints.pop(0)

        # Run path planning
        planner = RRT(self.map)
        traj = planner.plan_traj(start_wp, waypoints)
        while traj is None and not rospy.is_shutdown():
            rospy.logwarn('RRT: Restarting RRT...')
            traj = planner.plan_traj(start_wp, waypoints)

        # Convert trajectory of Beziers to dd241909_msgs/Trajectory
        traj_pieces = []
        for bezier in traj:
            piece = TrajectoryPolynomialPiece()
            for c in bezier.coeffs:
                piece.poly_x.append(c.x)
                piece.poly_y.append(c.y)
                piece.poly_z.append(c.z)
                piece.poly_yaw.append(0.0)
            piece.duration = rospy.Duration(bezier.T)
            traj_pieces.append(piece)
        traj_msg = Trajectory()
        traj_msg.header.frame_id = 'map'
        traj_msg.header.stamp = rospy.Time.now()
        traj_msg.pieces = traj_pieces

        # Set yaw values of traj_msg inplace
        self.set_yaw_gates(traj_msg, start_wp, traj)
        # self.set_yaw_rotating(traj_msg, start_wp, waypoints[-1])

        # Publish trajectory
        self.traj_pub.publish(traj_msg)

        # Visualise trajectory in rviz
        if self.use_rviz:
            publish_traj_to_rviz(traj_msg)

        self.plan_server.set_succeeded(SimpleResult(message=''))
Esempio n. 6
0
class RrtGa:
    '''
    结合RRT和GA算法来
    解决路径规划问题
    输入图,遗传算法的一些参数
    '''
    def __init__(self, my_map):
        self.map = my_map
        self.pop_num = 20  # 种群数量
        self.rrt = RRT(self.map)
        self.mutate_rate = 0.0001  # 突变概率
        self.iterations = 100
        self.cross_rate = 0.9

    def init_pop(self):
        paths = []
        for _ in range(self.pop_num):
            path = self.rrt.find_path()
            paths.append(path)
        return paths

    def find_path(self):
        self.pop = self.init_pop()  # 生成初始种群
        self.fits = [self.map.get_fitness(path)
                     for path in self.pop]  # 计算初始种群的fitness值
        for iteration in range(self.iterations):
            pass
Esempio n. 7
0
    def __init__(self):
        # self.command_sub_ = rospy.Subscriber('controller_commands_dev', Controller_Commands, self.cmdCallback, queue_size=5)
        # self.command_pub_ = rospy.Publisher('controller_commands', Controller_Commands, queue_size=1)
        # self.state_sub_ = rospy.Subscriber('state', State, self.stateCallback, queue_size=1)

        ref_lat = rospy.get_param("ref_lat")
        ref_lon = rospy.get_param("ref_lon")
        ref_h = rospy.get_param("ref_h")
        self.ref_pos = [ref_lat, ref_lon, ref_h]

        #Get the obstacles, boundaries, and drop location in order to initialize the RRT class
        mission_type, self.obstacles, self.boundary_list, self.boundary_poly, drop_location = tools.get_server_data(
            JudgeMission.MISSION_TYPE_DROP, self.ref_pos)

        self.RRT_planner = RRT(
            self.obstacles, self.boundary_list, animate=False
        )  #Other arguments are available but have been given default values in the RRT constructor

        # Advertise the path planning service call
        self._plan_server = rospy.Service('plan_path', PlanMissionPoints,
                                          self.waypoints_callback)

        #Set up a service call to get waypoints from the mission planner
        self.mission_data = rospy.ServiceProxy('plan_mission',
                                               PlanMissionPoints)
Esempio n. 8
0
 def test_rrt_init_args(self):
     rrt = RRT(start_state=(0., 0.),
               goal_state=(1, 1),
               dim_ranges=[(-2, 2), (-2, 2)],
               obstacles=None,
               step_size=0.1,
               max_iter=1000)
Esempio n. 9
0
    def test_build(self):
        num_retries = 3
        for _ in range(num_retries):
            rrt1 = RRT(start_state=(0.2, 0.2),
                       goal_state=(0.8, 0.8),
                       dim_ranges=[(0, 1), (0, 1)],
                       max_iter=1000)
            path = rrt1.build()
            if path is not None:
                break
            else:
                print("retrying...")

        self.assertIsNotNone(path)
        self.assertTrue(np.all(path[0] == [0.2, 0.2]))
        self.assertTrue(np.all(path[-1] == [0.8, 0.8]))
Esempio n. 10
0
    def rrt_local(self, pe, start, end):
        dims = len(start.vertex)
        lims = []
        for i in range(dims):
            if start.vertex[i] < end.vertex[i]:
                min = start.vertex[i] - 20
                max = end.vertex[i] + 20
            else:
                max = start.vertex[i] + 20
                min = end.vertex[i] - 20
            lims.append([min,max])

        rrt = RRT(50,
                  dims,
                  self.epsilon,
                  lims=np.array(lims),
                  connect_prob=.15,
                  collision_func=pe.test_collisions)
        plan = rrt.build_rrt(start.vertex, end.vertex, pe.robot)
        return plan
Esempio n. 11
0
    def test_rrt(self):
        """
        Tests that the RRT class works
        """

        env = StaticEnvironment((100, 100), 100)
        rrt = RRT(env)

        # Selection of random starting and ending points
        start = env.random_free_space()
        end = env.random_free_space()

        # Trying first the euclidian distance
        rrt.set_start(start)
        rrt.run(end, 200, metric='euclidian')

        # Trying first the distance defined by the local planner
        rrt.set_start(start)
        rrt.run(end, 200, metric='local')

        print('Static RRT OK')
Esempio n. 12
0
def solution(car):

    #boundary, start, goal, obstacles, space_resolution, stepFunction,
    #  primitives=5, primitive_bounds=[-math.pi/4.0, math.pi/4.0], rounding=4
    # Assumption: state = (id,prim,x,y,r,F,g)

    boundary = [[car.xlb, car.ylb], [car.xub, car.yub]]
    # start = [0,0,car.x0,car.y0,0,0,0]
    # gt = math.atan2(car.yt-car.y0, car.xt-car.x0)
    # goal = [-1,-1,car.xt,car.yt,gt,0,0]
    start = (car.x0, car.y0, 0.0, 0.0, 0, 0)  #(x,y,theta,phi,id,parent)
    goal = (car.xt, car.yt, 0.0, 0.0, 0, 0)
    obstacles = car.obs
    #space_resolution = min(obstacles, key = lambda t: t[2])[2]/4.0 #smallest obstacle
    space_resolution = 0.01
    stepFunction = car.step

    print("Resolution: {}".format(space_resolution))
    print("Start: {}".format(start))
    print("Goal: {}".format(goal))
    print("Boundary {}".format(boundary))

    # astar = AStar(boundary, start, goal, obstacles, space_resolution, stepFunction)
    rrt = RRT(boundary, start, goal, obstacles, space_resolution, stepFunction)

    controls = [0, 0, 0]
    print("Started Planning")
    # controls = astar.run()
    controls = rrt.run()
    print("End Planning")

    assert (controls != None)

    times = [0]
    for _ in controls:
        times.append(times[-1] + car.dt)

    return controls, times
Esempio n. 13
0
def main():
    fig = plt.figure()
    armax = fig.add_subplot(121,
                            autoscale_on=False,
                            xlim=(-15, 15),
                            ylim=(-15, 15))

    confax = fig.add_subplot(122,
                             autoscale_on=False,
                             xlim=(-2 * math.pi, 2.5 * math.pi),
                             ylim=(-2 * math.pi, 2.5 * math.pi))

    rrt = RRT(confax, Arm2D())
    confax.set_title("Configuration Space")
    armax.set_title("Arm")
    arm = Arm2DGraph(armax, confax, rrt)
    armax.set_aspect('equal')
    confax.set_aspect('equal')
    oldjoints = arm.arm.joints

    cx = []
    cy = []
    for theta1 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1):
        for theta2 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1):
            arm.arm.update(joints=[theta1, theta2])
            if arm.arm.inCollision():
                cx.append(theta1)
                cy.append(theta2)
    confax.scatter(cx, cy, c='r', marker='x')
    arm.arm.joints = oldjoints

    #ax.axis((-10, 10, -10, 10))
    fig.canvas.mpl_connect('button_press_event', arm.on_press)
    fig.canvas.mpl_connect('button_release_event', arm.on_release)
    fig.canvas.mpl_connect('motion_notify_event', arm.on_motion)
    fig.canvas.mpl_connect('motion_notify_event', arm.on_motion)

    fig.canvas.mpl_connect('key_press_event', arm.on_key_press)
    fig.canvas.mpl_connect('key_release_event', arm.on_key_release)
    armax.grid()
    confax.grid()

    ani = animation.FuncAnimation(fig, arm.animate, interval=10, blit=False)

    plt.show()
Esempio n. 14
0
    def setUp(self):
        # RRT 1: RRT with 1 node, in 3 dimensions
        self.rrt1 = RRT(start_state=(0., 0., 0.),
                        goal_state=(1, 1, 1),
                        dim_ranges=[(-2, 2)] * 3,
                        step_size=0.1)

        # RRT 2: RRT with 2 nodes, in 2 dimensions
        # (start) --> (node1)
        self.rrt2 = RRT(start_state=(0., 0.),
                        goal_state=(1, 1),
                        dim_ranges=[(-2, 2), (-2, 2)],
                        step_size=0.05)
        self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25))

        # RRT 3: RRT with 2 nodes, in 3 dimensions
        self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25),
                        goal_state=(0.75, 0.75, 0.75),
                        dim_ranges=[(0, 1)] * 3,
                        step_size=0.1)
        self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5))
        self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7))
Esempio n. 15
0
goal_region_radius = 1e-2

def goal_test(node):
    global goal
    return distance(node,goal) < goal_region_radius

def distance_from_goal(node):
    global goal
    return max(distance(node,goal)-goal_region_radius,0)


start = np.array([0,0,0])
goal = np.array([100.0,100.0,0])

rrt = RRT(state_ndim=3)

rrt.set_distance(distance)
rrt.set_cost(cost)
rrt.set_steer(steer)

rrt.set_goal_test(goal_test)
rrt.set_distance_from_goal(distance_from_goal)

rrt.set_sample(sample)
rrt.set_collision_check(collision_check)
rrt.set_collision_free(collision_free)

rrt.gamma_rrt = 100.0
rrt.eta = 50.0
rrt.c = 1
Esempio n. 16
0
goal_config = [500, 500, 500]
for i in range(N_ATTEMPT):

#start location
 Qinit_3d = (0, 0, 0)
 gx = random.choice(range(dd*3, ll+1, dd))
 gy = random.choice(range(dd*3, ll+1, dd))
 gz = random.choice(range(dd*3, ll+1, dd))
 gx = goal_config[0]
 gy = goal_config[1]
 gz = goal_config[2]
 print 'attempt: ',gx,gy,gz
 Qgoal_3d = (gx, gy, gz)
 Qgoal_3d = (dd*9, dd*9, dd*9)

 rrt = RRT(space, my_car, Qinit_3d, Qgoal_3d, QGOAL_BIAS, MAX_TREE_NODES)

 tree = rrt.build_rrt_3d()

 if rrt.path:
	 print 'path: ', rrt.path
	 # stats
	 print 'attempted: ',len(rrt.attempts)
	 print 'finalpath: ',len(rrt.path)

	 # draw expanded
	 for value in rrt.attempts:
		 curr = (value[0], value[1], value[2])
		 sphere(pos=curr, radius=11, color=color.yellow)

	 prev = (0,0,0)
Esempio n. 17
0
    if np.random.rand() < .9:
        statespace = np.random.rand(2) * np.array([.4, 2]) - np.array([.2, 1])
        time = np.random.randint(0, max_time_horizon, size=1) + 1
        #time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
        time = np.reshape(time, newshape=(1, ))
        return np.concatenate((statespace, time))
    else:  #goal bias
        statespace = goal[0:2]
        time = np.random.randint(0, max_time_horizon, size=1) + 1
        #time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
        time = np.reshape(time, newshape=(1, ))
        return np.concatenate((statespace, time))


start = np.array([0, -1, 0])
rrt = RRT(state_ndim=3, control_ndim=1)

rrt.sample_goal = lambda: goal

rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)

rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
#rrt.set_collision_check(isStateValid)
rrt.set_collision_free(lqr_rrt.collision_free)

rrt.set_distance_from_goal(distance_from_goal)
Esempio n. 18
0
                MAX_TREE_NODES = 1000
                QGOAL_BIAS = 0.33
                my_car = Car()
                space = ConfigSpace(g)

                Qinit_3d = (0, 0, 0)
                # x = g.objects[g.reverseLookup[goal]].pos.x
                # y = g.objects[g.reverseLookup[goal]].pos.y
                # z = g.objects[g.reverseLookup[goal]].pos.z
                # Qgoal_3d = (x, y, z)
                Qgoal_3d = g.objects[g.reverseLookup[goal]].pos
                #print 'GOAL',Qgoal_3d

                # RUN
                t1 = time.time()
                rrt = RRT(space, my_car, Qinit_3d, Qgoal_3d, QGOAL_BIAS,
                          MAX_TREE_NODES)
                tree = rrt.build_rrt_3d()
                t2 = time.time()

                if rrt.path:
                    # Order: run#, number of nodes, size, number of obstructions, time, path length, nodes expanded
                    print "R,%d,%d,%d,%d,%f,%d,%d" % (
                        run, len(g.locations), size, obstructions,
                        (100 * (t2 - t1)), len(rrt.path), len(rrt.attempts))
                else:
                    print "R,%d,%d,%d,%d,%f,%d,%d" % (
                        run, len(g.locations), size, obstructions,
                        (100 * (t2 - t1)), -1, len(rrt.attempts))

                # Draw the resulting path
                if draw and rrt.path:
Esempio n. 19
0
        '''
        ee = fr.ee(q)
        err = np.sum((np.asarray(desired_ee_rp) - np.asarray(ee[3:5]))**2)
        grad = np.asarray([
            0, 0, 0, 2 * (ee[3] - desired_ee_rp[0]),
            2 * (ee[4] - desired_ee_rp[1]), 0
        ])
        return err, grad

    print('Desired ee roll and pitch:', desired_ee_rp)
    joints_start = fr.home_joints.copy()
    joints_start[0] = -np.deg2rad(45)
    joints_target = joints_start.copy()
    joints_target[0] = np.deg2rad(45)

    rrt = RRT(fr, is_in_collision)
    constraint = ee_upright_constraint
    plan = rrt.plan(joints_start, joints_target, constraint)

    i = 0
    collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes')
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        joints = plan[i % len(plan)]
        fr.publish_joints(joints)
        fr.publish_collision_boxes(joints)
        collision_boxes_publisher.publish_boxes(boxes)

        i += 1
        rate.sleep()
    def main(self):
        """
        dims : graph's dimensions (numpy array of tuples)
        obstacles : obstacles to be placed in the graph (list of shapely.Polygons)
        home : home location (tuple)
        init_state : start location (tuple)
        goal_state : goal location (tuple)
        delta : distance between nodes (int)
        k : shrinking ball facotr (int)
        n : number of waypoints to push (int)
        path : list of waypoints (list of tuples)
        case : case number to set behavior (int)
        Units: meters
        """

        #rospy.init_node('RRTnode')
        #rospy.wait_for_service('/control/waypoints')

        wp_push = rospy.ServiceProxy('/control/waypoints', WaypointPush)

        #rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_cb)
        #rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.global_heading)
        #rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_position)

        rate = rospy.Rate(1)  # send msgs at 1 Hz
        start_time = rospy.get_time()
        rate.sleep()

        # while loop for gps_check:
        while self.gps_status == -1 or self.gps_status is None:
            rospy.loginfo("BAD GPS")
            rate.sleep()

        while not self.home_set:
            rospy.loginfo("Home not set!")
            rate.sleep()

        #		   E-x	     N-y       D-z
        dims = np.array([(-500, 2500), (-100, 2900), (-50, -50)])
        obstacle = [(Point(1300, 1300).buffer(500))]
        init = self.pos
        goal = (2300.0, 2600.0, -50.0)
        delta = 100
        k = 2

        graph = Graph(dims, obstacle)
        path = None
        trail = []
        position = []

        start_index = 0

        rospy.loginfo("Calculating Trajectory...")
        #print('Calculating Trajectory...\n')
        while not rospy.is_shutdown():
            if graph.num_nodes() == 0:
                #t0 = time.time()
                rrt = RRT(graph, init, goal, delta, k, path, self.heading)
            if graph.num_nodes() <= 250:
                path, leaves = rrt.search()
            else:
                init = path[path.index(rrt.brute_force(self.pos, path)) + 1]
                trail.append(path)
                position.append(self.pos)

                rate.sleep()
                graph.clear()

                pathNED = np.asarray(path)
                wp_msg = []
                pathLLA = self.frame.ConvNED2LLA(pathNED.T)

                for i in range(len(pathNED)):
                    wp_point = Waypoint()
                    wp_point.frame = 3

                    wp_point.x_lat = pathLLA[0][i]
                    wp_point.y_long = pathLLA[1][i]
                    wp_point.z_alt = pathLLA[2][i]
                    wp_msg += [wp_point]

                print(wp_msg, '\n')
                resp = wp_push(start_index, wp_msg)
                print(resp, '\n')

                start_index += path.index(rrt.brute_force(self.pos, path)) + 1
                #print('Search Time: {} secs\n'.format(time.time() - t0))
                rate.sleep()

            if path is not None:
                if dist(self.pos, goal) <= delta * 3 or goal in path:
                    rospy.loginfo("Goal reached")
                    #print('Goal Reached.\n')
                    print('trail :  ', trail, '\n')
                    print('position:  ', position, '\n')
                    break

            print('EOL')
            [0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4])
        joints_target[0] = -np.deg2rad(45)
    elif args.map2:
        joints_start = np.array(
            [0, np.pi / 6, 0, -2 * np.pi / 3, 0, 5 * np.pi / 6, np.pi / 4])
        joints_target = np.array(
            [0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4])
    else:
        joints_start = fr.home_joints.copy()
        joints_start[0] = -np.deg2rad(45)
        joints_target = joints_start.copy()
        joints_target[0] = np.deg2rad(45)

    if args.rrt:
        print("RRT: RRT planner is selected!")
        planner = RRT(fr, is_in_collision)
    elif args.rrtc:
        print("RRTC: RRT Connect planner is selected!")
        planner = RRTConnect(fr, is_in_collision)
    elif args.prm:
        print("PRM: PRM planner is selected!")
        planner = PRM(fr, is_in_collision)
    elif args.obprm:
        print("OB_PRM: OB_PRM planner is selected!")
        planner = OBPRM(fr, is_in_collision)

    constraint = ee_upright_constraint
    if args.prm or args.obprm:
        plan = planner.plan(joints_start, joints_target, constraint, args)
    else:
        plan = planner.plan(joints_start, joints_target, constraint)
Esempio n. 22
0
class TestRRTSteps(unittest.TestCase):
    def setUp(self):
        # RRT 1: RRT with 1 node, in 3 dimensions
        self.rrt1 = RRT(start_state=(0., 0., 0.),
                        goal_state=(1, 1, 1),
                        dim_ranges=[(-2, 2)] * 3,
                        step_size=0.1)

        # RRT 2: RRT with 2 nodes, in 2 dimensions
        # (start) --> (node1)
        self.rrt2 = RRT(start_state=(0., 0.),
                        goal_state=(1, 1),
                        dim_ranges=[(-2, 2), (-2, 2)],
                        step_size=0.05)
        self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25))

        # RRT 3: RRT with 2 nodes, in 3 dimensions
        self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25),
                        goal_state=(0.75, 0.75, 0.75),
                        dim_ranges=[(0, 1)] * 3,
                        step_size=0.1)
        self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5))
        self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7))

    def test_get_random_sample(self):
        sample = self.rrt1._get_random_sample()
        lower = np.array([d[0] for d in self.rrt1.dim_ranges])
        upper = np.array([d[1] for d in self.rrt1.dim_ranges])

        self.assertEqual(len(sample), len(self.rrt1.dim_ranges))
        self.assertTrue(np.all(sample >= lower))
        self.assertTrue(np.all(sample < upper))

    def test_get_nearest_neighbor(self):
        sample1 = np.array([0.1, 0.2, 0.3])
        neighbor1 = self.rrt1._get_nearest_neighbor(sample1)
        self.assertIs(neighbor1, self.rrt1.start)

        sample2 = np.array([0.25, 0.25])
        neighbor2 = self.rrt2._get_nearest_neighbor(sample2)
        self.assertIs(neighbor2, self.rrt2_node1)

        sample3 = np.array([0.7, 0.7, 0.7])
        neighbor3 = self.rrt3._get_nearest_neighbor(sample3)
        self.assertIs(neighbor3, self.rrt3_node2)

    def test_extend_sample(self):
        sample1 = np.array([0, 0, 0.1])
        sample1_node = self.rrt1._extend_sample(sample1, self.rrt1.start)

        self.assertIs(sample1_node.parent, self.rrt1.start)
        self.assertAlmostEqual(np.linalg.norm(sample1 - sample1_node.state),
                               0.,
                               places=3)

        sample2 = np.array([1, 1, 1])
        sample2_ext = np.array([0.0577, 0.0577, 0.0577])
        sample2_node = self.rrt1._extend_sample(sample2, self.rrt1.start)
        self.assertAlmostEqual(np.linalg.norm(sample2_ext -
                                              sample2_node.state),
                               0.,
                               places=3)

    def test_check_for_completion(self):
        complete1 = self.rrt2._check_for_completion(self.rrt2_node1)
        self.assertFalse(complete1)

        complete2 = self.rrt3._check_for_completion(self.rrt3_node2)
        self.assertTrue(complete2)

    def test_trace_path_from_start(self):
        path1 = self.rrt1._trace_path_from_start(self.rrt1.start)
        self.assertEqual(len(path1), 1)
        self.assertTrue(np.all(path1[0] == self.rrt1.start.state))

        path2 = self.rrt2._trace_path_from_start(self.rrt2_node1)
        self.assertEqual(len(path2), 2)
        self.assertTrue(np.all(path2[0] == self.rrt2.start.state))
        self.assertTrue(np.all(path2[1] == self.rrt2_node1.state))

        self.rrt3_node2.children.append(self.rrt3.goal)
        self.rrt3.goal.parent = self.rrt3_node2
        path3 = self.rrt3._trace_path_from_start()
        self.assertEqual(len(path3), 4)
        self.assertTrue(np.all(path3[0] == self.rrt3.start.state))
        self.assertTrue(np.all(path3[1] == self.rrt3_node1.state))
        self.assertTrue(np.all(path3[2] == self.rrt3_node2.state))
        self.assertTrue(np.all(path3[3] == self.rrt3.goal.state))

    def test_check_for_collision(self):
        sample = np.array([0.5, 0.5, 0.5])
        in_collision = self.rrt1._check_for_collision(sample)
        self.assertFalse(in_collision)
Esempio n. 23
0
def main():
    rospy.init_node('rrt')
    rrt = RRT()
    rospy.spin()
Esempio n. 24
0
    def is_collision(self, state):
        #-- Casts as a shapely point
        state_point = Point(state[0], state[1])
        if self.obstacles is None:
            return False
        else:
            #-- For each obstacle checks if the point is contained in the obstacle.
            for obstacle in self.obstacles:
                if obstacle.contains(state_point):
                    return True
            return False

    def graph_results(self, results, car_rrt, plot_tree=False):
        return graph_results(self, results, car_rrt, plot_tree)

    def animate_results(self, results, car_rrt, plot_tree=False):
        return animate_results(self, results, car_rrt, plot_tree)


if __name__ == '__main__':
    random.seed(446)
    obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]),
                Polygon([(4,4), (4,8), (7,1)]),
                Polygon([(2,2), (1,8), (2,4), (3,3)])]
    workspace = Workspace(dim=(12,12), obstacles=obstacles)
    car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=6, max_control_time=2, step_threshold=.05)
    results = car_rrt.build_rrt()
    print(results)
    ax = workspace.graph_results(results, car_rrt, plot_tree=False)
    plt.show()
Esempio n. 25
0
import json

from apf import APF
from rrt import RRT


def load_config(config_filename: str) -> dict:
    with open(config_filename) as f:
        config = json.load(f)
    return config


def pretty_print_config(config: dict):
    print(json.dumps(config, indent=2, sort_keys=False))


if __name__ == '__main__':
    config = load_config('config.json')
    apf_paraboloid = APF(config, paraboloid=True)
    apf_paraboloid.apf()

    apf_conical = APF(config, paraboloid=False)
    apf_conical.apf()

    rrt = RRT(config)
    rrt.bidirectional_rrt()
Esempio n. 26
0
	print "Enter your choice: "
	print "1) RRT"
	print "2) RRT Star"

	con = int(input())
	start = [X, Y]
	end = [gX, gY]

	generator = Generator(n)
	obstacleList = generator.generateRect()
	expandDist = 0.7
	goalSample = 0.2

	if con == 1:
		rrt = RRT(start, end, obstacleList, expandDist, goalSample)

	if con == 2:
		itera = n * 1000
		rrt = RRTStar(start, end, obstacleList, expandDist, goalSample, itera)

	xPath, yPath, xPlot, yPlot = rrt.planner()

	if xPath != None:
		fig1 = plt.figure()
		ax1 = fig1.add_subplot(111, aspect='equal')
		for (ox, oy, sx, sy) in obstacleList:
			ax1.add_patch(patches.Rectangle((ox - sx/2.0, oy - sy/2.0), sx, sy, color='black'))

		plt.plot(xPath, yPath)
		plt.scatter(xPlot, yPlot, marker='^', color='red')
Esempio n. 27
0
 def test_rrt_init_basic(self):
     rrt = RRT(start_state=(0.0, 0.0),
               goal_state=(1, 1),
               dim_ranges=[(-2, 2), (-2, 2)])
Esempio n. 28
0
def sample():
    if np.random.rand()<.9:
        statespace = np.random.rand(2)*np.array([.4,2])-np.array([.2,1])
        time = np.random.randint(0,max_time_horizon,size=1) + 1
        #time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
        time = np.reshape(time,newshape=(1,))
        return np.concatenate((statespace,time))
    else: #goal bias
        statespace = goal[0:2]
        time = np.random.randint(0,max_time_horizon,size=1) + 1
        #time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
        time = np.reshape(time,newshape=(1,))
        return np.concatenate((statespace,time))

start = np.array([0,-1,0])
rrt = RRT(state_ndim=3,control_ndim=1)

rrt.sample_goal = lambda : goal

rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)

rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
#rrt.set_collision_check(isStateValid)
rrt.set_collision_free(lqr_rrt.collision_free)

rrt.set_distance_from_goal(distance_from_goal)
Esempio n. 29
0
def main(argv=None):
    if (argv == None):
        argv = sys.argv[1:]

    width = 100.0
    height = 100.0

    pp = PathPlanningProblem(width, height, 60, 40, 40)
    initial, goals = pp.CreateProblemInstance()

    fig = plt.figure()
    ax = fig.add_subplot(1, 2, 1, aspect='equal')
    ax.set_xlim(0.0, width)
    ax.set_ylim(0.0, height)

    for o in pp.obstacles:
        ax.add_patch(copy.copy(o.patch))
    ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000')
    ax.add_patch(ip)

    for g in goals:
        g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00')
        ax.add_patch(g)

    qtd = QuadTreeDecomposition(pp, 0.2, initial, goals)
    qtd.Draw(ax)
    n = qtd.CountCells()

    start = timeit.default_timer()
    astar = AStarSearch(
        qtd.domain, qtd.root,
        Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1),
        Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1))
    stop = timeit.default_timer()
    print("A* Running Time: ", stop - start)
    print("A* Path Length : ", astar.path_length)
    plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-')
    ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))

    ax = fig.add_subplot(1, 2, 2, aspect='equal')
    ax.set_xlim(0.0, width)
    ax.set_ylim(0.0, height)

    for o in pp.obstacles:
        ax.add_patch(copy.copy(o.patch))
    ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000')
    ax.add_patch(ip)

    goal = None
    for g in goals:
        goal = g
        g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00')
        ax.add_patch(g)
    start = timeit.default_timer()
    spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000)
    path = spath[0]
    final = spath[1]
    if len(final) == 0:
        print("No path found")
    RRT.draw(RRT(0), plt, path, final)
    stop = timeit.default_timer()
    print("RRT Running Time: ", stop - start)
    if len(final) > 0:
        print("RRT Path Length : ", RRT.pathLen(RRT(0), final))
    ax.set_title('RRT')
    plt.show()
Esempio n. 30
0
 def test_rrt_init_bad_dims(self):
     with self.assertRaises(AssertionError):
         rrt = RRT(start_state=(0., 0.),
                   goal_state=(1, 1, 1),
                   dim_ranges=[(-2, 2), (-2, 2)])
Esempio n. 31
0
    return 0


def goal_test(node):
    goal_region_radius = .01
    n = 4
    return np.sum(np.abs(node['state'][0:n] -
                         goal[0:n])) < goal_region_radius  #disregards time
    return distance(
        node, goal
    ) < goal_region_radius  #need to think more carefully about this one


start = np.array([0, 0, 0, 0, 0])
lqr_rrt = LQR_RRT(A, B, Q, R, max_time_horizon)
rrt = RRT(state_ndim=5, control_ndim=2)

lqr_rrt.action_state_valid = action_state_valid

lqr_rrt.max_nodes_per_extension = 5

rrt.sample_goal = lambda: goal

rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)

rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_free(lqr_rrt.collision_free)
Esempio n. 32
0
def distance(from_node,to_point):
    assert len(to_point)==2
    #to_point is an array and from_point is a node
    return np.linalg.norm(to_point-from_node['state'])

goal_region_radius = .05
def goal_test(node):
    global goal
    return distance(node,goal) < goal_region_radius
    
def distance_from_goal(node):
    global goal
    return max(distance(node,goal)-goal_region_radius,0)

start = np.array([-1,-1])*1    
rrt = RRT(state_ndim=2,control_ndim=2)

rrt.set_same_state(same_state)
rrt.set_distance(distance)
rrt.set_cost(cost)
rrt.set_steer(steer)

rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_check(isStateValid)
rrt.set_collision_free(collision_free)

rrt.set_distance_from_goal(distance_from_goal)

rrt.gamma_rrt = 40.0
rrt.eta = 0.5
Esempio n. 33
0
if __name__ == "__main__":

    start = time.time()

    # parse command line args
    parser = argparse.ArgumentParser()
    parser.add_argument("-path_to_data", default="../results/")
    parser.add_argument("-path_to_output", default="../results/")
    parser.add_argument("-visualize", action="store_true")
    parser.add_argument("-method", choices=["RRT", "PRM"], default="RRT")
    parser.add_argument("-step", default=0.05, type=float)
    args = parser.parse_args()

    if args.method == "RRT":
        RRT(data_dir=args.path_to_data,
            out_dir=args.path_to_output,
            viz_=args.visualize,
            step_=args.step)
    elif args.method == "PRM":
        print("PRM not implemented!")
        # PRM(data_dir=args.path_to_data, out_dir=args.path_to_output, viz_=args.visualize, N=500)
    else:
        print("Choose RRT or PRM for method")
        exit()

    end = time.time()
    print(f"Elapsed Time: {(end-start):.4f}s")

    # keep visualization on screen
    if args.visualize: input("Press any button to continue...\n")
Esempio n. 34
0
    def rrt_planning(self):
        #initialize array
        X = []
        Y = []
        Z_euler=[]
        #set start point for orientation 
        angle_s= self.angle_orientation[0]
        #set start point for path
        x_s = self.x_array[0]
        y_s = self.x_array[0]
        
        l = len(self.order_pic)
        maxIter = 1000
        obstacleList=self.obstacleList
	    #Generate Path & Orientation based on given order
        for i in range(l):
            #get order number
            n = self.order_pic[i]
            print(n)
            #set goal point
            x_g = self.x_array[n]
            y_g = self.y_array[n]
            angle_g = self.angle_orientation[n]
            angle_g = angle_g+np.pi
            #call RRT from rrt.py
            rrt = RRT(start=[x_s, y_s], goal=[x_g, y_g], rand_area=[0, 9], obstacle_list=self.obstacleList)
            path = rrt.planning(animation=False)
            #print(path)
            smoothedPath = rrt.path_smoothing(path, maxIter, obstacleList)
            #print(smoothedPath)
            X_p,Y_p = map(list,zip(*smoothedPath))
            #Reverse X,Y coordinate for each segment
            X_p = np.flipud(X_p)
            Y_p = np.flipud(Y_p)

            m = len(X_p)-1
            
            num_point = 800
            #Store the path
            for j in range(m):
                #linaer interpolation of path
                n_x = np.linspace(X_p[j], X_p[j+1], num=num_point)
                n_y = np.linspace(Y_p[j], Y_p[j+1], num=num_point)

                X = np.concatenate([X,n_x])
                Y = np.concatenate([Y,n_y])
                     
            #linear interpolation of orientation
            n_z_euler=np.linspace(angle_s , angle_g, m*num_point)
            Z_euler=np.concatenate([Z_euler,n_z_euler])

            #update start point
            x_s = x_g
            y_s = y_g
            angle_s = angle_g
        
        Z = np.ones(len(X))*1.3
        X_euler = np.linspace(0, 0 , len(X))
        Y_euler = np.linspace(0, 0 , len(X))
        #print(X)
        
        self.number_of_points=len(X)
        return X, Y, Z, X_euler, Y_euler, Z_euler
Esempio n. 35
0
        time = np.reshape(time,newshape=(1,))
        return np.concatenate((statespace,time))
        

def distance_from_goal(node):
    return 0

def goal_test(node):
    goal_region_radius = .01
    n = 4
    return np.sum(np.abs(node['state'][0:n]-goal[0:n])) < goal_region_radius #disregards time
    return distance(node,goal) < goal_region_radius                     #need to think more carefully about this one

start = np.array([0,0,0,0,0])
lqr_rrt = LQR_RRT(A,B,Q,R,max_time_horizon)
rrt = RRT(state_ndim=5,control_ndim=2)

lqr_rrt.action_state_valid = action_state_valid

lqr_rrt.max_nodes_per_extension = 5

rrt.sample_goal = lambda : goal

rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)

rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_free(lqr_rrt.collision_free)