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)
