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