Switched to L2 DOF resolution in VectorField and GreedyIk

src/prpy/planning/vectorfield.py

@@ -439,7 +439,7 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
             SelfCollisionPlanningError,
         )
         from openravepy import CollisionReport, RaveCreateTrajectory
-        from ..util import GetCollisionCheckPts
+        from ..util import GetLinearCollisionCheckPts
         import time
         import scipy.integrate
 
@@ -526,11 +526,13 @@ def fn_callback(t, q):
                 if path.GetNumWaypoints() == 1:
                     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)
+                    # TODO: This will recheck the entire trajectory
+                    #  Ideally should just check the new portion of the trajectory
+                    from prpy.util import VanDerCorputSampleGenerator
+                    vdc = VanDerCorputSampleGenerator
+                    checks = GetLinearCollisionCheckPts(robot, path,
+                                                        norm_order=2,
+                                                        sampling_func=vdc)
                     # start_time=nonlocals['t_check'])
 
                 for t_check, q_check in checks:

src/prpy/planning/workspace.py

@@ -141,7 +141,7 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
 
     @ClonedPlanningMethod
     def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
-                          min_waypoint_index=None, **kw_args):
+                          min_waypoint_index=None, norm_order=2, **kw_args):
         """
         Plan a configuration space path given a workspace path.
         All timing information is ignored.
@@ -150,6 +150,11 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                     represented as OpenRAVE AffineTrajectory
         @param min_waypoint_index minimum waypoint index to reach
         @param timelimit timeout in seconds
+        @param norm_order: 1  ==>  The L1 norm
+                           2  ==>  The L2 norm
+                           inf  ==>  The L_infinity norm
+               Used to determine the resolution of collision checked waypoints
+               in the trajectory
         @return qtraj configuration space path
         """
         from .exceptions import (
@@ -179,6 +184,8 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
             t = 0.
             dt = traj.GetDuration()
 
+            q_resolutions = robot.GetActiveDOFResolutions()
+
             # Smallest CSpace step at which to give up
             min_step = min(robot.GetActiveDOFResolutions()) / 100.
             ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
@@ -206,11 +213,14 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                     infeasible_step = True
                     if qnew is not None:
                         # Found an IK
-                        step = abs(qnew - qcurr)
-                        if (max(step) < min_step) and qtraj:
+                        steps = abs(qnew - qcurr) / q_resolutions;
+                        norm = numpy.linalg.norm(steps, ord=norm_order)
+
+                        if (norm < min_step) and qtraj:
                             raise PlanningError('Not making progress.')
-                        infeasible_step = \
-                            any(step > robot.GetActiveDOFResolutions())
+
+                        infeasible_step = norm > 1.0
+
                     if infeasible_step:
                         # Backtrack and try half the step
                         dt = dt / 2.0