diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py new file mode 100644 index 0000000000..b88e51c680 --- /dev/null +++ b/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() diff --git a/PathPlanning/RRT/sobol/__init__.py b/PathPlanning/RRT/sobol/__init__.py new file mode 100644 index 0000000000..c95ac8983b --- /dev/null +++ b/PathPlanning/RRT/sobol/__init__.py @@ -0,0 +1 @@ +from .sobol import i4_sobol as sobol_quasirand diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py new file mode 100644 index 0000000000..428ba58a98 --- /dev/null +++ b/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 diff --git a/tests/test_rrt_with_sobol_sampler.py b/tests/test_rrt_with_sobol_sampler.py new file mode 100644 index 0000000000..43f25e85bc --- /dev/null +++ b/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()