Sobol sampler implemented (#413)

PathPlanning/RRT/rrt_with_sobol_sampler.py

@@ -0,0 +1,270 @@
+"""
+
+Path planning Sample Code with Randomized Rapidly-Exploring Random
+Trees with sobol low discrepancy sampler(RRTSobol).
+Sobol wiki https://en.wikipedia.org/wiki/Sobol_sequence
+
+The goal of low discrepancy samplers is to generate a sequence of points that
+optimizes a criterion called dispersion.  Intuitively, the idea is to place
+samples to cover the exploration space in a way that makes the largest
+uncovered area be as small as possible.  This generalizes of the idea of grid
+resolution.  For a grid, the resolution may be selected by defining the step
+size for each axis.  As the step size is decreased, the resolution increases.
+If a grid-based motion planning algorithm can increase the resolution
+arbitrarily, it becomes resolution complete.  Dispersion can be considered as a
+powerful generalization of the notion of resolution.
+
+Taken from
+LaValle, Steven M. Planning algorithms. Cambridge university press, 2006.
+
+
+authors:
+    First implementation AtsushiSakai(@Atsushi_twi)
+    Sobol sampler Rafael A.
+Rojas (rafaelrojasmiliani@gmail.com)
+
+
+"""
+
+import math
+import random
+from sobol import sobol_quasirand
+import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+show_animation = True
+
+
+class RRTSobol:
+    """
+    Class for RRTSobol planning
+    """
+
+    class Node:
+        """
+        RRTSobol Node
+        """
+
+        def __init__(self, x, y):
+            self.x = x
+            self.y = y
+            self.path_x = []
+            self.path_y = []
+            self.parent = None
+
+    def __init__(self,
+                 start,
+                 goal,
+                 obstacle_list,
+                 rand_area,
+                 expand_dis=3.0,
+                 path_resolution=0.5,
+                 goal_sample_rate=5,
+                 max_iter=500):
+        """
+        Setting Parameter
+
+        start:Start Position [x,y]
+        goal:Goal Position [x,y]
+        obstacle_list:obstacle Positions [[x,y,size],...]
+        randArea:Random Sampling Area [min,max]
+
+        """
+        self.start = self.Node(start[0], start[1])
+        self.end = self.Node(goal[0], goal[1])
+        self.min_rand = rand_area[0]
+        self.max_rand = rand_area[1]
+        self.expand_dis = expand_dis
+        self.path_resolution = path_resolution
+        self.goal_sample_rate = goal_sample_rate
+        self.max_iter = max_iter
+        self.obstacle_list = obstacle_list
+        self.node_list = []
+        self.sobol_inter_ = 0
+
+    def planning(self, animation=True):
+        """
+        rrt path planning
+
+        animation: flag for animation on or off
+        """
+
+        self.node_list = [self.start]
+        for i in range(self.max_iter):
+            rnd_node = self.get_random_node()
+            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
+            nearest_node = self.node_list[nearest_ind]
+
+            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
+
+            if self.check_collision(new_node, self.obstacle_list):
+                self.node_list.append(new_node)
+
+            if animation and i % 5 == 0:
+                self.draw_graph(rnd_node)
+
+            if self.calc_dist_to_goal(self.node_list[-1].x,
+                                      self.node_list[-1].y) <= self.expand_dis:
+                final_node = self.steer(self.node_list[-1], self.end,
+                                        self.expand_dis)
+                if self.check_collision(final_node, self.obstacle_list):
+                    return self.generate_final_course(len(self.node_list) - 1)
+
+            if animation and i % 5:
+                self.draw_graph(rnd_node)
+
+        return None  # cannot find path
+
+    def steer(self, from_node, to_node, extend_length=float("inf")):
+
+        new_node = self.Node(from_node.x, from_node.y)
+        d, theta = self.calc_distance_and_angle(new_node, to_node)
+
+        new_node.path_x = [new_node.x]
+        new_node.path_y = [new_node.y]
+
+        if extend_length > d:
+            extend_length = d
+
+        n_expand = math.floor(extend_length / self.path_resolution)
+
+        for _ in range(n_expand):
+            new_node.x += self.path_resolution * math.cos(theta)
+            new_node.y += self.path_resolution * math.sin(theta)
+            new_node.path_x.append(new_node.x)
+            new_node.path_y.append(new_node.y)
+
+        d, _ = self.calc_distance_and_angle(new_node, to_node)
+        if d <= self.path_resolution:
+            new_node.path_x.append(to_node.x)
+            new_node.path_y.append(to_node.y)
+            new_node.x = to_node.x
+            new_node.y = to_node.y
+
+        new_node.parent = from_node
+
+        return new_node
+
+    def generate_final_course(self, goal_ind):
+        path = [[self.end.x, self.end.y]]
+        node = self.node_list[goal_ind]
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        return path
+
+    def calc_dist_to_goal(self, x, y):
+        dx = x - self.end.x
+        dy = y - self.end.y
+        return math.hypot(dx, dy)
+
+    def get_random_node(self):
+        if random.randint(0, 100) > self.goal_sample_rate:
+            rand_coordinates, n = sobol_quasirand(2, self.sobol_inter_)
+
+            rand_coordinates = self.min_rand + \
+                rand_coordinates*(self.max_rand - self.min_rand)
+            self.sobol_inter_ = n
+            rnd = self.Node(*rand_coordinates)
+
+        else:  # goal point sampling
+            rnd = self.Node(self.end.x, self.end.y)
+        return rnd
+
+    def draw_graph(self, rnd=None):
+        plt.clf()
+        # for stopping simulation with the esc key.
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [sys.exit(0) if event.key == 'escape' else None])
+        if rnd is not None:
+            plt.plot(rnd.x, rnd.y, "^k")
+        for node in self.node_list:
+            if node.parent:
+                plt.plot(node.path_x, node.path_y, "-g")
+
+        for (ox, oy, size) in self.obstacle_list:
+            self.plot_circle(ox, oy, size)
+
+        plt.plot(self.start.x, self.start.y, "xr")
+        plt.plot(self.end.x, self.end.y, "xr")
+        plt.axis("equal")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+
+    @staticmethod
+    def plot_circle(x, y, size, color="-b"):  # pragma: no cover
+        deg = list(range(0, 360, 5))
+        deg.append(0)
+        xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
+        yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
+        plt.plot(xl, yl, color)
+
+    @staticmethod
+    def get_nearest_node_index(node_list, rnd_node):
+        dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
+                 for node in node_list]
+        minind = dlist.index(min(dlist))
+
+        return minind
+
+    @staticmethod
+    def check_collision(node, obstacle_list):
+
+        if node is None:
+            return False
+
+        for (ox, oy, size) in obstacle_list:
+            dx_list = [ox - x for x in node.path_x]
+            dy_list = [oy - y for y in node.path_y]
+            d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
+
+            if min(d_list) <= size**2:
+                return False  # collision
+
+        return True  # safe
+
+    @staticmethod
+    def calc_distance_and_angle(from_node, to_node):
+        dx = to_node.x - from_node.x
+        dy = to_node.y - from_node.y
+        d = math.hypot(dx, dy)
+        theta = math.atan2(dy, dx)
+        return d, theta
+
+
+def main(gx=6.0, gy=10.0):
+    print("start " + __file__)
+
+    # ====Search Path with RRTSobol====
+    obstacle_list = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
+                     (9, 5, 2), (8, 10, 1)]  # [x, y, radius]
+    # Set Initial parameters
+    rrt = RRTSobol(
+        start=[0, 0],
+        goal=[gx, gy],
+        rand_area=[-2, 15],
+        obstacle_list=obstacle_list)
+    path = rrt.planning(animation=show_animation)
+
+    if path is None:
+        print("Cannot find path")
+    else:
+        print("found path!!")
+
+        # Draw final path
+        if show_animation:
+            rrt.draw_graph()
+            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
+            plt.grid(True)
+            plt.pause(0.01)  # Need for Mac
+            plt.show()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/RRT/sobol/__init__.py

