Merge pull request #156 from personalrobotics/feature/generic_object_tsr

Added generic-object TSRs.

Author: mkoval

src/prpy/base/mobilebase.py

@@ -85,41 +85,35 @@ def Forward(self, meters, execute=True, direction=None, **kw_args):
         if abs(numpy.linalg.norm(direction) - 1.0) > 1e-3:
             raise ValueError('Direction must be a unit vector.')
 
-        if self.simulated:
-            with self.robot.GetEnv():
-                start_pose = self.robot.GetTransform()
-                offset_pose = numpy.eye(4)
-                offset_pose[0:3, 3] = meters * direction
-                goal_pose = numpy.dot(start_pose, offset_pose)
-
-            path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
-            if execute:
-                return self.robot.ExecutePath(path, **kw_args)
-            else:
-                return path
+        with self.robot.GetEnv():
+            start_pose = self.robot.GetTransform()
+            offset_pose = numpy.eye(4)
+            offset_pose[0:3, 3] = meters * direction
+            goal_pose = numpy.dot(start_pose, offset_pose)
+            
+        path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
+        if execute:
+            return self.robot.ExecutePath(path, **kw_args)
         else:
-            raise NotImplementedError('DriveForward is not implemented')
+            return path
 
     def Rotate(self, angle_rad, execute=True, **kw_args):
         """
         Rotates the robot the desired distance
         @param angle_rad the number of radians to rotate
         @param timeout duration to wait for execution
         """
-        if self.simulated:
-            with self.robot.GetEnv():
-                start_pose = self.robot.GetTransform()
-
+        with self.robot.GetEnv():
+            start_pose = self.robot.GetTransform()
+            
             relative_pose = openravepy.matrixFromAxisAngle([ 0., 0., angle_rad ])
             goal_pose = numpy.dot(start_pose, relative_pose)
 
-            path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
-            if execute:
-                return self.robot.ExecutePath(path, **kw_args)
-            else:
-                return path
+        path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
+        if execute:
+            return self.robot.ExecutePath(path, **kw_args)
         else:
-            raise NotImplementedError('Rotate is not implemented')
+            return path
 
     def DriveStraightUntilForce(self, direction, velocity=0.1, force_threshold=3.0,
                                 max_distance=None, timeout=None, left_arm=True, right_arm=True):

src/prpy/base/robot.py

@@ -110,8 +110,8 @@ def wrapper_method(*args, **kw_args):
 
             delegate_method = self.actions.get_action(name)
             @functools.wraps(delegate_method)
-            def wrapper_method(obj, *args, **kw_args):
-                return delegate_method(self, obj, *args, **kw_args)
+            def wrapper_method(*args, **kw_args):
+                return delegate_method(self, *args, **kw_args)
             return wrapper_method
 
         raise AttributeError('{0:s} is missing method "{1:s}".'.format(repr(self), name))

src/prpy/tsr/tsrlibrary.py

@@ -55,6 +55,7 @@ def wrapped_func(robot, kinbody, *args, **kw_args):
 
 class TSRLibrary(object):
     all_factories = collections.defaultdict(lambda: collections.defaultdict(dict))
+    generic_kinbody_key = "_*"  # Something that is unlikely to be an actual kinbody name
 
     def __init__(self, robot, robot_name=None):
         """
@@ -80,18 +81,28 @@ def __call__(self, kinbody, action_name, *args, **kw_args):
         @return list of TSRChains
         """
         kinbody_name = kw_args.get('kinbody_name', None)
-        if kinbody_name is None:
+        if kinbody_name is None and kinbody is not None:
             kinbody_name = self.get_object_type(kinbody)
             logger.debug('Inferred KinBody name "%s" for TSR.', kinbody_name)
 
+        f = None
         try:
             f = self.all_factories[self.robot_name][kinbody_name][action_name]
         except KeyError:
-            raise KeyError('There is no TSR factory registered for action "{:s}"'
-                           ' with robot "{:s}" and object "{:s}".'.format(
-                           action_name, self.robot_name, kinbody_name))
+            pass
 
-        return f(self.robot, kinbody, *args, **kw_args)
+        if f is None:
+            try:
+                f = self.all_factories[self.robot_name][self.generic_kinbody_key][action_name]
+            except KeyError:
+                raise KeyError('There is no TSR factory registered for action "{:s}"'
+                               ' with robot "{:s}" and object "{:s}".'.format(
+                        action_name, self.robot_name, kinbody_name))
+       
+        if kinbody is None:
+            return f(self.robot, *args, **kw_args)
+        else:
+            return f(self.robot, kinbody, *args, **kw_args)
 
     def load_yaml(self, yaml_file):
         """
@@ -167,6 +178,9 @@ def add_factory(cls, func, robot_name, object_name, action_name):
         logger.debug('Adding TSRLibrary factory for robot "%s", object "%s", action "%s".',
             robot_name, object_name, action_name)
 
+        if object_name is None:
+            object_name = cls.generic_kinbody_key
+
         if action_name in cls.all_factories[robot_name][object_name]:
             logger.warning('Overwriting duplicate TSR factory for action "%s"'
                            ' with robot "%s" and object "%s"',