Merge pull request #336 from personalrobotics/bugfix/collision_checki…

…ng_resolution

Bugfix/collision checking resolution

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=2)
+        seq = SampleTimeGenerator(0, required_checks, step=1, include_enpoints=True)
     else:
-        seq = sampling_func(0, required_checks, step=2)
+        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

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: