Reverted unnecessary lint fix.

Author: psigen

src/prpy/planning/vectorfield.py

@@ -437,10 +437,8 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
         from .exceptions import (
             CollisionPlanningError,
             SelfCollisionPlanningError,
-            TimeoutPlanningError
         )
         from openravepy import CollisionReport, RaveCreateTrajectory
-        from ..util import ComputeJointVelocityFromTwist, GetCollisionCheckPts, ComputeUnitTiming
         import time
         import scipy.integrate
 
@@ -456,9 +454,6 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
         env = robot.GetEnv()
         active_indices = robot.GetActiveDOFIndices()
 
-        # Get the robot's joint velocity limits
-        qdot_limit = robot.GetDOFVelocityLimits(active_indices)
-
         # Create a new trajectory matching the current
         # robot's joint configuration specification
         cspec = robot.GetActiveConfigurationSpecification('linear')
@@ -477,7 +472,6 @@ def fn_wrapper(t, q):
             """
             # Set the joint values, without checking the joint limits
             robot.SetActiveDOFValues(q, CheckLimitsAction.Nothing)
-
             return fn_vectorfield()
 
         def fn_status_callback(t, q):
@@ -490,13 +484,12 @@ def fn_status_callback(t, q):
             called after each integration time step, which means we are
             doing more checks than required.
             """
-            if time.time() - time_start >= timelimit:
-                raise TimeLimitError()
+            #if time.time() - time_start >= timelimit:
+            #    raise TimeLimitError()
 
             # Check joint position limits.
             # We do this before setting the joint angles.
             util.CheckJointLimits(robot, q)
-
             robot.SetActiveDOFValues(q)
 
             # Check collision.
@@ -528,25 +521,25 @@ def fn_callback(t, q):
 
                 # Run constraint checks at DOF resolution.
                 if path.GetNumWaypoints() == 1:
+                    t_start = 0.
                     checks = [(t, q)]
                 else:
-                    # TODO: This should start at t_check. Unfortunately, a bug
-                    # in GetCollisionCheckPts causes this to enter an infinite
-                    # loop.
-                    checks = GetCollisionCheckPts(robot, path,
-                        include_start=False) #start_time=nonlocals['t_check'])
+                    t_start = nonlocals['t_check']
+                    check_traj = util.GetTrajectoryTail(path, t_start)
+                    checks = util.GetCollisionCheckPts(robot, check_traj,
+                                                       include_start=False)
 
                 for t_check, q_check in checks:
-                    fn_status_callback(t_check, q_check)
+                    fn_status_callback(t_check + t_start, q_check)
 
                     # Record the time of this check so we continue checking at
                     # DOF resolution the next time the integrator takes a step.
                     nonlocals['t_check'] = t_check
 
-                return 0 # Keep going.
+                return 0  # Keep going.
             except PlanningError as e:
                 nonlocals['exception'] = e
-                return -1 # Stop.
+                return -1  # Stop.
 
         # Integrate the vector field to get a configuration space path.
         #
@@ -567,7 +560,7 @@ def fn_callback(t, q):
         integrator.integrate(t=integration_time_interval)
 
         t_cache = nonlocals['t_cache']
-        exception = nonlocals['exception'] 
+        exception = nonlocals['exception']
 
         if t_cache is None:
             raise exception or PlanningError('An unknown error has occurred.')

src/prpy/util.py

@@ -1770,6 +1770,7 @@ def GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=None):
         waypoint = traj.GetWaypoint(i)
         q1 = traj_cspec.ExtractJointValues(waypoint, robot, dof_indices)
         dq = numpy.abs(q1 - q0)
+        max_diff_float = numpy.max( numpy.abs(q1 - q0) / q_resolutions)
 
         # Get the number of steps (as a float) required for
         # each joint at DOF resolution