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

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