@@ -0,0 +1 @@
+from .sobol import i4_sobol as sobol_quasirand

PathPlanning/RRT/sobol/sobol.py

@@ -0,0 +1,911 @@
+"""
+  Licensing:
+    This code is distributed under the MIT license.
+
+  Authors:
+    Original FORTRAN77 version of i4_sobol by Bennett Fox.
+    MATLAB version by John Burkardt.
+    PYTHON version by Corrado Chisari
+
+    Original Python version of is_prime by Corrado Chisari
+
+    Original MATLAB versions of other functions by John Burkardt.
+    PYTHON versions by Corrado Chisari
+
+    Original code is available at
+    http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html
+
+    Note: the i4 prefix means that the function takes a numeric argument or
+          returns a number which is interpreted inside the function as a 4
+          byte integer
+    Note: the r4 prefix means that the function takes a numeric argument or
+          returns a number which is interpreted inside the function as a 4
+          byte float
+"""
+import math
+import sys
+import numpy as np
+
+atmost = None
+dim_max = None
+dim_num_save = None
+initialized = None
+lastq = None
+log_max = None
+maxcol = None
+poly = None
+recipd = None
+seed_save = None
+v = None
+
+
+def i4_bit_hi1(n):
+    """
+     I4_BIT_HI1 returns the position of the high 1 bit base 2 in an I4.
+
+      Discussion:
+
+        An I4 is an integer ( kind = 4 ) value.
+
+      Example:
+
+           N    Binary    Hi 1
+        ----    --------  ----
+           0           0     0
+           1           1     1
+           2          10     2
+           3          11     2
+           4         100     3
+           5         101     3
+           6         110     3
+           7         111     3
+           8        1000     4
+           9        1001     4
+          10        1010     4
+          11        1011     4
+          12        1100     4
+          13        1101     4
+          14        1110     4
+          15        1111     4
+          16       10000     5
+          17       10001     5
+        1023  1111111111    10
+        1024 10000000000    11
+        1025 10000000001    11
+
+      Licensing:
+
+        This code is distributed under the GNU LGPL license.
+
+      Modified:
+
+        26 October 2014
+
+      Author:
+
+        John Burkardt
+
+      Parameters:
+
+        Input, integer N, the integer to be measured.
+        N should be nonnegative.  If N is nonpositive, the function
+        will always be 0.
+
+        Output, integer BIT, the position of the highest bit.
+
+    """
+    i = n
+    bit = 0
+
+    while True:
+
+        if i <= 0:
+            break
+
+        bit = bit + 1
+        i = i // 2
+
+    return bit
+
+
+def i4_bit_lo0(n):
+    """
+     I4_BIT_LO0 returns the position of the low 0 bit base 2 in an I4.
+
+      Discussion:
+
+        An I4 is an integer ( kind = 4 ) value.
+
+      Example:
+
+           N    Binary    Lo 0
+        ----    --------  ----
+           0           0     1
+           1           1     2
+           2          10     1
+           3          11     3
+           4         100     1
+           5         101     2
+           6         110     1
+           7         111     4
+           8        1000     1
+           9        1001     2
+          10        1010     1
+          11        1011     3
+          12        1100     1
+          13        1101     2
+          14        1110     1
+          15        1111     5
+          16       10000     1
+          17       10001     2
+        1023  1111111111    11
+        1024 10000000000     1
+        1025 10000000001     2
+
+      Licensing:
+
+        This code is distributed under the GNU LGPL license.
+
+      Modified:
+
+        08 February 2018
+
+      Author:
+
+        John Burkardt
+
+      Parameters:
+
+        Input, integer N, the integer to be measured.
+        N should be nonnegative.
+
+        Output, integer BIT, the position of the low 1 bit.
+
+    """
+    bit = 0
+    i = n
+
+    while True:
+
+        bit = bit + 1
+        i2 = i // 2
+
+        if i == 2 * i2:
+            break
+
+        i = i2
+
+    return bit
+
+
+def i4_sobol_generate(m, n, skip):
+    """
+
+
+     I4_SOBOL_GENERATE generates a Sobol dataset.
+
+      Licensing:
+
+        This code is distributed under the MIT license.
+
+      Modified:
+
+        22 February 2011
+
+      Author:
+
+        Original MATLAB version by John Burkardt.
+        PYTHON version by Corrado Chisari
+
+      Parameters:
+
+        Input, integer M, the spatial dimension.
+
+        Input, integer N, the number of points to generate.
+
+        Input, integer SKIP, the number of initial points to skip.
+
+        Output, real R(M,N), the points.
+
+    """
+    r = np.zeros((m, n))
+    for j in range(1, n + 1):
+        seed = skip + j - 2
+        [r[0:m, j - 1], seed] = i4_sobol(m, seed)
+    return r
+
+
+def i4_sobol(dim_num, seed):
+    """
+
+
+     I4_SOBOL generates a new quasirandom Sobol vector with each call.
+
+      Discussion:
+
+        The routine adapts the ideas of Antonov and Saleev.
+
+      Licensing:
+
+        This code is distributed under the MIT license.
+
+      Modified:
+
+        22 February 2011
+
+      Author:
+
+        Original FORTRAN77 version by Bennett Fox.
+        MATLAB version by John Burkardt.
+        PYTHON version by Corrado Chisari
+
+      Reference:
+
+        Antonov, Saleev,
+        USSR Computational Mathematics and Mathematical Physics,
+        olume 19, 19, pages 252 - 256.
+
+        Paul Bratley, Bennett Fox,
+        Algorithm 659:
+        Implementing Sobol's Quasirandom Sequence Generator,
+        ACM Transactions on Mathematical Software,
+        Volume 14, Number 1, pages 88-100, 1988.
+
+        Bennett Fox,
+        Algorithm 647:
+        Implementation and Relative Efficiency of Quasirandom
+        Sequence Generators,
+        ACM Transactions on Mathematical Software,
+        Volume 12, Number 4, pages 362-376, 1986.
+
+        Ilya Sobol,
+        USSR Computational Mathematics and Mathematical Physics,
+        Volume 16, pages 236-242, 1977.
+
+        Ilya Sobol, Levitan,
+        The Production of Points Uniformly Distributed in a Multidimensional
+        Cube (in Russian),
+        Preprint IPM Akad. Nauk SSSR,
+        Number 40, Moscow 1976.
+
+      Parameters:
+
+        Input, integer DIM_NUM, the number of spatial dimensions.
+        DIM_NUM must satisfy 1 <= DIM_NUM <= 40.
+
+        Input/output, integer SEED, the "seed" for the sequence.
+        This is essentially the index in the sequence of the quasirandom
+        value to be generated.    On output, SEED has been set to the
+        appropriate next value, usually simply SEED+1.
+        If SEED is less than 0 on input, it is treated as though it were 0.
+        An input value of 0 requests the first (0-th) element of the sequence.
+
+        Output, real QUASI(DIM_NUM), the next quasirandom vector.
+
+    """
+
+    global atmost
+    global dim_max
+    global dim_num_save
+    global initialized
+    global lastq
+    global log_max
+    global maxcol
+    global poly
+    global recipd
+    global seed_save
+    global v
+
+    if not initialized or dim_num != dim_num_save:
+        initialized = 1
+        dim_max = 40
+        dim_num_save = -1
+        log_max = 30
+        seed_save = -1
+        #
+        #    Initialize (part of) V.
+        #
+        v = np.zeros((dim_max, log_max))
+        v[0:40, 0] = np.transpose([
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1
+        ])
+
+        v[2:40, 1] = np.transpose([
+            1, 3, 1, 3, 1, 3, 3, 1, 3, 1, 3, 1, 3, 1, 1, 3, 1, 3, 1, 3, 1, 3,
+            3, 1, 3, 1, 3, 1, 3, 1, 1, 3, 1, 3, 1, 3, 1, 3
+        ])
+
+        v[3:40, 2] = np.transpose([
+            7, 5, 1, 3, 3, 7, 5, 5, 7, 7, 1, 3, 3, 7, 5, 1, 1, 5, 3, 3, 1, 7,
+            5, 1, 3, 3, 7, 5, 1, 1, 5, 7, 7, 5, 1, 3, 3
+        ])
+
+        v[5:40, 3] = np.transpose([
+            1, 7, 9, 13, 11, 1, 3, 7, 9, 5, 13, 13, 11, 3, 15, 5, 3, 15, 7, 9,
+            13, 9, 1, 11, 7, 5, 15, 1, 15, 11, 5, 3, 1, 7, 9
+        ])
+
+        v[7:40, 4] = np.transpose([
+            9, 3, 27, 15, 29, 21, 23, 19, 11, 25, 7, 13, 17, 1, 25, 29, 3, 31,
+            11, 5, 23, 27, 19, 21, 5, 1, 17, 13, 7, 15, 9, 31, 9
+        ])
+
+        v[13:40, 5] = np.transpose([
+            37, 33, 7, 5, 11, 39, 63, 27, 17, 15, 23, 29, 3, 21, 13, 31, 25, 9,
+            49, 33, 19, 29, 11, 19, 27, 15, 25
+        ])
+
+        v[19:40, 6] = np.transpose([
+            13, 33, 115, 41, 79, 17, 29, 119, 75, 73, 105, 7, 59, 65, 21, 3,
+            113, 61, 89, 45, 107
+        ])
+
+        v[37:40, 7] = np.transpose([7, 23, 39])
+        #
+        #    Set POLY.
+        #
+        poly = [
+            1, 3, 7, 11, 13, 19, 25, 37, 59, 47, 61, 55, 41, 67, 97, 91, 109,
+            103, 115, 131, 193, 137, 145, 143, 241, 157, 185, 167, 229, 171,
+            213, 191, 253, 203, 211, 239, 247, 285, 369, 299
+        ]
+
+        atmost = 2**log_max - 1
+        #
+        #    Find the number of bits in ATMOST.
+        #
+        maxcol = i4_bit_hi1(atmost)
+        #
+        #    Initialize row 1 of V.
+        #
+        v[0, 0:maxcol] = 1
+
+        # Things to do only if the dimension changed.
+
+    if dim_num != dim_num_save:
+        #
+        #    Check parameters.
+        #
+        if (dim_num < 1 or dim_max < dim_num):
+            print('I4_SOBOL - Fatal error!')
+            print('    The spatial dimension DIM_NUM should satisfy:')
+            print('        1 <= DIM_NUM <= %d' % dim_max)
+            print('    But this input value is DIM_NUM = %d' % dim_num)
+            return None
+
+        dim_num_save = dim_num
+        #
+        #    Initialize the remaining rows of V.
+        #
+        for i in range(2, dim_num + 1):
+            #
+            #    The bits of the integer POLY(I) gives the form of polynomial
+            #    I.
+            #
+            #    Find the degree of polynomial I from binary encoding.
+            #
+            j = poly[i - 1]
+            m = 0
+            while True:
+                j = math.floor(j / 2.)
+                if (j <= 0):
+                    break
+                m = m + 1
+            #
+            #    Expand this bit pattern to separate components of the logical
+            #    array INCLUD.
+            #
+            j = poly[i - 1]
+            includ = np.zeros(m)
+            for k in range(m, 0, -1):
+                j2 = math.floor(j / 2.)
+                includ[k - 1] = (j != 2 * j2)
+                j = j2
+            #
+            #    Calculate the remaining elements of row I as explained
+            #    in Bratley and Fox, section 2.
+            #
+            for j in range(m + 1, maxcol + 1):
+                newv = v[i - 1, j - m - 1]
+                l_var = 1
+                for k in range(1, m + 1):
+                    l_var = 2 * l_var
+                    if (includ[k - 1]):
+                        newv = np.bitwise_xor(
+                            int(newv), int(l_var * v[i - 1, j - k - 1]))
+                v[i - 1, j - 1] = newv
+#
+#    Multiply columns of V by appropriate power of 2.
+#
+        l_var = 1
+        for j in range(maxcol - 1, 0, -1):
+            l_var = 2 * l_var
+            v[0:dim_num, j - 1] = v[0:dim_num, j - 1] * l_var
+#
+#    RECIPD is 1/(common denominator of the elements in V).
+#
+        recipd = 1.0 / (2 * l_var)
+        lastq = np.zeros(dim_num)
+
+    seed = int(math.floor(seed))
+
+    if (seed < 0):
+        seed = 0
+
+    if (seed == 0):
+        l_var = 1
+        lastq = np.zeros(dim_num)
+
+    elif (seed == seed_save + 1):
+        #
+        #    Find the position of the right-hand zero in SEED.
+        #
+        l_var = i4_bit_lo0(seed)
+
+    elif (seed <= seed_save):
+
+        seed_save = 0
+        lastq = np.zeros(dim_num)
+
+        for seed_temp in range(int(seed_save), int(seed)):
+            l_var = i4_bit_lo0(seed_temp)
+            for i in range(1, dim_num + 1):
+                lastq[i - 1] = np.bitwise_xor(
+                    int(lastq[i - 1]), int(v[i - 1, l_var - 1]))
+
+        l_var = i4_bit_lo0(seed)
+
+    elif (seed_save + 1 < seed):
+
+        for seed_temp in range(int(seed_save + 1), int(seed)):
+            l_var = i4_bit_lo0(seed_temp)
+            for i in range(1, dim_num + 1):
+                lastq[i - 1] = np.bitwise_xor(
+                    int(lastq[i - 1]), int(v[i - 1, l_var - 1]))
+
+        l_var = i4_bit_lo0(seed)
+#
+#    Check that the user is not calling too many times!
+#
+    if maxcol < l_var:
+        print('I4_SOBOL - Fatal error!')
+        print('    Too many calls!')
+        print('    MAXCOL = %d\n' % maxcol)
+        print('    L =            %d\n' % l_var)
+        return None
+
+
+#
+#    Calculate the new components of QUASI.
+#
+    quasi = np.zeros(dim_num)
+    for i in range(1, dim_num + 1):
+        quasi[i - 1] = lastq[i - 1] * recipd
+        lastq[i - 1] = np.bitwise_xor(
+            int(lastq[i - 1]), int(v[i - 1, l_var - 1]))
+
+    seed_save = seed
+    seed = seed + 1
+
+    return [quasi, seed]
+
+
+def i4_uniform_ab(a, b, seed):
+    """
+
+
+     I4_UNIFORM_AB returns a scaled pseudorandom I4.
+
+      Discussion:
+
+        The pseudorandom number will be scaled to be uniformly distributed
+        between A and B.
+
+      Licensing:
+
+        This code is distributed under the GNU LGPL license.
+
+      Modified:
+
+        05 April 2013
+
+      Author:
+
+        John Burkardt
+
+      Reference:
+
+        Paul Bratley, Bennett Fox, Linus Schrage,
+        A Guide to Simulation,
+        Second Edition,
+        Springer, 1987,
+        ISBN: 0387964673,
+        LC: QA76.9.C65.B73.
+
+        Bennett Fox,
+        Algorithm 647:
+        Implementation and Relative Efficiency of Quasirandom
+        Sequence Generators,
+        ACM Transactions on Mathematical Software,
+        Volume 12, Number 4, December 1986, pages 362-376.
+
+        Pierre L'Ecuyer,
+        Random Number Generation,
+        in Handbook of Simulation,
+        edited by Jerry Banks,
+        Wiley, 1998,
+        ISBN: 0471134031,
+        LC: T57.62.H37.
+
+        Peter Lewis, Allen Goodman, James Miller,
+        A Pseudo-Random Number Generator for the System/360,
+        IBM Systems Journal,
+        Volume 8, Number 2, 1969, pages 136-143.
+
+      Parameters:
+
+        Input, integer A, B, the minimum and maximum acceptable values.
+
+        Input, integer SEED, a seed for the random number generator.
+
+        Output, integer C, the randomly chosen integer.
+
+        Output, integer SEED, the updated seed.
+
+    """
+
+    i4_huge = 2147483647
+
+    seed = int(seed)
+
+    seed = (seed % i4_huge)
+
+    if seed < 0:
+        seed = seed + i4_huge
+
+    if seed == 0:
+        print('')
+        print('I4_UNIFORM_AB - Fatal error!')
+        print('  Input SEED = 0!')
+        sys.exit('I4_UNIFORM_AB - Fatal error!')
+
+    k = (seed // 127773)
+
+    seed = 167 * (seed - k * 127773) - k * 2836
+
+    if seed < 0:
+        seed = seed + i4_huge
+
+    r = seed * 4.656612875E-10
+    #
+    #  Scale R to lie between A-0.5 and B+0.5.
+    #
+    a = round(a)
+    b = round(b)
+
+    r = (1.0 - r) * (min(a, b) - 0.5) \
+        + r * (max(a, b) + 0.5)
+    #
+    #  Use rounding to convert R to an integer between A and B.
+    #
+    value = round(r)
+
+    value = max(value, min(a, b))
+    value = min(value, max(a, b))
+    value = int(value)
+
+    return value, seed
+
+
+def prime_ge(n):
+    """
+
+
+     PRIME_GE returns the smallest prime greater than or equal to N.
+
+      Example:
+
+          N    PRIME_GE
+
+        -10     2
+          1     2
+          2     2
+          3     3
+          4     5
+          5     5
+          6     7
+          7     7
+          8    11
+          9    11
+         10    11
+
+      Licensing:
+
+        This code is distributed under the MIT license.
+
+      Modified:
+
+        22 February 2011
+
+      Author:
+
+        Original MATLAB version by John Burkardt.
+        PYTHON version by Corrado Chisari
+
+      Parameters:
+
+        Input, integer N, the number to be bounded.
+
+        Output, integer P, the smallest prime number that is greater
+        than or equal to N.
+
+    """
+    p = max(math.ceil(n), 2)
+    while not isprime(p):
+        p = p + 1
+
+    return p
+
+
+def isprime(n):
+    """
+
+
+     IS_PRIME returns True if N is a prime number, False otherwise
+
+      Licensing:
+
+        This code is distributed under the MIT license.
+
+      Modified:
+
+        22 February 2011
+
+      Author:
+
+        Corrado Chisari
+
+      Parameters:
+
+        Input, integer N, the number to be checked.
+
+        Output, boolean value, True or False
+
+    """
+    if n != int(n) or n < 1:
+        return False
+    p = 2
+    while p < n:
+        if n % p == 0:
+            return False
+        p += 1
+
+    return True
+
+
+def r4_uniform_01(seed):
+    """
+
+
+     R4_UNIFORM_01 returns a unit pseudorandom R4.
+
+      Discussion:
+
+        This routine implements the recursion
+
+          seed = 167 * seed mod ( 2^31 - 1 )
+          r = seed / ( 2^31 - 1 )
+
+        The integer arithmetic never requires more than 32 bits,
+        including a sign bit.
+
+        If the initial seed is 12345, then the first three computations are
+
+          Input     Output      R4_UNIFORM_01
+          SEED      SEED
+
+             12345   207482415  0.096616
+         207482415  1790989824  0.833995
+        1790989824  2035175616  0.947702
+
+      Licensing:
+
+        This code is distributed under the GNU LGPL license.
+
+      Modified:
+
+        04 April 2013
+
+      Author:
+
+        John Burkardt
+
+      Reference:
+
+        Paul Bratley, Bennett Fox, Linus Schrage,
+        A Guide to Simulation,
+        Second Edition,
+        Springer, 1987,
+        ISBN: 0387964673,
+        LC: QA76.9.C65.B73.
+
+        Bennett Fox,
+        Algorithm 647:
+        Implementation and Relative Efficiency of Quasirandom
+        Sequence Generators,
+        ACM Transactions on Mathematical Software,
+        Volume 12, Number 4, December 1986, pages 362-376.
+
+        Pierre L'Ecuyer,
+        Random Number Generation,
+        in Handbook of Simulation,
+        edited by Jerry Banks,
+        Wiley, 1998,
+        ISBN: 0471134031,
+        LC: T57.62.H37.
+
+        Peter Lewis, Allen Goodman, James Miller,
+        A Pseudo-Random Number Generator for the System/360,
+        IBM Systems Journal,
+        Volume 8, Number 2, 1969, pages 136-143.
+
+      Parameters:
+
+        Input, integer SEED, the integer "seed" used to generate
+        the output random number.  SEED should not be 0.
+
+        Output, real R, a random value between 0 and 1.
+
+        Output, integer SEED, the updated seed.  This would
+        normally be used as the input seed on the next call.
+
+    """
+
+    i4_huge = 2147483647
+
+    if (seed == 0):
+        print('')
+        print('R4_UNIFORM_01 - Fatal error!')
+        print('  Input SEED = 0!')
+        sys.exit('R4_UNIFORM_01 - Fatal error!')
+
+    seed = (seed % i4_huge)
+
+    if seed < 0:
+        seed = seed + i4_huge
+
+    k = (seed // 127773)
+
+    seed = 167 * (seed - k * 127773) - k * 2836
+
+    if seed < 0:
+        seed = seed + i4_huge
+
+    r = seed * 4.656612875E-10
+
+    return r, seed
+
+
+def r8mat_write(filename, m, n, a):
+    """
+
+
+     R8MAT_WRITE writes an R8MAT to a file.
+
+      Licensing:
+
+        This code is distributed under the GNU LGPL license.
+
+      Modified:
+
+        12 October 2014
+
+      Author:
+
+        John Burkardt
+
+      Parameters:
+
+        Input, string FILENAME, the name of the output file.
+
+        Input, integer M, the number of rows in A.
+
+        Input, integer N, the number of columns in A.
+
+        Input, real A(M,N), the matrix.
+    """
+
+    with open(filename, 'w') as output:
+        for i in range(0, m):
+            for j in range(0, n):
+                s = '  %g' % (a[i, j])
+                output.write(s)
+            output.write('\n')
+
+
+def tau_sobol(dim_num):
+    """
+
+
+     TAU_SOBOL defines favorable starting seeds for Sobol sequences.
+
+      Discussion:
+
+        For spatial dimensions 1 through 13, this routine returns
+        a "favorable" value TAU by which an appropriate starting point
+        in the Sobol sequence can be determined.
+
+        These starting points have the form N = 2**K, where
+        for integration problems, it is desirable that
+                TAU + DIM_NUM - 1 <= K
+        while for optimization problems, it is desirable that
+                TAU < K.
+
+      Licensing:
+
+        This code is distributed under the MIT license.
+
+      Modified:
+
+        22 February 2011
+
+      Author:
+
+        Original FORTRAN77 version by Bennett Fox.
+        MATLAB version by John Burkardt.
+        PYTHON version by Corrado Chisari
+
+      Reference:
+
+        IA Antonov, VM Saleev,
+        USSR Computational Mathematics and Mathematical Physics,
+        Volume 19, 19, pages 252 - 256.
+
+        Paul Bratley, Bennett Fox,
+        Algorithm 659:
+        Implementing Sobol's Quasirandom Sequence Generator,
+        ACM Transactions on Mathematical Software,
+        Volume 14, Number 1, pages 88-100, 1988.
+
+        Bennett Fox,
+        Algorithm 647:
+        Implementation and Relative Efficiency of Quasirandom
+        Sequence Generators,
+        ACM Transactions on Mathematical Software,
+        Volume 12, Number 4, pages 362-376, 1986.
+
+        Stephen Joe, Frances Kuo
+        Remark on Algorithm 659:
+        Implementing Sobol's Quasirandom Sequence Generator,
+        ACM Transactions on Mathematical Software,
+        Volume 29, Number 1, pages 49-57, March 2003.
+
+        Ilya Sobol,
+        USSR Computational Mathematics and Mathematical Physics,
+        Volume 16, pages 236-242, 1977.
+
+        Ilya Sobol, YL Levitan,
+        The Production of Points Uniformly Distributed in a Multidimensional
+        Cube (in Russian),
+        Preprint IPM Akad. Nauk SSSR,
+        Number 40, Moscow 1976.
+
+      Parameters:
+
+                Input, integer DIM_NUM, the spatial dimension.    Only values
+                of 1 through 13 will result in useful responses.
+
+                Output, integer TAU, the value TAU.
+
+    """
+    dim_max = 13
+
+    tau_table = [0, 0, 1, 3, 5, 8, 11, 15, 19, 23, 27, 31, 35]
+
+    if 1 <= dim_num <= dim_max:
+        tau = tau_table[dim_num]
+    else:
+        tau = -1
+
+    return tau

tests/test_rrt_with_sobol_sampler.py

@@ -0,0 +1,30 @@
+import os
+import sys
+import random
+from unittest import TestCase
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
+sys.path.append(os.path.dirname(
+    os.path.abspath(__file__)) + "/../PathPlanning/RRT")
+
+try:
+    from PathPlanning.RRT import rrt_with_sobol_sampler as m
+except ImportError:
+    raise
+
+
+print(__file__)
+
+random.seed(12345)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main(gx=1.0, gy=1.0)
+
+
+if __name__ == '__main__':  # pragma: no cover
+    test = Test()
+    test.test1()