1import logging; logger = logging.getLogger("morse." + __name__)
2from morse.robots.grasping_robot import GraspingRobot
3from morse.core.services import service
4from morse.core import blenderapi
5
6class PR2(GraspingRobot):
7    """
8    The MORSE model of the Willow Garage's PR2 robot.
9
10    The PR2 uses the :doc:`armature_actuator <../actuators/armature>`
11    for control of the armatures.
12
13    Model Info
14    ----------
15
16    The model is imported from a Collada file that is generated from the
17    `PR2 URDF file  <http://www.ros.org/wiki/pr2_description>`_.
18    The .dae file can be found at:
19    ``$MORSE_ROOT/data/robots/pr2/pr2.dae``
20    The imported .blend file can be found at:
21    ``$MORSE_ROOT/data/robots/pr2/pr2_25_original.blend``
22
23    The URDF to Collada converter changed all the object names, so these
24    were remapped to the original URDF names. A renamed version of the
25    PR2 model can be found at:
26    ``$MORSE_ROOT/data/robots/pr2/pr2_25_rename.blend`` , this file
27    includes the script that is used to rename all the objects.
28
29    A model with MORSE integration for the armature can be found at
30    (**This is the model that you probably want to use in MORSE**):
31    ``$MORSE_ROOT/data/robots/pr2/pr2_25_morse.blend``.
32
33    TODO
34    ----
35
36    - Create sensors and actuators to control the PR2 armature. `A
37      SensorActuator class would be handy for this
38      <https://sympa.laas.fr/sympa/arc/morse-users/2011-07/msg00099.html>`_.
39    - Expand the armature to include the hands.
40    - Add an actuator to control the movement of the PR2 base.
41    - ROS integration.
42    - ...
43
44    """
45
46    _name = 'PR2 robot'
47
48    def __init__(self, obj, parent=None):
49        """
50        Constructor method.
51        Receives the reference to the Blender object.
52        """
53        logger.info('%s initialization' % obj.name)
54        # Call the constructor of the parent class
55        GraspingRobot.__init__(self, obj, parent)
56
57        """
58        We define here the name of the pr2 grasping hand:
59        """
60        self.hand_name = 'Hand.Grasp.PR2'
61
62        self.armatures = []
63        # Search armatures and torso in all objects parented to the pr2 empty
64        for obj in self.bge_object.childrenRecursive:
65            # Check if obj is an armature
66            if type(obj).__name__ == 'BL_ArmatureObject':
67                self.armatures.append(obj.name)
68            if obj.name == 'torso_lift_joint':
69                self.torso = obj
70
71        # constant that holds the original height of the torso from the ground
72        # These values come from the pr2 urdf file
73        self.TORSO_BASE_HEIGHT = (0.739675 + 0.051)
74        self.TORSO_LOWER = 0.0  # lower limit on the torso z-translantion
75        self.TORSO_UPPER = 0.31  # upper limit on the torso z-translation
76
77        logger.info('Component initialized')
78
79
80    @service
81    def get_armatures(self):
82        """
83        Returns a list of all the armatures on the PR2 robot.
84        """
85        return self.armatures
86
87    @service
88    def set_torso(self, height):
89        """
90        MORSE Service that sets the z-translation of the torso to original_z + height.
91        """
92        if self.TORSO_LOWER < height < self.TORSO_UPPER:
93            self.torso.localPosition = [-0.05, 0, self.TORSO_BASE_HEIGHT + height]
94            return "New torso z position: " + str(self.torso.localPosition[2])
95        else:
96            return "Not a valid height, value has to be between 0.0 and 0.31!"
97
98    @service
99    def get_torso(self):
100        """
101        Returns the z-translation of the torso.
102        """
103        return self.torso.localPosition[2] - self.TORSO_BASE_HEIGHT
104
105    @service
106    def get_torso_minmax(self):
107        """
108        Returns the minimum an maximum z-translation that the torso can
109        make from the base.  Returns a list [min,max]
110        """
111        return [self.TORSO_LOWER, self.TORSO_UPPER]
112
113    def default_action(self):
114        """
115        Main function of this component.
116        """
117        pass
118