def __init__(self, simulation=True): """Initializes various aspects of the Fetch.""" rospy.init_node("fetch") rospy.loginfo("initializing the Fetch...") self.arm = Arm() self.arm_joints = ArmJoints() self.base = Base() self.camera = RGBD() self.head = Head() self.gripper = Gripper(self.camera) self.torso = Torso() self.joint_reader = JointStateReader() # Tucked arm starting joint angle configuration self.names = ArmJoints().names() self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0] self.tucked_list = [(x,y) for (x,y) in zip(self.names, self.tucked)] # Initial (x,y,yaw) position of the robot wrt map origin. We keep this # fixed so that we can reset to this position as needed. The HSR's # `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is # the rotation about that axis (intuitively, the z axis). For the base, # `base.odom` supplies both `position` and `orientation` attributes. start = copy.deepcopy(self.base.odom.position) yaw = Base._yaw_from_quaternion(self.base.odom.orientation) self.start_pose = np.array([start.x, start.y, yaw]) rospy.loginfo("...finished initialization!")
def __init__(self, simulation=True): """Initializes various aspects of the Fetch. TODOs: get things working, also use `simulation` flag to change ROS topic names if necessary (especially for the cameras!). UPDATE: actually I don't think this is necessary now, they have the same topic names. """ rospy.init_node("fetch") self.arm = Arm() self.arm_joints = ArmJoints() self.base = Base() self.camera = RGBD() self.head = Head() self.gripper = Gripper(self.camera) self.torso = Torso() self.joint_reader = JointStateReader() # Tucked arm starting joint angle configuration self.names = ArmJoints().names() self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0] self.tucked_list = [(x, y) for (x, y) in zip(self.names, self.tucked)] # Initial (x,y,yaw) position of the robot wrt map origin. We keep this # fixed so that we can reset to this position as needed. The HSR's # `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is # the rotation about that axis (intuitively, the z axis). For the base, # `base.odom` supplies both `position` and `orientation` attributes. start = copy.deepcopy(self.base.odom.position) yaw = Base._yaw_from_quaternion(self.base.odom.orientation) self.start_pose = np.array([start.x, start.y, yaw]) self.TURN_SPEED = 0.3 self.num_restarts = 0
def __init__(self, torso_center, lower_body_flag): self._body_center = torso_center self._lower_body_flag = lower_body_flag self.head = Head( Point(torso_center.x, torso_center.y + 15, torso_center.z)) self.torso = Torso( Point(torso_center.x, torso_center.y, torso_center.z)) self.left_arm = Arm( Point(torso_center.x + 6.6, torso_center.y + 8, torso_center.z)) self.right_arm = Arm( Point(torso_center.x - 6.6, torso_center.y + 8, torso_center.z)) self.right_arm.set_slocal_rotate_angle(180, 0, 0) if self._lower_body_flag: self.left_leg = Leg( Point(torso_center.x + 4, torso_center.y - 10.6, torso_center.z)) self.right_leg = Leg( Point(torso_center.x - 4, torso_center.y - 10.6, torso_center.z)) self.right_leg.set_hlocal_rotate_angle(180, 0, 0)
for k, v in zip(names, arm_vals): print '{}\t{:.4f}'.format(k, v) print("") # Move and then read the joints again to be clear pose = [0, 0, 0, 0, 0, 0, 0] pose[1] = DEGS_TO_RADS * -70 arm.move_to_joints(ArmJoints.from_list(pose)) arm_vals = reader.get_joints(names) for k, v in zip(names, arm_vals): print '{}\t{:.4f}'.format(k, v) print("") if __name__ == "__main__": rospy.init_node('arm_demo') wait_for_time() # Set things up torso = Torso() torso.set_height(Torso.MAX_HEIGHT) arm = Arm() reader = JointStateReader() rospy.sleep(0.5) rospy.loginfo("created torso, arm, and reader") #test_shoulders(arm, torso) #test_poses(arm, torso) test_reader(arm, reader)
def __init__(self, context): super(Milestone1GUI, self).__init__(context) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server() #init torso self.torso = Torso() #init gripper self.l_gripper = Gripper('l') self.r_gripper = Gripper('r') #init navigation self.navigation = Navigation() self._widget = QWidget() self._widget.setFixedSize(600, 600) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) large_box = QtGui.QVBoxLayout() #Button for outreaching to recieve book from Human button_box = QtGui.QVBoxLayout() box_1 = QtGui.QHBoxLayout() box_2 = QtGui.QHBoxLayout() box_3 = QtGui.QHBoxLayout() box_4 = QtGui.QHBoxLayout() box_5 = QtGui.QHBoxLayout() box_6 = QtGui.QHBoxLayout() box_7 = QtGui.QHBoxLayout() box_8 = QtGui.QHBoxLayout() box_9 = QtGui.QHBoxLayout() box_10 = QtGui.QHBoxLayout() box_1.addItem(QtGui.QSpacerItem(15, 2)) box_1.addWidget( self.create_button('Prepare To Take', self.prepare_to_take)) box_1.addItem(QtGui.QSpacerItem(445, 2)) box_2.addItem(QtGui.QSpacerItem(15, 2)) box_2.addWidget( self.create_button('Take From Human', self.take_from_human)) box_2.addItem(QtGui.QSpacerItem(445, 2)) box_3.addItem(QtGui.QSpacerItem(15, 2)) box_3.addWidget( self.create_button('Prepare To Navigate', self.prepare_to_navigate)) box_3.addItem(QtGui.QSpacerItem(445, 2)) # Button to move to shelf box_5.addItem(QtGui.QSpacerItem(15, 2)) box_5.addWidget( self.create_button('Navigate To Shelf', self.navigate_to_shelf)) box_5.addItem(QtGui.QSpacerItem(445, 2)) box_4.addItem(QtGui.QSpacerItem(15, 2)) box_4.addWidget( self.create_button('Place On Shelf', self.place_on_shelf)) box_4.addItem(QtGui.QSpacerItem(445, 2)) box_6.addItem(QtGui.QSpacerItem(15, 2)) box_6.addWidget( self.create_button('Give Information', self.give_information)) box_6.addItem(QtGui.QSpacerItem(445, 2)) box_7.addItem(QtGui.QSpacerItem(15, 2)) box_7.addWidget( self.create_button('Navigate To Person', self.navigate_to_person)) box_7.addItem(QtGui.QSpacerItem(445, 2)) self.book_textbox = QtGui.QLineEdit() self.book_textbox.setFixedWidth(100) box_8.addItem(QtGui.QSpacerItem(15, 2)) box_8.addWidget(self.book_textbox) box_8.addWidget( self.create_button('Pick Up Book', self.pick_up_from_shelf_button)) box_8.addItem(QtGui.QSpacerItem(445, 2)) box_9.addItem(QtGui.QSpacerItem(15, 2)) box_9.addWidget(self.create_button('Localize', self.spin_base)) box_9.addItem(QtGui.QSpacerItem(445, 2)) box_10.addItem(QtGui.QSpacerItem(15, 2)) box_10.addWidget( self.create_button('Non-nav Pick Up', self.pick_up_from_shelf)) box_10.addItem(QtGui.QSpacerItem(445, 2)) button_box.addItem(QtGui.QSpacerItem(20, 120)) button_box.addLayout(box_1) button_box.addLayout(box_2) button_box.addLayout(box_3) button_box.addLayout(box_5) button_box.addLayout(box_4) button_box.addLayout(box_6) button_box.addLayout(box_7) button_box.addLayout(box_8) button_box.addLayout(box_9) button_box.addLayout(box_10) button_box.addItem(QtGui.QSpacerItem(20, 240)) large_box.addLayout(button_box) self.marker_perception = ReadMarkers() self.speech_recognition = SpeechRecognition(self) self.bookDB = BookDB() self.book_map = self.bookDB.getAllBookCodes() self._widget.setObjectName('Milestone1GUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet( "QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../librarian_gui_background.jpg"))