From 3a88500f812795b33890941cfc2355f6ec9673fb Mon Sep 17 00:00:00 2001
From: Jennifer King <jeking@andrew.cmu.edu>
Date: Wed, 27 Jul 2016 14:15:37 -0700
Subject: [PATCH 1/2] Updating GetLinearCollisionCheckPts to check every dof
 resolution

---
 src/prpy/util.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/prpy/util.py b/src/prpy/util.py
index 43cc46b..f7be8d8 100644
--- a/src/prpy/util.py
+++ b/src/prpy/util.py
@@ -1738,9 +1738,9 @@ def GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=None):
     seq = None
     if sampling_func is None:
         # (default) Linear sequence, from start to end
-        seq = SampleTimeGenerator(0, required_checks, step=2)
+        seq = SampleTimeGenerator(0, required_checks, step=1)
     else:
-        seq = sampling_func(0, required_checks, step=2)
+        seq = sampling_func(0, required_checks, step=1)
 
     # Sample a check and return the associated time in the original
     # trajectory and joint position

From 2fa8ba1b3a1e0bf76b6cea4e7170e9fb5ea90067 Mon Sep 17 00:00:00 2001
From: Jennifer King <jeking@andrew.cmu.edu>
Date: Wed, 27 Jul 2016 15:21:20 -0700
Subject: [PATCH 2/2] Fixing unit tests. Updating SampleTimeGenerator to allow
 the option of including end point

---
 src/prpy/util.py   | 14 +++++++++-----
 tests/test_util.py | 11 +++++------
 2 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/src/prpy/util.py b/src/prpy/util.py
index f7be8d8..93ac3a5 100644
--- a/src/prpy/util.py
+++ b/src/prpy/util.py
@@ -1473,7 +1473,7 @@ def VanDerCorputSequence(lower=0.0, upper=1.0, include_endpoints=True):
     return (scale * val + lower for val in chain(endpoints, raw_seq))
 
 
-def SampleTimeGenerator(start, end, step=1):
+def SampleTimeGenerator(start, end, step=1, include_endpoints=False, **kwargs):
     """
     Generate a linear sequence of values from start to end, with
     specified step size. Works with int or float values.
@@ -1487,7 +1487,8 @@ def SampleTimeGenerator(start, end, step=1):
     @param float start: The start value of the sequence.
     @param float end:   The last value of the sequence.
     @param float step:  The step-size between values.
-
+    @param bool include_endpoints: If true, include the start and end value
+       
     @returns generator: A sequence of float values.
     """
     if end <= start:
@@ -1503,9 +1504,12 @@ def SampleTimeGenerator(start, end, step=1):
         t = t + step
     if (end - float(prev_t)) > (step / 2.0):
         yield float(end)
+        prev_t = end
+    if include_endpoints and (end - float(prev_t)) > 1e-6:
+        yield float(end)
 
 
-def VanDerCorputSampleGenerator(start, end, step=2):
+def VanDerCorputSampleGenerator(start, end, step=2, **kwargs):
     """
     This wraps VanDerCorputSequence() in a way that's useful for
     collision-checking.
@@ -1738,9 +1742,9 @@ def GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=None):
     seq = None
     if sampling_func is None:
         # (default) Linear sequence, from start to end
-        seq = SampleTimeGenerator(0, required_checks, step=1)
+        seq = SampleTimeGenerator(0, required_checks, step=1, include_enpoints=True)
     else:
-        seq = sampling_func(0, required_checks, step=1)
+        seq = sampling_func(0, required_checks, step=1, include_endpoints=True)
 
     # Sample a check and return the associated time in the original
     # trajectory and joint position
diff --git a/tests/test_util.py b/tests/test_util.py
index fb9547c..a29be18 100644
--- a/tests/test_util.py
+++ b/tests/test_util.py
@@ -259,7 +259,7 @@ def test_GetLinearCollisionCheckPts_TwoPointTraj_LessThanDOFRes(self):
         # because the L2 norm = 1.32
         q0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         q1 = 0.5 * self.dof_resolutions
-        desired_num_checks = 2
+        desired_num_checks = 3
         traj = self.CreateTrajectory(q0, q1)
         # Linear sampling
         linear = prpy.util.SampleTimeGenerator
@@ -292,7 +292,7 @@ def test_GetLinearCollisionCheckPts_TwoPointTraj_EqualToDOFRes(self):
         # the actual end position does not need to be checked.
         q0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         q1 = 1.0 * self.dof_resolutions
-        desired_num_checks = 2
+        desired_num_checks = 4
         traj = self.CreateTrajectory(q0, q1)
         # Linear sampling
         linear = prpy.util.SampleTimeGenerator
@@ -301,9 +301,8 @@ def test_GetLinearCollisionCheckPts_TwoPointTraj_EqualToDOFRes(self):
                                                       norm_order=2, \
                                                       sampling_func=linear)
         num_checks = 0
-        for t, q in checks:
+        for _ in checks:
             num_checks = num_checks + 1
-            prev_q = q # the robot configuration
         if num_checks != desired_num_checks:
             error = str(num_checks) + ' is the wrong number of check pts.'
             self.fail(error)
@@ -318,7 +317,7 @@ def test_GetLinearCollisionCheckPts_TwoPointTraj_GreaterThanDOFRes(self):
         # because the L2 norm = 3.17.
         q0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         q1 = 1.2 * self.dof_resolutions
-        desired_num_checks = 3
+        desired_num_checks = 5
         traj = self.CreateTrajectory(q0, q1)
         # Linear sampling
         linear = prpy.util.SampleTimeGenerator
@@ -327,7 +326,7 @@ def test_GetLinearCollisionCheckPts_TwoPointTraj_GreaterThanDOFRes(self):
                                                       norm_order=2, \
                                                       sampling_func=linear)
         num_checks = 0
-        for t, q in checks:
+        for _, q in checks:
             num_checks = num_checks + 1
             prev_q = q # the robot configuration
         if num_checks != desired_num_checks: