Merge pull request #313 from athackst/feature/tsr_gripper_library

Feature/tsr gripper library

src/prpy/base/endeffector.py

@@ -73,5 +73,5 @@ def SetActive(self):
         This both: (1) sets the current manipulator as active and (2) sets the
         active DOF values to thosse associated with this end-effector.
         """
-        self.GetRobot().SetActiveManipulator(self.manipulator)
-        self.GetRobot().SetActiveDOFs(self.GetArmIndices())
+        self.manipulator.GetRobot().SetActiveManipulator(self.manipulator)
+        self.manipulator.GetRobot().SetActiveDOFs(self.manipulator.GetArmIndices())

src/prpy/tsr/tsrlibrary.py

@@ -56,6 +56,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
+    generic_robot_key = "_*"
 
     def __init__(self, robot, robot_name=None):
         """
@@ -88,12 +89,28 @@ def __call__(self, kinbody, action_name, *args, **kw_args):
         f = None
         try:
             f = self.all_factories[self.robot_name][kinbody_name][action_name]
+            logger.info('Using robot specific TSR for object')
         except KeyError:
             pass
 
+        if f is None:
+            try:
+                f = self.all_factories[self.generic_robot_key][kinbody_name][action_name]
+                logger.info('Using generic TSR for object')
+            except KeyError:
+                pass
+
         if f is None:
             try:
                 f = self.all_factories[self.robot_name][self.generic_kinbody_key][action_name]
+                logger.info('Using robot specific generic object')
+            except KeyError:
+                pass
+
+        if f is None:
+            try:
+                f = self.all_factories[self.generic_robot_key][self.generic_kinbody_key][action_name]
+                logger.info('Using generic object')
             except KeyError:
                 raise KeyError('There is no TSR factory registered for action "{:s}"'
                                ' with robot "{:s}" and object "{:s}".'.format(
@@ -181,6 +198,9 @@ def add_factory(cls, func, robot_name, object_name, action_name):
         if object_name is None:
             object_name = cls.generic_kinbody_key
 
+        if robot_name is None:
+            robot_name = cls.generic_robot_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"',