Merge pull request #174 from personalrobotics/bugfix/GreedyIK

Fixed GreedyIK planner in MoveUntilTouch.

Author: mkoval

src/prpy/base/wam.py

@@ -28,13 +28,17 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
+import logging
 import numpy
 import openravepy
+import warnings
 from manipulator import Manipulator
 from prpy.clone import Clone
 from .. import util
 from .. import exceptions
 
+logger = logging.getLogger('wam')
+
 class WAM(Manipulator):
     def __init__(self, sim, owd_namespace,
                  iktype=openravepy.IkParameterization.Type.Transform6D):
@@ -242,13 +246,14 @@ def ClearTrajectoryStatus(manipulator):
         if not manipulator.simulated:
             manipulator.controller.SendCommand('ClearStatus')
 
-    def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
+    def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
                        max_force=5.0, max_torque=None, ignore_collisions=None, **kw_args):
         """Execute a straight move-until-touch action.
         This action stops when a sufficient force is is felt or the manipulator
         moves the maximum distance. The motion is considered successful if the
         end-effector moves at least distance. In simulation, a move-until-touch
         action proceeds until the end-effector collids with the environment.
+
         @param direction unit vector for the direction of motion in the world frame
         @param distance minimum distance in meters
         @param max_distance maximum distance in meters
@@ -258,6 +263,14 @@ def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
         @param **kw_args planner parameters
         @return felt_force flag indicating whether we felt a force.
         """
+
+        if max_distance is None:
+            max_distance = 1.
+            warnings.warn(
+                'MoveUntilTouch now requires the "max_distance" argument.'
+                ' This will be an error in the future.',
+                DeprecationWarning)
+
         # TODO: Is ignore_collisions a list of names or KinBody pointers?
         if max_torque is None:
             max_torque = numpy.array([100.0, 100.0, 100.0 ])

src/prpy/logger.py

@@ -28,7 +28,7 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import collections, logging, sys
+import collections, logging, sys, warnings
 
 class ColoredFormatter(logging.Formatter):
     def __init__(self, default):
@@ -80,6 +80,9 @@ def initialize_logging():
         spammy_logger = logging.getLogger(spammy_logger_name)
         spammy_logger.setLevel(logging.WARNING)
 
+    # Enable deprecation warnings, which are off by default in Python 2.7.
+    warnings.simplefilter('default')
+
     return base_logger
 
 def remove_ros_logger():

src/prpy/planning/workspace.py

@@ -105,6 +105,8 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
             raise ValueError('Direction must be non-zero')
         elif max_distance is not None and max_distance < distance:
             raise ValueError('Max distance is less than minimum distance.')
+        elif max_distance is not None and not numpy.isfinite(max_distance):
+            raise ValueError('Max distance must be finite.')
 
         # Normalize the direction vector.
         direction = numpy.array(direction, dtype='float')
@@ -224,7 +226,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                 # Otherwise we'll gracefully terminate.
                 else:
                     logger.warning('Terminated early at time %f < %f: %s',
-                                   t, traj.GetDuration(), e.message)
+                                   t, traj.GetDuration(), str(e))
 
         # Return as much of the trajectory as we have solved.
         SetTrajectoryTags(qtraj, {Tags.CONSTRAINED: True}, append=True)