From 83b693062d2a9aed7388201723ac9367ddd8f841 Mon Sep 17 00:00:00 2001 From: Pras Velagapudi Date: Thu, 28 Apr 2016 21:55:49 -0400 Subject: [PATCH] Changes VanDerCorputSampleGenerator to include goal. This refactors VanDerCorputSampleGenerator to include the start and end (goal) values as the first two samples to be checked, followed by the discretized intermediate checks within the interval range. This is an alternate way to address #297 and #303, but avoids the non-termination problem introduced in #303 by simplifying the generator loop. The resulting output is identical to the expected output of #303. --- src/prpy/util.py | 68 ++++++++++++++++-------------------------------- 1 file changed, 22 insertions(+), 46 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index 42dc3fc..0470c70 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -1510,8 +1510,7 @@ def VanDerCorputSampleGenerator(start, end, step=2): Generates a sequence of values from start to end, with specified step size, using an approximate binary Van der Corput sequence. - The end value is also returned if it's more than half the - distance from the closest value. + The start and end values will always be checked first. For example, on the interval [0.0, 13.7], the sequence is: [0.0, 13.7, 12.0, 6.0, 4.0, 8.0, 2.0, 10.0] @@ -1522,8 +1521,6 @@ def VanDerCorputSampleGenerator(start, end, step=2): @returns generator: A sequence of float values. """ - import itertools - # 'start' and 'end' must be positive because # itertools.islice() only accepts a positive integer if end <= start: @@ -1532,48 +1529,27 @@ def VanDerCorputSampleGenerator(start, end, step=2): if not (step > 0): raise ValueError("The 'step' value must be positive.") - # The duration, rounded to nearest step-size - mod_end = int(end - (end % step)) - steps_to_take = mod_end / float(step) - leftover_time = end - float(mod_end) - - # Keep a list to make sure we return all the sample values - times_sampled = [False for i in range(mod_end + 1)] - - vdc = VanDerCorputSequence(start, steps_to_take) - vdc_seq = itertools.islice(vdc, steps_to_take + 1) - count = 0 - for s in vdc_seq: - # Snap this sample value to the desired step-size - idx = int(step * numpy.round(s)) - if (idx % step) != 0: - idx = idx + 1 - - # If required, return the actual end-point value (a float) as - # the 2nd sample point to be returned. Then the next sample - # point is the end-point rounded to step-size. - if count == 1: - if leftover_time > (step / 2.0): - yield float(end) - - count = count + 1 - while True: - if times_sampled[idx] is False: - times_sampled[idx] = True - yield float(idx) - break - else: - # We have already sampled at this value of t, - # so lets try a different value of t. - decimals = (s % 1) - if decimals < 0.5: - idx = idx - step - if (idx < 0): # handle wrap past zero - idx = int(end - 1) - else: - idx = idx + step - if (idx > end): # handle wrap past end - idx = int(start + 1) + # Construct the points at which checks must occur to cover this range. + check_bins = numpy.arange(start, end, step) + is_checked = [False] * len(check_bins) + + # Always return the start and end points first. + is_checked[0] = True + yield start + yield end + + # Return a collision-checking sequence that eventually covers the range. + vdc = VanDerCorputSequence(lower=start, upper=end, include_endpoints=False) + for s in vdc: + idx = numpy.digitize((s,), check_bins) + if is_checked[idx]: + continue + + is_checked[idx] = True + yield float(check_bins[idx]) + + if numpy.all(is_checked): + return def GetCollisionCheckPts(robot, traj, include_start=True, start_time=0.,