AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sobol sampler implemented (#413)

Browse Source
This commit is contained in:
Rafael Rojas
2021-01-10 05:26:40 +01:00
committed by GitHub
parent d5ce0351b6
commit 44142564cc
4 changed files with 1212 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

270
PathPlanning/RRT/rrt_with_sobol_sampler.py Normal file
View File

@@ -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()

1
PathPlanning/RRT/sobol/__init__.py Normal file
View File

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

911
PathPlanning/RRT/sobol/sobol.py Normal file
View File

@@ -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

30
tests/test_rrt_with_sobol_sampler.py Normal file
View File

@@ -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()