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 compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def test_reader(arm, reader):
names = ArmJoints.names()
arm_vals = reader.get_joints(names)
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("")
def move_to_joints(self, joint_state):
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))
def test_shoulders(arm, torso):
# Shoulder pan joint
# Could be useful to see the individual joints. Note that using
# `arm.move_to_joints()` doesn't require MoveIt. If we adjust the 0-th
# indexed joint angle, that will rotate the arm like if we were rotating our
# arm going across at eye level, with the hand, wrist, elbow, etc., FIXED.
print("shoulder pan joint")
pose0 = [0, 0, 0, 0, 0, 0, 0]
pose0[0] = DEGS_TO_RADS * -92
arm.move_to_joints(ArmJoints.from_list(pose0))
pose0[0] = DEGS_TO_RADS * 92
arm.move_to_joints(ArmJoints.from_list(pose0))
# Shoulder LIFT joint, now imagine we have a straight arm (actually, it just
# has to be fixed, not straight...) but we move it up and down. :-)
print("shoulder LIFT joint")
pose1 = [0, 0, 0, 0, 0, 0, 0]
pose1[1] = DEGS_TO_RADS * -70
arm.move_to_joints(ArmJoints.from_list(pose1))
pose1[1] = DEGS_TO_RADS * 87 # ha, this is amusing
arm.move_to_joints(ArmJoints.from_list(pose1))
def move_to_joints(self, joint_state):
""" Often we'll write down the seven joints and call this.
I think this doesn't require MoveIt, but the downside is we can't easily
generalize to different robotic systems since we normally want to go to
an (x,y,z) coordinate. Using our disco fetch example, the joint_state is
just one list, with the trajectory consisting of one target element
(which, again is the 7DOF joint angle we specify).
"""
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))