diff --git a/src/prpy/base/endeffector.py b/src/prpy/base/endeffector.py index 2a7b76c..afa8419 100644 --- a/src/prpy/base/endeffector.py +++ b/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()) diff --git a/src/prpy/tsr/tsrlibrary.py b/src/prpy/tsr/tsrlibrary.py index 85caf43..8eca237 100644 --- a/src/prpy/tsr/tsrlibrary.py +++ b/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"',