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, 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, 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)
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)
class Robot_Skeleton(object): """Basic bare-bones solution for the Fetch robot interface. We recommend extending this class with additional convenience methods based on your application needs. """ 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 body_start_pose(self, start_height=0.10, end_height=0.10, velocity_factor=0.5): """Sets the robot's body to some initial configuration. Tucks the arm using motion planning. NEVER directly set joints as that often leads to collisions. Args: start_height: Height in meters for Fetch before arm-tuck. end_height: Height in meters for Fetch after arm-tuck. velocity_factor: controls the speed, closer to 0 means slower, closer to 1 means faster. (If 0.0, then it turns into 1.0 for some reason.) Values greater than 1.0 are cut to 1.0. """ self.torso.set_height(start_height) self.arm.move_to_joint_goal(self.tucked_list, velocity_factor=velocity_factor) self.torso.set_height(end_height) def head_start_pose(self, pan=0.0, tilt=0.0): """Sets the robot's head to some initial configuration. Args: pan: Value in radians for head sideways rotation, counterclockwise when looking at robot from an aerial view. tilt: Value in radians for head up/down movement, positive means looking downwards. """ self.head.pan_tilt(pan=pan, tilt=tilt) def get_img_data(self): """Obtain camera and depth image. Returns: Tuple containing RGB camera image and corresponding depth image. """ c_img = self.camera.read_color_data() d_img = self.camera.read_depth_data() return (c_img, d_img) def create_grasp_pose(self, x, y, z, rot_x, rot_y, rot_z): """Creates a pose in the world for the robot's end-effect to go to. Args: x, y, z, rot_x, rot_y, rot_z: A 6-D pose description. """ pose_name = self.gripper.create_grasp_pose_intuitive( x, y, z, rot_x, rot_y, rot_z) return pose_name def move_to_pose(self, pose_name, velocity_factor=None): """Moves to a pose. In the HSR, moved the `hand_palm_link` to the frame named `pose_name` at the correct pose. For the Fetch we should be able to extract the pose from `pose_name` and then call the Arm's `move_to_pose` method. Args: pose_name: A string name for the pose to go velocity_factor: controls the speed, closer to 0 means slower, closer to 1 means faster. (If 0.0, then it turns into 1.0 for some reason.) Values greater than 1.0 are cut to 1.0. """ # See: # http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29 # https://answers.ros.org/question/256354/does-tftransformlistenerlookuptransform-return-quaternion-position-or-translation-and-rotation/ # First frame should be the reference frame, use `base_link`, not `odom`. point, quat = self.gripper.tl.lookupTransform('base_link', pose_name, rospy.Time(0)) # See: # https://github.com/cse481wi18/cse481wi18/blob/indigo-devel/applications/scripts/cart_arm_demo.py # https://github.com/cse481wi18/cse481wi18/wiki/Lab-19%3A-Cartesian-space-manipulation ps = PoseStamped() ps.header.frame_id = 'base_link' ps.pose = Pose( Point(point[0], point[1], point[2]), Quaternion(quat[0], quat[1], quat[2], quat[3]) ) # See `arm.py` written by Justin Huang error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor) if error is not None: rospy.logerr(error) def open_gripper(self): self.gripper.open() def close_gripper(self, width=0.0, max_effort=100): self.gripper.close(width=width, max_effort=max_effort)
class Body: 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) def horiz_move(self, gx, gy, gz): movement = tf.translate(Vector( gx, gy, gz)) # Affine matrix for horizontal movement. self._body_center = movement * self._init_torso_center self.head.horiz_move(x, y, z) self.torso.horiz_move(x, y, z) self.left_arm.horiz_move(x, y, z) self.right_arm.horiz_move(x, y, z) if self._lower_body_flag: self.left_leg.horiz_move(x, y, z) self.right_leg.horiz_move(x, y, z) def calc_body_center(self): return self._body_center def get_property_list(self): property_list = [] property_list.extend(self.head.get_property_list()) property_list.extend(self.torso.get_property_list()) property_list.extend(self.left_arm.get_property_list()) property_list.extend(self.right_arm.get_property_list()) if self._lower_body_flag: property_list.extend(self.left_leg.get_property_list()) property_list.extend(self.right_leg.get_property_list()) return property_list def constr_json_data(self): # Construcat JSON data json_data_head = self.head.constr_json_data() json_data_torso = self.torso.constr_json_data() json_data_left_arm = self.left_arm.constr_json_data() json_data_right_arm = self.right_arm.constr_json_data() if self._lower_body_flag: json_data_left_leg = self.left_leg.constr_json_data() json_data_right_leg = self.right_leg.constr_json_data() json_data = { 'joint_angle': { 'left_shoulder': json_data_left_arm['joint_angle']['shoulder'], 'left_elbow': json_data_left_arm['joint_angle']['elbow'], 'right_shoulder': json_data_right_arm['joint_angle']['shoulder'], 'right_elbow': json_data_right_arm['joint_angle']['elbow'] }, 'true_position': { 'head': json_data_head['true_position']['head'], 'torso': json_data_torso['true_position']['torso'], 'left_shouler': json_data_left_arm['true_position']['shoulder'], 'left_elbow': json_data_left_arm['true_position']['elbow'], 'left_hand': json_data_left_arm['true_position']['hand'], 'right_shoulder': json_data_right_arm['true_position']['shoulder'], 'right_elbow': json_data_right_arm['true_position']['elbow'], 'right_hand': json_data_right_arm['true_position']['hand'] } } if self._lower_body_flag: json_data['joint_angle']['left_hip'] = json_data_left_leg[ 'joint_angle']['hip'] json_data['joint_angle']['left_knee'] = json_data_left_leg[ 'joint_angle']['knee'] json_data['joint_angle']['right_hip'] = json_data_right_leg[ 'joint_angle']['hip'] json_data['joint_angle']['right_knee'] = json_data_right_leg[ 'joint_angle']['knee'] json_data['true_position']['left_hip'] = json_data_left_leg[ 'true_position']['hip'] json_data['true_position']['left_knee'] = json_data_left_leg[ 'true_position']['knee'] json_data['true_position']['left_foot'] = json_data_left_leg[ 'true_position']['foot'] json_data['true_position']['right_hip'] = json_data_right_leg[ 'true_position']['hip'] json_data['true_position']['right_knee'] = json_data_right_leg[ 'true_position']['knee'] json_data['true_position']['right_foot'] = json_data_right_leg[ 'true_position']['foot'] return json_data def set_from_json_data(self, json_data): # Set angle parameters from json data (not json file!) self.left_arm.set_from_json_data( json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow']) self.right_arm.set_from_json_data( json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow']) if self._lower_body_flag: self.left_leg.set_from_json_data( json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee']) self.right_leg.set_from_json_data( json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee']) def is_collision_body_parts(self): def collision_detection_generator(): # left arm vs. cranicm yield self.left_arm.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left arm vs. torso yield self.left_arm.is_collision_to_capsule( self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) # left arm vs. right upperarm yield self.left_arm.is_collision_to_capsule( self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius()) # left arm vs. right elbow yield self.left_arm.is_collision_to_sphere( self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius()) # left arm vs. right forearm yield self.left_arm.is_collision_to_capsule( self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius()) # right arm vs. cranicm yield self.right_arm.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # right arm vs. torso yield self.right_arm.is_collision_to_capsule( self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) if self._lower_body_flag: # left arm vs. left thigh yield self.left_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # left arm vs. left knee yield self.left_arm.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # left arm vs. left calf yield self.left_arm.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # left arm vs. right thigh yield self.left_arm.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left arm vs. right knee yield self.left_arm.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left arm vs. right calf yield self.left_arm.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left knee yield self.right_arm.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # right arm vs. left calf yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # right arm vs. right thigh yield self.right_arm.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # right arm vs. right knee yield self.right_arm.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # right arm vs. right calf yield self.right_arm.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # left leg vs. cranicm yield self.left_leg.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left leg vs. right thigh yield self.left_leg.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left leg vs. right knee yield self.left_leg.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left leg vs. right calf yield self.left_leg.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right leg vs. cranicm yield self.right_leg.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # torso vs. left knee yield self.torso.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # torso vs. left calf yield self.torso.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # torso vs. right knee yield self.torso.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # torso vs. right calf yield self.torso.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) for detection in collision_detection_generator(): if detection: return True return False
class Body: 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) def horiz_move(self, gx, gy, gz): movement = tf.translate(Vector(gx, gy, gz)) # Affine matrix for horizontal movement. self._body_center = movement * self._init_torso_center self.head.horiz_move(x, y, z) self.torso.horiz_move(x, y, z) self.left_arm.horiz_move(x, y, z) self.right_arm.horiz_move(x, y, z) if self._lower_body_flag: self.left_leg.horiz_move(x, y, z) self.right_leg.horiz_move(x, y, z) def calc_body_center(self): return self._body_center def get_property_list(self): property_list = [] property_list.extend(self.head.get_property_list()) property_list.extend(self.torso.get_property_list()) property_list.extend(self.left_arm.get_property_list()) property_list.extend(self.right_arm.get_property_list()) if self._lower_body_flag: property_list.extend(self.left_leg.get_property_list()) property_list.extend(self.right_leg.get_property_list()) return property_list def constr_json_data(self): # Construcat JSON data json_data_head = self.head.constr_json_data() json_data_torso = self.torso.constr_json_data() json_data_left_arm = self.left_arm.constr_json_data() json_data_right_arm = self.right_arm.constr_json_data() if self._lower_body_flag: json_data_left_leg = self.left_leg.constr_json_data() json_data_right_leg= self.right_leg.constr_json_data() json_data = { 'joint_angle': { 'left_shoulder': json_data_left_arm['joint_angle']['shoulder'], 'left_elbow': json_data_left_arm['joint_angle']['elbow'], 'right_shoulder': json_data_right_arm['joint_angle']['shoulder'], 'right_elbow': json_data_right_arm['joint_angle']['elbow'] }, 'true_position': { 'head': json_data_head['true_position']['head'], 'torso': json_data_torso['true_position']['torso'], 'left_shouler': json_data_left_arm['true_position']['shoulder'], 'left_elbow': json_data_left_arm['true_position']['elbow'], 'left_hand': json_data_left_arm['true_position']['hand'], 'right_shoulder': json_data_right_arm['true_position']['shoulder'], 'right_elbow': json_data_right_arm['true_position']['elbow'], 'right_hand': json_data_right_arm['true_position']['hand'] } } if self._lower_body_flag: json_data['joint_angle']['left_hip'] = json_data_left_leg['joint_angle']['hip'] json_data['joint_angle']['left_knee'] = json_data_left_leg['joint_angle']['knee'] json_data['joint_angle']['right_hip'] = json_data_right_leg['joint_angle']['hip'] json_data['joint_angle']['right_knee'] = json_data_right_leg['joint_angle']['knee'] json_data['true_position']['left_hip'] = json_data_left_leg['true_position']['hip'] json_data['true_position']['left_knee'] = json_data_left_leg['true_position']['knee'] json_data['true_position']['left_foot'] = json_data_left_leg['true_position']['foot'] json_data['true_position']['right_hip'] = json_data_right_leg['true_position']['hip'] json_data['true_position']['right_knee'] = json_data_right_leg['true_position']['knee'] json_data['true_position']['right_foot'] = json_data_right_leg['true_position']['foot'] return json_data def set_from_json_data(self, json_data): # Set angle parameters from json data (not json file!) self.left_arm.set_from_json_data(json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow']) self.right_arm.set_from_json_data(json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow']) if self._lower_body_flag: self.left_leg.set_from_json_data(json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee']) self.right_leg.set_from_json_data(json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee']) def is_collision_body_parts(self): def collision_detection_generator(): # left arm vs. cranicm yield self.left_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left arm vs. torso yield self.left_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) # left arm vs. right upperarm yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius()) # left arm vs. right elbow yield self.left_arm.is_collision_to_sphere(self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius()) # left arm vs. right forearm yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius()) # right arm vs. cranicm yield self.right_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # right arm vs. torso yield self.right_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) if self._lower_body_flag: # left arm vs. left thigh yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # left arm vs. left knee yield self.left_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # left arm vs. left calf yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # left arm vs. right thigh yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left arm vs. right knee yield self.left_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left arm vs. right calf yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left knee yield self.right_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # right arm vs. left calf yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # right arm vs. right thigh yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # right arm vs. right knee yield self.right_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # right arm vs. right calf yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # left leg vs. cranicm yield self.left_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left leg vs. right thigh yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left leg vs. right knee yield self.left_leg.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left leg vs. right calf yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right leg vs. cranicm yield self.right_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # torso vs. left knee yield self.torso.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # torso vs. left calf yield self.torso.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # torso vs. right knee yield self.torso.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # torso vs. right calf yield self.torso.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) for detection in collision_detection_generator(): if detection: return True return False
class Robot_Interface(object): """For usage with the Fetch robot.""" 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 body_start_pose(self, start_height=0.10, end_height=0.10, velocity_factor=None): """Sets the robot's body to some initial configuration. The HSR uses `whole_body.move_to_go()` which initializes an appropriate posture so that the hand doesn't collide with movement. For the Fetch, we should probably make the torso extend, so the arms can extend more easily without collisions. Use `move_to_joint_goal` since that uses motion planning. Do NOT directly set the joints without planning!! """ self.torso.set_height(start_height) self.arm.move_to_joint_goal(self.tucked_list, velocity_factor=velocity_factor) self.torso.set_height(end_height) # Specific to the siemens challenge (actually a lot of this stuff is ...) if self.num_restarts == 0: self.base.turn(angular_distance=45 * DEG_TO_RAD) self.num_restarts += 1 def head_start_pose(self): """Hard-coded starting pose for the robot's head. These values are from the HSR. The Fetch needs a different pan and tilt. Positive pan means rotating counterclockwise when looking at robot from an aerial view. """ self.head.pan_tilt(pan=0.0, tilt=0.8) def position_start_pose(self, offsets=None, do_something=False): """Assigns the robot's base to some starting pose. Mainly to "reset" the robot to the original starting position (and also, rotation about base axis) after it has moved, usually w/no offsets. Ugly workaround: we have target (x,y), and compute the distance to the point and the angle. We turn the Fetch according to that angle, and go forward. Finally, we do a second turn which corresponds to the target yaw at the end. This turns w.r.t. the current angle, so we undo the effect of the first turn. See `examples/test_position_start_pose.py` for tests. Args: offsets: a list of length 3, indicating offsets in the x, y, and yaws, respectively, to be added onto the starting pose. """ # Causing problems during my tests of the Siemens demo. if not do_something: return current_pos = copy.deepcopy(self.base.odom.position) current_theta = Base._yaw_from_quaternion( self.base.odom.orientation) # [-pi, pi] ss = np.array([current_pos.x, current_pos.y, current_theta]) # Absolute target position and orientation specified with `pp`. pp = np.copy(self.start_pose) if offsets: pp += np.array(offsets) # Get distance to travel, critically assumes `pp` is starting position. dist = np.sqrt((ss[0] - pp[0])**2 + (ss[1] - pp[1])**2) rel_x = ss[0] - pp[0] rel_y = ss[1] - pp[1] assert -1 <= rel_x / dist <= 1 assert -1 <= rel_y / dist <= 1 # But we also need to be *facing* the correct direction, w/input [-1,1]. # First, get the opposite view (facing "outwards"), then flip by 180. desired_facing = np.arctan2(rel_y, rel_x) # [-pi, pi], facing outward desired_theta = math.pi + desired_facing # [0, 2*pi], flip by 180 if desired_theta > math.pi: desired_theta -= 2 * math.pi # [-pi, pi] # Reconcile with the current theta. Got this by basically trial/error angle = desired_theta - current_theta # [-2*pi, 2*pi] if angle > math.pi: angle -= 2 * math.pi elif angle < -math.pi: angle += 2 * math.pi self.base.turn(angular_distance=angle, speed=self.TURN_SPEED) self.base.go_forward(distance=dist, speed=0.2) # Back at the start x, y, but now need to consider the _second_ turn. # The robot is facing at `desired_theta` rads, but wants `pp[2]` rads. final_angle = pp[2] - desired_theta if final_angle > math.pi: final_angle -= 2 * math.pi elif final_angle < -math.pi: final_angle += 2 * math.pi self.base.turn(angular_distance=final_angle, speed=self.TURN_SPEED) def get_img_data(self): """Obtain camera and depth image. Returns: Tuple containing RGB camera image and corresponding depth image. """ c_img = self.camera.read_color_data() d_img = self.camera.read_depth_data() return (c_img, d_img) def get_depth(self, point, d_img): """Compute mean depth near grasp point. NOTE: assumes that we have a simlar `cfg.ZRANGE` as with the HSR. I'm not sure where exactly this comes from. """ y, x = int(point[0]), int(point[1]) # z_box = d_img[y-ZRANGE:y+ZRANGE, x-ZRANGE:x+ZRANGE] z_box = d_img[y - 20:y + 20, x - 20:x + 20] indx = np.nonzero(z_box) z = np.mean(z_box[indx]) return z # y, x = int(point[0]), int(point[1]) # # z_box = d_img[y-ZRANGE:y+ZRANGE, x-ZRANGE:x+ZRANGE] # z_box = d_img[y-10:y + 10, x - 10:x + 10] # indx = np.nonzero(z_box) # z = np.mean(z_box[indx]) # return z def get_rot(self, direction): """Compute rotation of gripper such that given vector is grasped. Currently this directly follows the HSR code as there's nothing Fetch-dependent. """ dy, dx = direction[0], direction[1] dx *= -1 if dy < 0: dx *= -1 dy *= -1 rot = np.arctan2(dy, dx) rot = np.pi - rot return rot def create_grasp_pose(self, x, y, z, rot): """ If `intuitive=True` then x,y,z,rot interpreted wrt some link in the world, e.g., 'odom' for the Fetch. It's False by default to maintain backwards compatibility w/Siemens-based code. """ pose_name = self.gripper.create_grasp_pose(x, y, z, rot) return pose_name def open_gripper(self): self.gripper.open() def close_gripper(self): self.gripper.close() def move_to_pose(self, pose_name, z_offset, velocity_factor=None): """Moves to a pose. In the HSR, moved the `hand_palm_link` to the frame named `pose_name` at the correct pose. For the Fetch we should be able to extract the pose from `pose_name` and then call the Arm's `move_to_pose` method. Args: pose_name: A string name for the pose to go z_offset: Scalar offset in z-direction, offset is wrt the pose specified by `pose_name`. velocity_factor: controls the speed, closer to 0 means slower, closer to 1 means faster. (If 0.0, then it turns into 1.0 for some reason.) Values greater than 1.0 are cut to 1.0. """ # See: # http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29 # https://answers.ros.org/question/256354/does-tftransformlistenerlookuptransform-return-quaternion-position-or-translation-and-rotation/ # First frame should be the reference frame, use `base_link`, not `odom`. point, quat = self.gripper.tl.lookupTransform('base_link', pose_name, rospy.Time(0)) z_point = point[2] + z_offset # See: # https://github.com/cse481wi18/cse481wi18/blob/indigo-devel/applications/scripts/cart_arm_demo.py # https://github.com/cse481wi18/cse481wi18/wiki/Lab-19%3A-Cartesian-space-manipulation ps = PoseStamped() ps.header.frame_id = 'base_link' ps.pose = Pose(Point(point[0], point[1], z_point), Quaternion(quat[0], quat[1], quat[2], quat[3])) # See `arm.py` written by Justin Huang error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor) if error is not None: rospy.logerr(error) def find_ar(self, ar_number, velocity_factor=None): try: ar_name = 'ar_marker/' + str(ar_number) # HSR code, with two hard-coded offsets? #self.whole_body.move_end_effector_pose(geometry.pose(y=0.08, z=-0.3), ar_name) # Fetch 'translation'. Note the `ar_name` for pose name. point, quat = self.gripper.tl.lookupTransform( 'base_link', ar_name, rospy.Time(0)) y_point = point[1] + 0.08 z_point = point[2] - 0.3 ps = PoseStamped() ps.header.frame_id = 'base_link' ps.pose = Pose(Point(point[0], y_point, z_point), Quaternion(quat[0], quat[1], quat[2], quat[3])) error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor) if error is not None: rospy.logerr(error) return True except: return False def pan_head(self, tilt): """Adjusts tilt of the robot, AND set pan at zero. Args: tilt: Value in radians, positive means looking downwards. """ self.head.pan_tilt(pan=0, tilt=tilt)
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"))
class Milestone1GUI(Plugin): RECEIVE_FROM_HUMAN_R_POS = [ 0.00952670015493673, 0.3270780665526253, 0.03185028324084582, -1.3968658009779173, -3.058799671876592, -1.1083678332942686, -1.6314425515258866 ] READ_FIDUCIAL_R_POS = [ 0.41004856860653505, 0.29772362823537935, -0.019944325676627628, -1.8394298656773618, -0.44139252862458106, -0.09922194050427624, -4.761735317011306 ] NAVIGATE_R_POS = [ -0.3594077470836499, 0.971353000916152, -1.9647276598906076, -1.431900313132731, -1.1839177367219755, -0.09817772642188527, -1.622044624784374 ] PLACE_ON_SHELF_R_POS = [ -0.15015144487461773, 0.4539704512093072, -0.10846018983280459, -0.9819529421527269, -3.0207362886631235, -0.4990689162195487, -1.6026396464199553 ] LOWER_ON_SHELF_R_POS = [ -0.2159792990560645, 0.6221451583409631, -0.1886376030177479, -0.959223940465513, 9.690004126983537, -0.2866303982732683, 111.39703078836274 ] RELEASE_BOOK_R_POS = [ -0.15545746838546493, 0.44145040258984786, -0.13267376861465774, -0.972108533778647, -3.028545645401449, -0.38572817936010495, 0.008017066746929924 ] TUCKED_UNDER_L_POS = [ 0.05688828299939419, 1.2955758539090194, 1.7180547710154033, -1.4511548177467404, -0.28718096455759035, -0.0938208188421713, -10.980395466225648 ] sound_sig = Signal(SoundRequest) 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")) def get_function(self, text): functions = { "bring-me-a-book": self.pick_up_from_shelf_routine, "put-this-book-back": self.prepare_to_take, "give-me-information": self.give_information_routine, "here-you-go": self.put_this_book_back_routine, } if text not in functions: print("Could not find key %s" % text) return None return functions[text] def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def prepare_to_take(self): # Open gripper and move arms to take book self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('l', 5.0) self.move_arm('r', 5.0) # Increase these numbers for slower movement self.l_gripper.close_gripper() self.r_gripper.open_gripper(True) self._sound_client.say("Please give me a book") def take_from_human(self): self.marker_perception.is_listening = True # Close gripper and move arms to see book self.r_gripper.close_gripper(True) self._sound_client.say("Thank you") time.sleep(1) self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS self.move_arm('r', 5.0, True) # Increase these numbers for slower movement self.look_at_r_gripper() rospy.loginfo("marker id returned by get_marker_id is: " + str(self.marker_perception.get_marker_id())) def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def prepare_to_navigate(self): self.marker_perception.is_listening = False # Tuck arms self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('r', 5.0) # Increase these numbers for slower movement #spin 360 * rotate_count degress clock wise def spin_base(self, rotate_count): # Adjust height and tuck arms before localization self._sound_client.say("Please wait while I orient myself.") self.torso.move(.15) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('l', 5.0, True) self.move_arm('r', 5.0, True) self.l_gripper.close_gripper() self.r_gripper.close_gripper() if not rotate_count: rotate_count = 2 topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.5) start_time = rospy.get_rostime() while rospy.get_rostime() < start_time + rospy.Duration( 15.0 * rotate_count): base_publisher.publish(twist_msg) def navigate_to_shelf(self, marker_id=None): # Move forward, place book on the shelf, and move back if marker_id is None or marker_id is False: marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is False: rospy.loginfo("wuuuuuuuut") if marker_id is None: self._sound_client.say("I don't think I am holding a book " "right now") rospy.logwarn("Navigate to shelf called when marker id is None") return book = self.book_map.get(unicode(marker_id)) if book is None: self._sound_client.say("The book that I am holding is unknown " "to me") rospy.logwarn( "Navigate to shelf called when marker id is not in database") return x = book.getXCoordinate() y = book.getYCoordinate() self.navigation.move_to_shelf(x, y) # Ye Olde Dropping way of putting a book on the shelf. Deprecating. def drop_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.r_gripper.open_gripper(True) self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.move_base(False) self.marker_perception.reset_marker_id() def place_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(False) self.marker_perception.reset_marker_id() def give_information(self): marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is not None: book = self.book_map.get(unicode(marker_id)) if book is None: rospy.logwarn( "Give information called when marker id is not in database" ) self._sound_client.say( "The book that I am holding is unknown to me") else: self._sound_client.say(book.getInformation()) else: rospy.logwarn("Give information called when marker id is None") self._sound_client.say( "I don't think I am holding a book right now") def pick_up_from_shelf_button(self): self.pick_up_from_shelf_routine(self.book_textbox.text()) def pick_up_from_shelf_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say( "The book you requested is not present in the database.") else: self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.move_arm('l', 5.0) self.l_gripper.close_gripper() # self.marker_perception.set_marker_id(book_id) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf(book_id) # Navigate to book location self.pick_up_from_shelf() # Pick up from the shelf self.prepare_to_navigate() time.sleep(5) self.navigate_to_person( ) # Navigate to designated help desk location self.give_book() # Give Book to Human def put_this_book_back_routine(self): self.take_from_human() time.sleep(5) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf() self.place_on_shelf() self.prepare_to_navigate() # TODO: return to human? def give_information_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say( "The book you requested is not present in the database.") return book = self.book_map.get(unicode(book_id)) self._sound_client.say(book.getInformation()) def pick_up_from_shelf(self): self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(True) self.r_gripper.close_gripper(True) self.move_base(False) def navigate_to_person(self): x = 2.82690143585 y = -0.416650295258 z = 0.252587109056 w = 0.967574158573 self.navigation.move_to_shelf(x, y, z, w) def give_book(self): self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('r', 4.0, True) self.r_gripper.open_gripper(True) # Moves forward to the bookshelf (or backward if isForward is false) def move_base(self, isForward): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) distance = 0.25 if not isForward: distance *= -1 twist_msg = Twist() twist_msg.linear = Vector3(distance, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for x in range(0, 15): rospy.loginfo("Moving the base") base_publisher.publish(twist_msg) time.sleep(0.15) time.sleep(1.5) # Moves arms using kinematics def move_arm(self, side_prefix, time_to_joints=2.0, wait=False): # forward kinematics if (side_prefix == 'r'): if self.saved_r_arm_pose is None: rospy.logerr('Target pose for right arm is None, cannot move.') else: self.freeze_arm(side_prefix) self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints, wait) else: # side_prefix == 'l' if self.saved_l_arm_pose is None: rospy.logerr('Target pose for left arm is None, cannot move.') else: self.freeze_arm(side_prefix) self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints, wait) pass def freeze_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [controller_name] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def move_to_joints(self, side_prefix, positions, time_to_joint, wait=False): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) if side_prefix == 'r': traj_goal.trajectory.joint_names = self.r_joint_names action_client = self.r_traj_action_client else: traj_goal.trajectory.joint_names = self.l_joint_names action_client = self.l_traj_action_client action_client.send_goal(traj_goal) if wait: time.sleep(time_to_joint)
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"))
class Milestone1GUI(Plugin): RECEIVE_FROM_HUMAN_R_POS = [0.00952670015493673, 0.3270780665526253, 0.03185028324084582, -1.3968658009779173, -3.058799671876592, -1.1083678332942686, -1.6314425515258866] READ_FIDUCIAL_R_POS = [0.41004856860653505, 0.29772362823537935, -0.019944325676627628, -1.8394298656773618, -0.44139252862458106, -0.09922194050427624, -4.761735317011306] NAVIGATE_R_POS = [-0.3594077470836499, 0.971353000916152, -1.9647276598906076, -1.431900313132731, -1.1839177367219755, -0.09817772642188527, -1.622044624784374] PLACE_ON_SHELF_R_POS = [-0.15015144487461773, 0.4539704512093072, -0.10846018983280459, -0.9819529421527269, -3.0207362886631235, -0.4990689162195487, -1.6026396464199553] LOWER_ON_SHELF_R_POS = [-0.2159792990560645, 0.6221451583409631, -0.1886376030177479, -0.959223940465513, 9.690004126983537, -0.2866303982732683, 111.39703078836274] RELEASE_BOOK_R_POS = [-0.15545746838546493, 0.44145040258984786, -0.13267376861465774, -0.972108533778647, -3.028545645401449, -0.38572817936010495, 0.008017066746929924] TUCKED_UNDER_L_POS = [0.05688828299939419, 1.2955758539090194, 1.7180547710154033, -1.4511548177467404, -0.28718096455759035, -0.0938208188421713, -10.980395466225648] sound_sig = Signal(SoundRequest) 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")) def get_function(self, text): functions = { "bring-me-a-book": self.pick_up_from_shelf_routine, "put-this-book-back": self.prepare_to_take, "give-me-information": self.give_information_routine, "here-you-go": self.put_this_book_back_routine, } if text not in functions: print ("Could not find key %s" % text) return None return functions[text] def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def prepare_to_take(self): # Open gripper and move arms to take book self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('l', 5.0) self.move_arm('r', 5.0) # Increase these numbers for slower movement self.l_gripper.close_gripper() self.r_gripper.open_gripper(True) self._sound_client.say("Please give me a book") def take_from_human(self): self.marker_perception.is_listening = True # Close gripper and move arms to see book self.r_gripper.close_gripper(True) self._sound_client.say("Thank you") time.sleep(1) self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS self.move_arm('r', 5.0, True) # Increase these numbers for slower movement self.look_at_r_gripper() rospy.loginfo("marker id returned by get_marker_id is: " + str(self.marker_perception.get_marker_id())) def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def prepare_to_navigate(self): self.marker_perception.is_listening = False # Tuck arms self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('r', 5.0) # Increase these numbers for slower movement #spin 360 * rotate_count degress clock wise def spin_base(self, rotate_count): # Adjust height and tuck arms before localization self._sound_client.say("Please wait while I orient myself.") self.torso.move(.15) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('l', 5.0, True) self.move_arm('r', 5.0, True) self.l_gripper.close_gripper() self.r_gripper.close_gripper() if not rotate_count: rotate_count = 2 topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.5) start_time = rospy.get_rostime() while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count): base_publisher.publish(twist_msg) def navigate_to_shelf(self, marker_id = None): # Move forward, place book on the shelf, and move back if marker_id is None or marker_id is False: marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is False: rospy.loginfo("wuuuuuuuut") if marker_id is None: self._sound_client.say("I don't think I am holding a book " "right now") rospy.logwarn("Navigate to shelf called when marker id is None") return book = self.book_map.get(unicode(marker_id)) if book is None: self._sound_client.say("The book that I am holding is unknown " "to me") rospy.logwarn("Navigate to shelf called when marker id is not in database") return x = book.getXCoordinate() y = book.getYCoordinate() self.navigation.move_to_shelf(x, y) # Ye Olde Dropping way of putting a book on the shelf. Deprecating. def drop_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.r_gripper.open_gripper(True) self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.move_base(False) self.marker_perception.reset_marker_id() def place_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(False) self.marker_perception.reset_marker_id() def give_information(self): marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is not None: book = self.book_map.get(unicode(marker_id)) if book is None: rospy.logwarn("Give information called when marker id is not in database") self._sound_client.say("The book that I am holding is unknown to me") else: self._sound_client.say(book.getInformation()) else: rospy.logwarn("Give information called when marker id is None") self._sound_client.say("I don't think I am holding a book right now") def pick_up_from_shelf_button(self): self.pick_up_from_shelf_routine(self.book_textbox.text()) def pick_up_from_shelf_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say("The book you requested is not present in the database.") else: self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.move_arm('l', 5.0) self.l_gripper.close_gripper() # self.marker_perception.set_marker_id(book_id) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf(book_id) # Navigate to book location self.pick_up_from_shelf() # Pick up from the shelf self.prepare_to_navigate() time.sleep(5) self.navigate_to_person() # Navigate to designated help desk location self.give_book() # Give Book to Human def put_this_book_back_routine(self): self.take_from_human() time.sleep(5) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf() self.place_on_shelf() self.prepare_to_navigate() # TODO: return to human? def give_information_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say("The book you requested is not present in the database.") return book = self.book_map.get(unicode(book_id)) self._sound_client.say(book.getInformation()) def pick_up_from_shelf(self): self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(True) self.r_gripper.close_gripper(True) self.move_base(False) def navigate_to_person(self): x = 2.82690143585 y = -0.416650295258 z = 0.252587109056 w = 0.967574158573 self.navigation.move_to_shelf(x, y, z, w) def give_book(self): self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('r', 4.0, True) self.r_gripper.open_gripper(True) # Moves forward to the bookshelf (or backward if isForward is false) def move_base(self, isForward): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) distance = 0.25 if not isForward: distance *= -1 twist_msg = Twist() twist_msg.linear = Vector3(distance, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for x in range(0, 15): rospy.loginfo("Moving the base") base_publisher.publish(twist_msg) time.sleep(0.15) time.sleep(1.5) # Moves arms using kinematics def move_arm(self, side_prefix, time_to_joints = 2.0, wait = False): # forward kinematics if (side_prefix == 'r'): if self.saved_r_arm_pose is None: rospy.logerr('Target pose for right arm is None, cannot move.') else: self.freeze_arm(side_prefix) self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints, wait) else: # side_prefix == 'l' if self.saved_l_arm_pose is None: rospy.logerr('Target pose for left arm is None, cannot move.') else: self.freeze_arm(side_prefix) self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints, wait) pass def freeze_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [controller_name] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def move_to_joints(self, side_prefix, positions, time_to_joint, wait = False): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) if side_prefix == 'r': traj_goal.trajectory.joint_names = self.r_joint_names action_client = self.r_traj_action_client else: traj_goal.trajectory.joint_names = self.l_joint_names action_client = self.l_traj_action_client action_client.send_goal(traj_goal) if wait: time.sleep(time_to_joint)