Merge branch 'master' of https://github.com/personalrobotics/prpy into bugfix/cloning_race

Author: psigen

src/prpy/base/robot.py

@@ -39,7 +39,8 @@
 from ..planning.mac_smoother import MacSmoother
 from ..util import SetTrajectoryTags
 
-logger = logging.getLogger('robot')
+logger = logging.getLogger(__name__)
+
 
 class Robot(openravepy.Robot):
     def __init__(self, robot_name=None):
@@ -570,7 +571,7 @@ def defer_trajectory(traj_future, kw_args):
                 postprocess_trajectory(traj)
 
                 # Optionally execute the trajectory.
-                if kw_args.get('execute', True):
+                if kw_args.get('execute', False):
                     # We know defer = True if we're in this function, so we
                     # don't have to set it explicitly.
                     traj = yield trollius.From(
@@ -584,7 +585,7 @@ def defer_trajectory(traj_future, kw_args):
             postprocess_trajectory(result)
 
             # Optionally execute the trajectory.
-            if kw_args.get('execute', True):
+            if kw_args.get('execute', False):
                 result = self.ExecutePath(result, **kw_args)
 
             return result

src/prpy/bind.py

@@ -31,13 +31,14 @@
 import logging
 from clone import CloneException
 
-logger = logging.getLogger('bind')
+logger = logging.getLogger(__name__)
+
 
 class NotCloneableException(CloneException):
     pass
 
+
 class InstanceDeduplicator(object):
-    logger = logging.getLogger('bind')
     USERDATA_PREFIX = 0xDEADBEEF
     USERDATA_CHILDREN = '__children__'
     USERDATA_DESTRUCTOR = '__destructor__'
@@ -209,11 +210,11 @@ def cleanup_callback(owner, flag):
             # skipped
             while children:
                 child = children.pop()
-                InstanceDeduplicator.logger.debug(child)
+                logger.debug(child)
                 clear_referrers(child)
                 # NOTE: this is also an acceptable body for the loop :)
                 # clear_referrers(children[0])
-            InstanceDeduplicator.logger.debug(owner)
+            logger.debug(owner)
             if canonical_instance != None:
                 clear_referrers(canonical_instance)
 
@@ -233,7 +234,7 @@ def get_storage_methods(cls, target):
                 handle = env.RegisterBodyCallback(InstanceDeduplicator.cleanup_callback)
                 user_data[cls.USERDATA_DESTRUCTOR] = handle
             else:
-                cls.logger.warning(
+                logger.warning(
                     'Your version of OpenRAVE does not supply Python bindings'
                     ' for Environment.RegisterBodyCallback. PrPy will leak'
                     ' unless you call prpy.bind.InstanceDeduplicator.remove_storage'

src/prpy/exceptions.py

@@ -17,3 +17,30 @@ class SynchronizationException(PrPyException):
     """
     Controller synchronization failed.
     """
+
+class SerializationException(PrPyException):
+    """
+    Serialization failed.
+    """
+
+class UnsupportedTypeSerializationException(SerializationException):
+    """
+    Serialization failed due to an unknown type.
+    """
+    def __init__(self, value):
+        self.value = value
+        self.type = type(self.value)
+
+        super(UnsupportedTypeSerializationException, self).__init__(
+            'Serializing type "{:s}.{:s}" is not supported.'.format(
+                self.type.__module__, self.type.__name__))
+
+class UnsupportedTypeDeserializationException(SerializationException):
+    """
+    Deserialization failed due to an unknown type.
+    """
+    def __init__(self, type_name):
+        self.type_name = type_name
+
+        super(UnsupportedTypeDeserializationException, self).__init__(
+            'Deserializing type "{:s}" is not supported.'.format(type_name))

src/prpy/planning/base.py

@@ -37,7 +37,7 @@
 from ..util import CopyTrajectory, GetTrajectoryTags, SetTrajectoryTags
 from .exceptions import PlanningError, UnsupportedPlanningError
 
-logger = logging.getLogger('planning')
+logger = logging.getLogger(__name__)
 
 
 class Tags(object):
@@ -49,6 +49,7 @@ class Tags(object):
     POSTPROCESS_TIME = 'postprocess_time'
     EXECUTION_TIME = 'execution_time'
 
+
 class MetaPlanningError(PlanningError):
     def __init__(self, message, errors):
         PlanningError.__init__(self, message)

src/prpy/planning/chomp.py

@@ -38,7 +38,7 @@
                   PlanningMethod, Tags)
 import prpy.tsr
 
-logger = logging.getLogger('prpy.planning.chomp')
+logger = logging.getLogger(__name__)
 
 DistanceFieldKey = collections.namedtuple('DistanceFieldKey',
     [ 'kinematics_hash', 'enabled_mask', 'dof_values', 'dof_indices' ])

src/prpy/planning/ik.py

@@ -35,7 +35,7 @@
                   PlanningError,
                   PlanningMethod)
 
-logger = logging.getLogger('prpy.planning.ik')
+logger = logging.getLogger(__name__)
 
 
 class IKPlanner(BasePlanner):

src/prpy/planning/mk.py

@@ -33,7 +33,7 @@
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
                   PlanningMethod, Tags)
 
-logger = logging.getLogger('planning')
+logger = logging.getLogger(__name__)
 
 def DoNothing(robot):
     return numpy.zeros(robot.GetActiveDOF())

src/prpy/planning/ompl.py

@@ -32,9 +32,9 @@
 from ..util import CopyTrajectory, SetTrajectoryTags
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
                   PlanningMethod, Tags)
-from openravepy import PlannerStatus 
+from openravepy import PlannerStatus
 
-logger = logging.getLogger('pypy.planning.ompl')
+logger = logging.getLogger(__name__)
 
 
 class OMPLPlanner(BasePlanner):

