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