From 3b957c41d3c07d5ae443f571ab2a3258f52fc53a Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 16:17:47 +0200 Subject: [PATCH 01/14] Sobol distribution implemented --- PathPlanning/RRT/rrt.py | 11 + PathPlanning/RRT/sobol/__init__.py | 1 + PathPlanning/RRT/sobol/sobol.py | 905 +++++++++++++++++++++++++++++ 3 files changed, 917 insertions(+) create mode 100644 PathPlanning/RRT/sobol/__init__.py create mode 100644 PathPlanning/RRT/sobol/sobol.py diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 834fb24e39..0e6a8fd3b5 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -14,6 +14,11 @@ show_animation = True +import sys +import os +sys.path.append(os.path.dirname(os.path.abspath(__file__))) +from sobol import sobol_quasirand + class RRT: """ @@ -60,6 +65,7 @@ def __init__(self, self.max_iter = max_iter self.obstacle_list = obstacle_list self.node_list = [] + self.sobol_inter_ = 0 def planning(self, animation=True): """ @@ -144,6 +150,11 @@ def get_random_node(self): rnd = self.Node( random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)) + 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 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..daebe630fc --- /dev/null +++ b/PathPlanning/RRT/sobol/sobol.py @@ -0,0 +1,905 @@ +""" + 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 from http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html +""" +import math +import datetime +from numpy import * + +#from numpy import bitwise_xor + + +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): + """ + #*****************************************************************************80 + # + # 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): + """ + #*****************************************************************************80 + # + # 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 = 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): + """ + #*****************************************************************************80 + # + # 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, 1980, 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' in globals().keys(): + initialized = 0 + dim_num_save = -1 + + 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 = zeros((dim_max, log_max)) + v[0:40, 0] = 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] = 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] = 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] = 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] = 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] = 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] = 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] = 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 + + 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 = 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 = 1 + for k in range(1, m + 1): + l = 2 * l + if (includ[k - 1]): + newv = bitwise_xor( + int(newv), int(l * v[i - 1, j - k - 1])) + v[i - 1, j - 1] = newv +# +# Multiply columns of V by appropriate power of 2. +# + l = 1 + for j in range(maxcol - 1, 0, -1): + l = 2 * l + v[0:dim_num, j - 1] = v[0:dim_num, j - 1] * l +# +# RECIPD is 1/(common denominator of the elements in V). +# + recipd = 1.0 / (2 * l) + lastq = zeros(dim_num) + + seed = int(math.floor(seed)) + + if (seed < 0): + seed = 0 + + if (seed == 0): + l = 1 + lastq = zeros(dim_num) + + elif (seed == seed_save + 1): + # + # Find the position of the right-hand zero in SEED. + # + l = i4_bit_lo0(seed) + + elif (seed <= seed_save): + + seed_save = 0 + l = 1 + lastq = zeros(dim_num) + + for seed_temp in range(int(seed_save), int(seed)): + l = i4_bit_lo0(seed_temp) + for i in range(1, dim_num + 1): + lastq[i - 1] = bitwise_xor( + int(lastq[i - 1]), int(v[i - 1, l - 1])) + + l = i4_bit_lo0(seed) + + elif (seed_save + 1 < seed): + + for seed_temp in range(int(seed_save + 1), int(seed)): + l = i4_bit_lo0(seed_temp) + for i in range(1, dim_num + 1): + lastq[i - 1] = bitwise_xor( + int(lastq[i - 1]), int(v[i - 1, l - 1])) + + l = i4_bit_lo0(seed) +# +# Check that the user is not calling too many times! +# + if maxcol < l: + print('I4_SOBOL - Fatal error!') + print(' Too many calls!') + print(' MAXCOL = %d\n' % maxcol) + print(' L = %d\n' % l) + return + + +# +# Calculate the new components of QUASI. +# + quasi = zeros(dim_num) + for i in range(1, dim_num + 1): + quasi[i - 1] = lastq[i - 1] * recipd + lastq[i - 1] = bitwise_xor(int(lastq[i - 1]), int(v[i - 1, l - 1])) + + seed_save = seed + seed = seed + 1 + + return [quasi, seed] + + +def i4_uniform_ab(a, b, seed): + """ + #*****************************************************************************80 + # + # 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. + # + """ + from sys import exit + + 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!') + exit('I4_UNIFORM_AB - Fatal error!') + + k = (seed // 127773) + + seed = 16807 * (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): + """ + #*****************************************************************************80 + # + # 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): + """ + #*****************************************************************************80 + # + # 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): + """ + #*****************************************************************************80 + # + # R4_UNIFORM_01 returns a unit pseudorandom R4. + # + # Discussion: + # + # This routine implements the recursion + # + # seed = 16807 * 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. + # + """ + from sys import exit + + i4_huge = 2147483647 + + if (seed == 0): + print('') + print('R4_UNIFORM_01 - Fatal error!') + print(' Input SEED = 0!') + exit('R4_UNIFORM_01 - Fatal error!') + + seed = (seed % i4_huge) + + if seed < 0: + seed = seed + i4_huge + + k = (seed // 127773) + + seed = 16807 * (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): + """ + #*****************************************************************************80 + # + # 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. + # + """ + output = open(filename, 'w') + + for i in range(0, m): + for j in range(0, n): + s = ' %g' % (a[i, j]) + output.write(s) + output.write('\n') + + output.close() + + return + + +def tau_sobol(dim_num): + """ + #*****************************************************************************80 + # + # 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, 1980, 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 and dim_num <= dim_max: + tau = tau_table[dim_num] + else: + tau = -1 + + return tau From ddbb5c57fba37e7279fd7d476d40751f4df18edf Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 16:35:41 +0200 Subject: [PATCH 02/14] sobol.py fixed for CodeFactor --- PathPlanning/RRT/sobol/sobol.py | 60 +++++++++++++-------------------- 1 file changed, 24 insertions(+), 36 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index daebe630fc..5364667c0d 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -16,9 +16,10 @@ """ import math import datetime -from numpy import * +import sys +import numpy as np -#from numpy import bitwise_xor +# from numpy import np.bitwise_xor def i4_bit_hi1(n): @@ -193,7 +194,7 @@ def i4_sobol_generate(m, n, skip): # Output, real R(M,N), the points. # """ - r = zeros((m, n)) + 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) @@ -268,17 +269,6 @@ def i4_sobol(dim_num, seed): # 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' in globals().keys(): initialized = 0 @@ -293,43 +283,43 @@ def i4_sobol(dim_num, seed): # # Initialize (part of) V. # - v = zeros((dim_max, log_max)) - v[0:40, 0] = transpose([ + 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] = transpose([ + 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] = transpose([ + 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] = transpose([ + 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] = transpose([ + 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] = transpose([ + 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] = transpose([ + 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] = transpose([7, 23, 39]) + v[37:40, 7] = np.transpose([7, 23, 39]) # # Set POLY. # @@ -384,7 +374,7 @@ def i4_sobol(dim_num, seed): # Expand this bit pattern to separate components of the logical array INCLUD. # j = poly[i - 1] - includ = zeros(m) + includ = np.zeros(m) for k in range(m, 0, -1): j2 = math.floor(j / 2.) includ[k - 1] = (j != 2 * j2) @@ -399,7 +389,7 @@ def i4_sobol(dim_num, seed): for k in range(1, m + 1): l = 2 * l if (includ[k - 1]): - newv = bitwise_xor( + newv = np.bitwise_xor( int(newv), int(l * v[i - 1, j - k - 1])) v[i - 1, j - 1] = newv # @@ -413,7 +403,7 @@ def i4_sobol(dim_num, seed): # RECIPD is 1/(common denominator of the elements in V). # recipd = 1.0 / (2 * l) - lastq = zeros(dim_num) + lastq = np.zeros(dim_num) seed = int(math.floor(seed)) @@ -422,7 +412,7 @@ def i4_sobol(dim_num, seed): if (seed == 0): l = 1 - lastq = zeros(dim_num) + lastq = np.zeros(dim_num) elif (seed == seed_save + 1): # @@ -434,12 +424,12 @@ def i4_sobol(dim_num, seed): seed_save = 0 l = 1 - lastq = zeros(dim_num) + lastq = np.zeros(dim_num) for seed_temp in range(int(seed_save), int(seed)): l = i4_bit_lo0(seed_temp) for i in range(1, dim_num + 1): - lastq[i - 1] = bitwise_xor( + lastq[i - 1] = np.bitwise_xor( int(lastq[i - 1]), int(v[i - 1, l - 1])) l = i4_bit_lo0(seed) @@ -449,7 +439,7 @@ def i4_sobol(dim_num, seed): for seed_temp in range(int(seed_save + 1), int(seed)): l = i4_bit_lo0(seed_temp) for i in range(1, dim_num + 1): - lastq[i - 1] = bitwise_xor( + lastq[i - 1] = np.bitwise_xor( int(lastq[i - 1]), int(v[i - 1, l - 1])) l = i4_bit_lo0(seed) @@ -467,10 +457,10 @@ def i4_sobol(dim_num, seed): # # Calculate the new components of QUASI. # - quasi = zeros(dim_num) + quasi = np.zeros(dim_num) for i in range(1, dim_num + 1): quasi[i - 1] = lastq[i - 1] * recipd - lastq[i - 1] = bitwise_xor(int(lastq[i - 1]), int(v[i - 1, l - 1])) + lastq[i - 1] = np.bitwise_xor(int(lastq[i - 1]), int(v[i - 1, l - 1])) seed_save = seed seed = seed + 1 @@ -541,7 +531,6 @@ def i4_uniform_ab(a, b, seed): # Output, integer SEED, the updated seed. # """ - from sys import exit i4_huge = 2147483647 @@ -556,7 +545,7 @@ def i4_uniform_ab(a, b, seed): print('') print('I4_UNIFORM_AB - Fatal error!') print(' Input SEED = 0!') - exit('I4_UNIFORM_AB - Fatal error!') + sys.exit('I4_UNIFORM_AB - Fatal error!') k = (seed // 127773) @@ -749,7 +738,6 @@ def r4_uniform_01(seed): # normally be used as the input seed on the next call. # """ - from sys import exit i4_huge = 2147483647 @@ -757,7 +745,7 @@ def r4_uniform_01(seed): print('') print('R4_UNIFORM_01 - Fatal error!') print(' Input SEED = 0!') - exit('R4_UNIFORM_01 - Fatal error!') + sys.exit('R4_UNIFORM_01 - Fatal error!') seed = (seed % i4_huge) From d9fd72f031752dec3ea45948708bc01a4aaac58c Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 16:38:11 +0200 Subject: [PATCH 03/14] sobol.py fixed for CodeFactor --- PathPlanning/RRT/sobol/sobol.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 5364667c0d..c84f23140b 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -351,7 +351,7 @@ def i4_sobol(dim_num, seed): 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 + return None dim_num_save = dim_num # @@ -451,7 +451,7 @@ def i4_sobol(dim_num, seed): print(' Too many calls!') print(' MAXCOL = %d\n' % maxcol) print(' L = %d\n' % l) - return + return None # @@ -803,8 +803,6 @@ def r8mat_write(filename, m, n, a): output.close() - return - def tau_sobol(dim_num): """ From 6658a86114f9cfd22b404bf03d67675910ec30c2 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 16:46:11 +0200 Subject: [PATCH 04/14] sobol.py fixed for python3.8 CI --- PathPlanning/RRT/rrt.py | 7 +- PathPlanning/RRT/sobol/sobol.py | 918 ++++++++++++++++---------------- 2 files changed, 461 insertions(+), 464 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 0e6a8fd3b5..ed4fffe122 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -12,13 +12,13 @@ import matplotlib.pyplot as plt import numpy as np -show_animation = True - import sys import os sys.path.append(os.path.dirname(os.path.abspath(__file__))) from sobol import sobol_quasirand +show_animation = True + class RRT: """ @@ -147,9 +147,6 @@ def calc_dist_to_goal(self, x, y): def get_random_node(self): if random.randint(0, 100) > self.goal_sample_rate: - rnd = self.Node( - random.uniform(self.min_rand, self.max_rand), - random.uniform(self.min_rand, self.max_rand)) rand_coordinates, n = sobol_quasirand(2, self.sobol_inter_) rand_coordinates = self.min_rand + \ rand_coordinates*(self.max_rand - self.min_rand) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index c84f23140b..d149ab351c 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -15,7 +15,6 @@ Original code is available from http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html """ import math -import datetime import sys import numpy as np @@ -94,59 +93,59 @@ def i4_bit_hi1(n): def i4_bit_lo0(n): """ - #*****************************************************************************80 - # - # 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. - # + *****************************************************************************80 + + 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 @@ -166,33 +165,33 @@ def i4_bit_lo0(n): def i4_sobol_generate(m, n, skip): """ - #*****************************************************************************80 - # - # 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. - # + *****************************************************************************80 + + 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): @@ -203,71 +202,71 @@ def i4_sobol_generate(m, n, skip): def i4_sobol(dim_num, seed): """ - #*****************************************************************************80 - # - # 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, 1980, 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. - # + *****************************************************************************80 + + 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, 1980, 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. + """ if not 'initialized' in globals().keys(): @@ -385,24 +384,24 @@ def i4_sobol(dim_num, seed): # for j in range(m + 1, maxcol + 1): newv = v[i - 1, j - m - 1] - l = 1 + l_var = 1 for k in range(1, m + 1): - l = 2 * l + l_var = 2 * l_var if (includ[k - 1]): newv = np.bitwise_xor( - int(newv), int(l * v[i - 1, j - k - 1])) + 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 = 1 + l_var = 1 for j in range(maxcol - 1, 0, -1): - l = 2 * l - v[0:dim_num, j - 1] = v[0:dim_num, j - 1] * l + 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) + recipd = 1.0 / (2 * l_var) lastq = np.zeros(dim_num) seed = int(math.floor(seed)) @@ -411,46 +410,46 @@ def i4_sobol(dim_num, seed): seed = 0 if (seed == 0): - l = 1 + l_var = 1 lastq = np.zeros(dim_num) elif (seed == seed_save + 1): # # Find the position of the right-hand zero in SEED. # - l = i4_bit_lo0(seed) + l_var = i4_bit_lo0(seed) elif (seed <= seed_save): seed_save = 0 - l = 1 + l_var = 1 lastq = np.zeros(dim_num) for seed_temp in range(int(seed_save), int(seed)): - l = i4_bit_lo0(seed_temp) + 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 - 1])) + int(lastq[i - 1]), int(v[i - 1, l_var - 1])) - l = i4_bit_lo0(seed) + l_var = i4_bit_lo0(seed) elif (seed_save + 1 < seed): for seed_temp in range(int(seed_save + 1), int(seed)): - l = i4_bit_lo0(seed_temp) + 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 - 1])) + int(lastq[i - 1]), int(v[i - 1, l_var - 1])) - l = i4_bit_lo0(seed) + l_var = i4_bit_lo0(seed) # # Check that the user is not calling too many times! # - if maxcol < l: + if maxcol < l_var: print('I4_SOBOL - Fatal error!') print(' Too many calls!') print(' MAXCOL = %d\n' % maxcol) - print(' L = %d\n' % l) + print(' L = %d\n' % l_var) return None @@ -460,7 +459,8 @@ def i4_sobol(dim_num, seed): 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 - 1])) + lastq[i - 1] = np.bitwise_xor( + int(lastq[i - 1]), int(v[i - 1, l_var - 1])) seed_save = seed seed = seed + 1 @@ -470,66 +470,66 @@ def i4_sobol(dim_num, seed): def i4_uniform_ab(a, b, seed): """ - #*****************************************************************************80 - # - # 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. - # + *****************************************************************************80 + + 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 @@ -577,46 +577,46 @@ def i4_uniform_ab(a, b, seed): def prime_ge(n): """ - #*****************************************************************************80 - # - # 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. - # + *****************************************************************************80 + + 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): @@ -627,28 +627,28 @@ def prime_ge(n): def isprime(n): """ - #*****************************************************************************80 - # - # 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 - # + *****************************************************************************80 + + 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 @@ -663,80 +663,80 @@ def isprime(n): def r4_uniform_01(seed): """ - #*****************************************************************************80 - # - # R4_UNIFORM_01 returns a unit pseudorandom R4. - # - # Discussion: - # - # This routine implements the recursion - # - # seed = 16807 * 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. - # + *****************************************************************************80 + + R4_UNIFORM_01 returns a unit pseudorandom R4. + + Discussion: + + This routine implements the recursion + + seed = 16807 * 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 @@ -766,32 +766,32 @@ def r4_uniform_01(seed): def r8mat_write(filename, m, n, a): """ - #*****************************************************************************80 - # - # 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. - # + *****************************************************************************80 + + 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. + """ output = open(filename, 'w') @@ -806,78 +806,78 @@ def r8mat_write(filename, m, n, a): def tau_sobol(dim_num): """ - #*****************************************************************************80 - # - # 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, 1980, 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. - # + *****************************************************************************80 + + 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, 1980, 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 From c7f1375c7af25becf1206505a8d009fb3c449209 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 16:54:19 +0200 Subject: [PATCH 05/14] sobol.py fixed for python3.8 CI --- PathPlanning/RRT/rrt.py | 4 +- PathPlanning/RRT/sobol/sobol.py | 119 ++++++++++++++++---------------- 2 files changed, 62 insertions(+), 61 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index ed4fffe122..ee97707b84 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -9,11 +9,11 @@ import math import random +import sys +import os import matplotlib.pyplot as plt import numpy as np -import sys -import os sys.path.append(os.path.dirname(os.path.abspath(__file__))) from sobol import sobol_quasirand diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index d149ab351c..95893e35b6 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -12,7 +12,8 @@ Original MATLAB versions of other functions by John Burkardt. PYTHON versions by Corrado Chisari - Original code is available from http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html + Original code is available at + http://people.sc.fsu.edu/~jburkardt/py_src/sobol/sobol.html """ import math import sys @@ -93,7 +94,7 @@ def i4_bit_hi1(n): def i4_bit_lo0(n): """ - *****************************************************************************80 + I4_BIT_LO0 returns the position of the low 0 bit base 2 in an I4. @@ -165,7 +166,7 @@ def i4_bit_lo0(n): def i4_sobol_generate(m, n, skip): """ - *****************************************************************************80 + I4_SOBOL_GENERATE generates a Sobol dataset. @@ -202,7 +203,7 @@ def i4_sobol_generate(m, n, skip): def i4_sobol(dim_num, seed): """ - *****************************************************************************80 + I4_SOBOL generates a new quasirandom Sobol vector with each call. @@ -228,7 +229,7 @@ def i4_sobol(dim_num, seed): Antonov, Saleev, USSR Computational Mathematics and Mathematical Physics, - olume 19, 1980, pages 252 - 256. + olume 19, 19, pages 252 - 256. Paul Bratley, Bennett Fox, Algorithm 659: @@ -470,7 +471,7 @@ def i4_sobol(dim_num, seed): def i4_uniform_ab(a, b, seed): """ - *****************************************************************************80 + I4_UNIFORM_AB returns a scaled pseudorandom I4. @@ -549,7 +550,7 @@ def i4_uniform_ab(a, b, seed): k = (seed // 127773) - seed = 16807 * (seed - k * 127773) - k * 2836 + seed = 167 * (seed - k * 127773) - k * 2836 if seed < 0: seed = seed + i4_huge @@ -577,7 +578,7 @@ def i4_uniform_ab(a, b, seed): def prime_ge(n): """ - *****************************************************************************80 + PRIME_GE returns the smallest prime greater than or equal to N. @@ -627,7 +628,7 @@ def prime_ge(n): def isprime(n): """ - *****************************************************************************80 + IS_PRIME returns True if N is a prime number, False otherwise @@ -663,7 +664,7 @@ def isprime(n): def r4_uniform_01(seed): """ - *****************************************************************************80 + R4_UNIFORM_01 returns a unit pseudorandom R4. @@ -671,7 +672,7 @@ def r4_uniform_01(seed): This routine implements the recursion - seed = 16807 * seed mod ( 2^31 - 1 ) + seed = 167 * seed mod ( 2^31 - 1 ) r = seed / ( 2^31 - 1 ) The integer arithmetic never requires more than 32 bits, @@ -754,7 +755,7 @@ def r4_uniform_01(seed): k = (seed // 127773) - seed = 16807 * (seed - k * 127773) - k * 2836 + seed = 167 * (seed - k * 127773) - k * 2836 if seed < 0: seed = seed + i4_huge @@ -766,7 +767,7 @@ def r4_uniform_01(seed): def r8mat_write(filename, m, n, a): """ - *****************************************************************************80 + R8MAT_WRITE writes an R8MAT to a file. @@ -806,70 +807,70 @@ def r8mat_write(filename, m, n, a): def tau_sobol(dim_num): """ - *****************************************************************************80 + 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. + 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. + 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. + This code is distributed under the MIT license. Modified: - 22 February 2011 + 22 February 2011 Author: - Original FORTRAN77 version by Bennett Fox. - MATLAB version by John Burkardt. - PYTHON version by Corrado Chisari + 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, 1980, 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. + 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: From 7c26eb6f9b596a6375d505461b4c0153d62c1726 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 17:11:51 +0200 Subject: [PATCH 06/14] sobol.py fixed for python3.8 CI --- PathPlanning/RRT/sobol/sobol.py | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 95893e35b6..48032be971 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -21,6 +21,18 @@ # from numpy import np.bitwise_xor +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): """ @@ -270,11 +282,23 @@ def i4_sobol(dim_num, seed): """ - if not 'initialized' in globals().keys(): + 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 'initialized' is None: initialized = 0 dim_num_save = -1 - if (not initialized or dim_num != dim_num_save): + if not initialized or dim_num != dim_num_save: initialized = 1 dim_max = 40 dim_num_save = -1 From 37c56d26b3e15c28b592200c290fda6e7dd36d04 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 17:17:52 +0200 Subject: [PATCH 07/14] sobol.py fixed for python3.8 CI, tabs where hidded, now fixed --- PathPlanning/RRT/sobol/sobol.py | 62 ++++++++++++++++----------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 48032be971..03b3dc9f1f 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -273,7 +273,7 @@ def i4_sobol(dim_num, seed): 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 + 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. @@ -305,7 +305,7 @@ def i4_sobol(dim_num, seed): log_max = 30 seed_save = -1 # - # Initialize (part of) V. + # Initialize (part of) V. # v = np.zeros((dim_max, log_max)) v[0:40, 0] = np.transpose([ @@ -345,7 +345,7 @@ def i4_sobol(dim_num, seed): v[37:40, 7] = np.transpose([7, 23, 39]) # - # Set POLY. + # Set POLY. # poly = [ 1, 3, 7, 11, 13, 19, 25, 37, 59, 47, 61, 55, 41, 67, 97, 91, 109, @@ -355,37 +355,36 @@ def i4_sobol(dim_num, seed): atmost = 2**log_max - 1 # - # Find the number of bits in ATMOST. + # Find the number of bits in ATMOST. # maxcol = i4_bit_hi1(atmost) # - # Initialize row 1 of V. + # Initialize row 1 of V. # v[0, 0:maxcol] = 1 -# -# Things to do only if the dimension changed. -# + # Things to do only if the dimension changed. + if dim_num != dim_num_save: # - # Check parameters. + # 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) + 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. + # 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. + # The bits of the integer POLY(I) gives the form of polynomial I. # - # Find the degree of polynomial I from binary encoding. + # Find the degree of polynomial I from binary encoding. # j = poly[i - 1] m = 0 @@ -394,19 +393,20 @@ def i4_sobol(dim_num, seed): if (j <= 0): break m = m + 1 -# -# Expand this bit pattern to separate components of the logical array INCLUD. -# + # + # 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. -# + # + # 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 @@ -417,14 +417,14 @@ def i4_sobol(dim_num, seed): 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. +# 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 is 1/(common denominator of the elements in V). # recipd = 1.0 / (2 * l_var) lastq = np.zeros(dim_num) @@ -440,7 +440,7 @@ def i4_sobol(dim_num, seed): elif (seed == seed_save + 1): # - # Find the position of the right-hand zero in SEED. + # Find the position of the right-hand zero in SEED. # l_var = i4_bit_lo0(seed) @@ -468,18 +468,18 @@ def i4_sobol(dim_num, seed): l_var = i4_bit_lo0(seed) # -# Check that the user is not calling too many times! +# 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) + print(' Too many calls!') + print(' MAXCOL = %d\n' % maxcol) + print(' L = %d\n' % l_var) return None # -# Calculate the new components of QUASI. +# Calculate the new components of QUASI. # quasi = np.zeros(dim_num) for i in range(1, dim_num + 1): @@ -898,7 +898,7 @@ def tau_sobol(dim_num): Parameters: - Input, integer DIM_NUM, the spatial dimension. Only values + Input, integer DIM_NUM, the spatial dimension. Only values of 1 through 13 will result in useful responses. Output, integer TAU, the value TAU. From eba410c2cf0120fa81d048131f3d33c9123c8b50 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Thu, 1 Oct 2020 18:07:49 +0200 Subject: [PATCH 08/14] Trying to fix E402 in rrt.py and E501 in sobol.py --- PathPlanning/RRT/rrt.py | 6 +++++- PathPlanning/RRT/sobol/sobol.py | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index ee97707b84..348d275849 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -15,7 +15,11 @@ import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__))) -from sobol import sobol_quasirand + +try: + from sobol import sobol_quasirand +except ImportError: + raise show_animation = True diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 03b3dc9f1f..9808110bee 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -382,7 +382,8 @@ def i4_sobol(dim_num, seed): # for i in range(2, dim_num + 1): # - # The bits of the integer POLY(I) gives the form of polynomial I. + # The bits of the integer POLY(I) gives the form of polynomial + # I. # # Find the degree of polynomial I from binary encoding. # From 4400786c4e40e67b09bb98ff2bfb9f266c444eff Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Sun, 25 Oct 2020 23:16:17 +0100 Subject: [PATCH 09/14] single file with rrt implementation with sobol added --- PathPlanning/RRT/rrt.py | 18 +- PathPlanning/RRT/rrt_with_sobol_sampler.py | 249 +++++++++++++++++++++ 2 files changed, 252 insertions(+), 15 deletions(-) create mode 100644 PathPlanning/RRT/rrt_with_sobol_sampler.py diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 348d275849..834fb24e39 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -9,18 +9,9 @@ import math import random -import sys -import os import matplotlib.pyplot as plt import numpy as np -sys.path.append(os.path.dirname(os.path.abspath(__file__))) - -try: - from sobol import sobol_quasirand -except ImportError: - raise - show_animation = True @@ -69,7 +60,6 @@ def __init__(self, self.max_iter = max_iter self.obstacle_list = obstacle_list self.node_list = [] - self.sobol_inter_ = 0 def planning(self, animation=True): """ @@ -151,11 +141,9 @@ def calc_dist_to_goal(self, x, y): 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) + rnd = self.Node( + random.uniform(self.min_rand, self.max_rand), + random.uniform(self.min_rand, self.max_rand)) else: # goal point sampling rnd = self.Node(self.end.x, self.end.y) return rnd diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py new file mode 100644 index 0000000000..1000114515 --- /dev/null +++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py @@ -0,0 +1,249 @@ +""" + +Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRTSobol) + +author: AtsushiSakai(@Atsushi_twi) + +""" + +import math +import random +from sobol import sobol_quasirand + +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] + obstacleList: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: [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, obstacleList): + + if node is None: + return False + + for (ox, oy, size) in obstacleList: + 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==== + obstacleList = [(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=obstacleList) + 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() From 9a8bb39f6b01d17e5175a6d7019d428b9aa6eabf Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Mon, 26 Oct 2020 15:24:58 +0100 Subject: [PATCH 10/14] CI error, line 3 too long fixed --- PathPlanning/RRT/rrt_with_sobol_sampler.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py index 1000114515..9379bca65c 100644 --- a/PathPlanning/RRT/rrt_with_sobol_sampler.py +++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py @@ -1,6 +1,7 @@ """ -Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRTSobol) +Path planning Sample Code with Randomized Rapidly-Exploring Random +Trees with sobol sampler(RRTSobol) author: AtsushiSakai(@Atsushi_twi) From 4b13c913d922b2c2dcefd902a39274c038d0c8e7 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Fri, 27 Nov 2020 11:30:12 +0100 Subject: [PATCH 11/14] [PathPlanning RRT] sobol fixed and test added --- PathPlanning/RRT/rrt_with_sobol_sampler.py | 38 +++++++++++++++++----- PathPlanning/RRT/sobol/sobol.py | 8 +---- tests/test_rrt_with_sobol_sampler.py | 27 +++++++++++++++ 3 files changed, 57 insertions(+), 16 deletions(-) create mode 100644 tests/test_rrt_with_sobol_sampler.py diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py index 9379bca65c..b88e51c680 100644 --- a/PathPlanning/RRT/rrt_with_sobol_sampler.py +++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py @@ -1,15 +1,35 @@ """ Path planning Sample Code with Randomized Rapidly-Exploring Random -Trees with sobol sampler(RRTSobol) +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) -author: AtsushiSakai(@Atsushi_twi) """ import math import random from sobol import sobol_quasirand +import sys import matplotlib.pyplot as plt import numpy as np @@ -48,7 +68,7 @@ def __init__(self, start:Start Position [x,y] goal:Goal Position [x,y] - obstacleList:obstacle Positions [[x,y,size],...] + obstacle_list:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] """ @@ -160,7 +180,7 @@ def draw_graph(self, rnd=None): # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + 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: @@ -194,12 +214,12 @@ def get_nearest_node_index(node_list, rnd_node): return minind @staticmethod - def check_collision(node, obstacleList): + def check_collision(node, obstacle_list): if node is None: return False - for (ox, oy, size) in obstacleList: + 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)] @@ -222,14 +242,14 @@ def main(gx=6.0, gy=10.0): print("start " + __file__) # ====Search Path with RRTSobol==== - obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), - (9, 5, 2), (8, 10, 1)] # [x, y, radius] + 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=obstacleList) + obstacle_list=obstacle_list) path = rrt.planning(animation=show_animation) if path is None: diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 9808110bee..34b98b5e68 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -19,8 +19,6 @@ import sys import numpy as np -# from numpy import np.bitwise_xor - atmost = None dim_max = None dim_num_save = None @@ -294,10 +292,6 @@ def i4_sobol(dim_num, seed): global seed_save global v - if 'initialized' is None: - initialized = 0 - dim_num_save = -1 - if not initialized or dim_num != dim_num_save: initialized = 1 dim_max = 40 @@ -817,8 +811,8 @@ def r8mat_write(filename, m, n, a): Input, integer N, the number of columns in A. Input, real A(M,N), the matrix. - """ + output = open(filename, 'w') for i in range(0, m): diff --git a/tests/test_rrt_with_sobol_sampler.py b/tests/test_rrt_with_sobol_sampler.py new file mode 100644 index 0000000000..df5a1dfda7 --- /dev/null +++ b/tests/test_rrt_with_sobol_sampler.py @@ -0,0 +1,27 @@ +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() From fbebc7d2289a26fe1565e70cea9c2d7121840061 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Sat, 28 Nov 2020 07:58:00 +0100 Subject: [PATCH 12/14] [test] rrt with sovol sampler test --- tests/test_rrt_with_sobol_sampler.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/test_rrt_with_sobol_sampler.py b/tests/test_rrt_with_sobol_sampler.py index df5a1dfda7..43f25e85bc 100644 --- a/tests/test_rrt_with_sobol_sampler.py +++ b/tests/test_rrt_with_sobol_sampler.py @@ -4,7 +4,9 @@ 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") +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: @@ -22,6 +24,7 @@ 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() From 6b219a79dbc6f6680f5e28ecbd528a6553c47d96 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Wed, 6 Jan 2021 12:18:26 +0100 Subject: [PATCH 13/14] sobol quality --- PathPlanning/RRT/sobol/sobol.py | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index 34b98b5e68..b6d41ea828 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -14,6 +14,13 @@ 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 @@ -34,7 +41,6 @@ def i4_bit_hi1(n): """ - I4_BIT_HI1 returns the position of the high 1 bit base 2 in an I4. Discussion: @@ -104,8 +110,6 @@ def i4_bit_hi1(n): def i4_bit_lo0(n): """ - - I4_BIT_LO0 returns the position of the low 0 bit base 2 in an I4. Discussion: @@ -813,15 +817,12 @@ def r8mat_write(filename, m, n, a): Input, real A(M,N), the matrix. """ - output = open(filename, 'w') - - for i in range(0, m): - for j in range(0, n): - s = ' %g' % (a[i, j]) - output.write(s) - output.write('\n') - - output.close() + 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): From 146e3f10c6d4649b1b585ffd5149489ea0a692d6 Mon Sep 17 00:00:00 2001 From: Rojas Rafael Date: Wed, 6 Jan 2021 12:53:28 +0100 Subject: [PATCH 14/14] sobol quality --- PathPlanning/RRT/sobol/sobol.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/PathPlanning/RRT/sobol/sobol.py b/PathPlanning/RRT/sobol/sobol.py index b6d41ea828..428ba58a98 100644 --- a/PathPlanning/RRT/sobol/sobol.py +++ b/PathPlanning/RRT/sobol/sobol.py @@ -446,7 +446,6 @@ def i4_sobol(dim_num, seed): elif (seed <= seed_save): seed_save = 0 - l_var = 1 lastq = np.zeros(dim_num) for seed_temp in range(int(seed_save), int(seed)): @@ -904,7 +903,7 @@ def tau_sobol(dim_num): tau_table = [0, 0, 1, 3, 5, 8, 11, 15, 19, 23, 27, 31, 35] - if 1 <= dim_num and dim_num <= dim_max: + if 1 <= dim_num <= dim_max: tau = tau_table[dim_num] else: tau = -1