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 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)