src/prpy/planning/retimer.py

@@ -31,9 +31,10 @@
 import logging, numpy, openravepy, os, tempfile
 from ..util import CopyTrajectory, SimplifyTrajectory, HasAffineDOFs
 from base import BasePlanner, PlanningError, PlanningMethod, UnsupportedPlanningError
-from openravepy import PlannerStatus 
+from openravepy import PlannerStatus
+
+logger = logging.getLogger(__name__)
 
-logger = logging.getLogger('retimer')
 
 class OpenRAVERetimer(BasePlanner):
     def __init__(self, algorithm, default_options=None):
@@ -141,7 +142,7 @@ def __init__(self, **kwargs):
 
 class HauserParabolicSmoother(OpenRAVERetimer):
     def __init__(self, do_blend=True, do_shortcut=True, blend_radius=0.5,
-                 blend_iterations=4, **kwargs):
+                 blend_iterations=0, timelimit=3., **kwargs):
         super(HauserParabolicSmoother, self).__init__(
                 'HauserParabolicSmoother', **kwargs)
 
@@ -150,6 +151,7 @@ def __init__(self, do_blend=True, do_shortcut=True, blend_radius=0.5,
             'do_shortcut': int(do_shortcut),
             'blend_radius': float(blend_radius),
             'blend_iterations': int(blend_iterations),
+            'time_limit': float(timelimit),
         })
 
 

src/prpy/planning/snap.py

@@ -6,7 +6,7 @@
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
-# 
+#
 # - Redistributions of source code must retain the above copyright notice, this
 #   list of conditions and the following disclaimer.
 # - Redistributions in binary form must reproduce the above copyright notice,
@@ -15,7 +15,7 @@
 # - Neither the name of Carnegie Mellon University nor the names of its
 #   contributors may be used to endorse or promote products derived from this
 #   software without specific prior written permission.
-# 
+#
 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -32,6 +32,7 @@
 from ..util import SetTrajectoryTags
 from base import BasePlanner, PlanningError, PlanningMethod, Tags
 
+
 class SnapPlanner(BasePlanner):
     """Planner that checks the straight-line trajectory to the goal.
 
@@ -40,15 +41,15 @@ class SnapPlanner(BasePlanner):
     due to an environment or self collision, the planner immediately returns
     failure by raising a PlanningError. Collision checking is performed using
     the standard CheckPathAllConstraints method.
-    
+
     SnapPlanner is intended to be used only as a "short circuit" to speed up
     planning between nearby configurations. This planner is most commonly used
     as the first item in a Sequence meta-planner to avoid calling a motion
     planner when the trivial solution is valid.
     """
     def __init__(self):
         super(SnapPlanner, self).__init__()
- 
+
     def __str__(self):
         return 'SnapPlanner'
 
@@ -86,7 +87,6 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         # close to the configuration of the arm, so we don't need to do any
         # custom IK ranking.
         manipulator = robot.GetActiveManipulator()
-        current_config = robot.GetDOFValues(manipulator.GetArmIndices())
         ik_param = openravepy.IkParameterization(goal_pose, ikp.Transform6D)
         ik_solution = manipulator.FindIKSolution(
             ik_param, ikfo.CheckEnvCollisions,
@@ -101,7 +101,7 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
     def _Snap(self, robot, goal, **kw_args):
         Closed = openravepy.Interval.Closed
 
-        curr = robot.GetActiveDOFValues()
+        start = robot.GetActiveDOFValues()
         active_indices = robot.GetActiveDOFIndices()
 
         # Use the CheckPathAllConstraints helper function to collision check
@@ -110,28 +110,32 @@ def _Snap(self, robot, goal, **kw_args):
         params = openravepy.Planner.PlannerParameters()
         params.SetRobotActiveJoints(robot)
         params.SetGoalConfig(goal)
-        check = params.CheckPathAllConstraints(curr, goal, [], [], 0., Closed)
+        check = params.CheckPathAllConstraints(start, goal, [], [], 0., Closed)
 
         # The function returns a bitmask of ConstraintFilterOptions flags,
         # indicating which constraints are violated. We'll abort if any
         # constraints are violated.
         if check != 0:
             raise PlanningError('Straight line trajectory is not valid.')
 
-        # Create a two-point trajectory that starts at our current
-        # configuration and takes us to the goal.
-        traj = openravepy.RaveCreateTrajectory(self.env, '')
+        # Create a trajectory that starts at our current configuration.
         cspec = robot.GetActiveConfigurationSpecification()
-        active_indices = robot.GetActiveDOFIndices()
+        traj = openravepy.RaveCreateTrajectory(self.env, '')
+        traj.Init(cspec)
 
-        waypoints = numpy.zeros((2, cspec.GetDOF()))
-        cspec.InsertJointValues(waypoints[0, :], curr, robot,
-                                active_indices, False)
-        cspec.InsertJointValues(waypoints[1, :], goal, robot,
+        start_waypoint = numpy.zeros(cspec.GetDOF())
+        cspec.InsertJointValues(start_waypoint, start, robot,
                                 active_indices, False)
+        traj.Insert(0, start_waypoint.ravel())
 
-        traj.Init(cspec)
-        traj.Insert(0, waypoints.ravel())
+        # Make the trajectory end at the goal configuration, as long as it
+        # was not identical to the start configuration.
+        if not numpy.allclose(start, goal):
+            goal_waypoint = numpy.zeros(cspec.GetDOF())
+            cspec.InsertJointValues(goal_waypoint, goal, robot,
+                                    active_indices, False)
+            traj.Insert(1, goal_waypoint.ravel())
 
+        # Tag the return trajectory as smooth (in joint space) and return it.
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj