Source code for morse.builder.robots.pr2

import logging; logger = logging.getLogger("pr2." + __name__)

from morse.builder import *
from morse.builder.sensors import *
from morse.builder.actuators import *

[docs]class BarePR2(GroundRobot): """ A PR2 model, without any sensor or actuator. """ def __init__(self, name = None): GroundRobot.__init__(self, 'pr2', name) self.properties(classpath = "morse.robots.pr2.PR2", COLOR = "0.0, 0.0, 1.0")
[docs] def set_color(self, color = (0.0, 0.0, 0.8)): """ Allows to change the PR2 head color. """ self.get_child("head_tilt_link").material_slots['HeadTilt'].material.node_tree.nodes['Material'].material.diffuse_color = color
[docs]class BasePR2(BarePR2): """ A PR2 only equipped with its armatures for the arms, the torso and the head. It also provides the compound sensor ``pr2.joint_state`` that exports the whole joint state of the robot. """ def __init__(self, name = None): BarePR2.__init__(self, name) # Armatures and armature pose sensors # The armatures are already present in the PR2 blender model. # torso self.torso = Armature(armature_name = "torso_armature") self.torso_pose = ArmaturePose() self.torso_pose.name = "pose" self.torso.append(self.torso_pose) self.append(self.torso) # head self.head = Armature(armature_name = "head_armature") self.head_pose = ArmaturePose() self.head_pose.name = "pose" self.head.append(self.head_pose) self.torso.append(self.head) # arms self.l_arm = Armature(armature_name = "l_arm_armature") self.l_arm_pose = ArmaturePose() self.l_arm_pose.name = "pose" self.l_arm.append(self.l_arm_pose) self.torso.append(self.l_arm) self.r_arm = Armature(armature_name = "r_arm_armature") self.r_arm_pose = ArmaturePose() self.r_arm_pose.name = "pose" self.r_arm.append(self.r_arm_pose) self.torso.append(self.r_arm) # joint state self.joint_state = CompoundSensor([self.torso_pose, self.head_pose, self.l_arm_pose, self.r_arm_pose]) self.append(self.joint_state)
[docs] def add_interface(self, interface): if interface == "socket": self.joint_state.add_stream("socket", "morse.middleware.sockets.jointstate.PR2JointStatePublisher") self.torso.add_service('socket') self.head.add_service('socket') self.l_arm.add_service('socket') self.r_arm.add_service('socket') elif interface == "ros": self.joint_state.add_stream("ros", "morse.middleware.ros.jointstate.JointStatePR2Publisher", topic = "/joint_states") self.joint_state.add_service("ros") self.torso_pose.add_stream("ros", "morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher", topic="/torso_lift_controller/state") self.torso.add_overlay("ros", "morse.middleware.ros.overlays.armatures.ArmatureController", namespace = "/torso_lift_controller") self.head_pose.add_stream("ros", "morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher", topic="/head_controller/state") self.head.add_overlay("ros", "morse.middleware.ros.overlays.armatures.ArmatureController", namespace = "/head_controller") self.l_arm_pose.add_stream("ros", "morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher", topic="/l_arm_controller/state") self.l_arm.add_overlay("ros", "morse.middleware.ros.overlays.armatures.ArmatureController", namespace = "/l_arm_controller") self.r_arm_pose.add_stream("ros", "morse.middleware.ros.jointtrajectorycontrollers.JointTrajectoryControllerStatePublisher", topic="/r_arm_controller/state") self.r_arm.add_overlay("ros", "morse.middleware.ros.overlays.armatures.ArmatureController", namespace = "/r_arm_controller")
[docs]class LocalizedPR2(BasePR2): def __init__(self, name = None, with_keyboard = True, show_laser = False): BasePR2.__init__(self, name) ################################### # Actuators ################################### # Motion controller self.motion = MotionXYW() self.motion.properties(ControlType = 'Position') self.append(self.motion) # (optionally) keyboard controller if with_keyboard: keyboard = Keyboard() self.append(keyboard) ################################### # Sensors ################################### self.pose = Pose() self.append(self.pose)
[docs] def add_interface(self, interface): BasePR2.add_interface(self, interface) if interface == "ros": self.motion.add_stream("ros", topic="/cmd_vel") self.pose.add_stream("ros", method="morse.middleware.ros.pose.TFPublisher")