def __init__(self): self._joint_state_listener = JointStateListener() self._arm_planners = {} self.side_joint_trajectory = {} self._arm_mover = ArmMover() for arm in ['left_arm', 'right_arm']: self._arm_planners[arm] = ArmPlanner(arm) self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm, DEFAULT_SIDE_JOINT_TRAJECTORY[arm])
class ArmTasks: ''' Performs high level tasks for the PR2 arms. This class does high level tasks for the arms alone. **Attributes:** **side_joint_trajectory ([[double]]):** Joint trajectory for moving to the side ''' def __init__(self): self._joint_state_listener = JointStateListener() self._arm_planners = {} self.side_joint_trajectory = {} self._arm_mover = ArmMover() for arm in ['left_arm', 'right_arm']: self._arm_planners[arm] = ArmPlanner(arm) self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm, DEFAULT_SIDE_JOINT_TRAJECTORY[arm]) def move_arm_to_side(self, arm_name): ''' A function that persistently tries to move an arm to the side. Unless the arm is physically stuck in the world, the arm will be at the robot's side at the end of this function. However, as last resort, the robot uses an open-loop movement to a specific joint state so there is no guarantee the arm's path will be collision-free. Specifically, this function tries in order to: 1. Move the arm to the side joint pose using collision-free planning (calls move arm) 2. If the starting state is in collision, it tries twice to move the state out of collision by executing a short open-loop trajectory to a free state. 3. If the starting state is outside joint angles, it tries twice to move the state into joint angles by executing a short open-loop trajectory to a state within joint angle limits. 4. Moves open loop to each of the joint states along the side joint trajectory. By defalut there are three of these points. The first is high, near the center of the robot's body. The second is high and to the robot's side. The last is low and at the robots side. After each of the first two moves, the robot tries to move to the final joint state collision free before executing the next trajectory open-loop. **Args:** **arm_name (string):** The name of the arm ('right_arm' or 'left_arm') to be moved to side. ''' if self._at_side(arm_name): rospy.loginfo(arm_name + ' is already at the side position') return self._arm_mover.move_into_joint_limits(arm_name) joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state( self.side_joint_trajectory[arm_name][-1]) # first try to move directly there using move arm handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0) if handle.reached_goal(): return for t in range(2): checked_side = False for e in handle.get_errors(): rospy.loginfo('When moving arm, error was:' + str(e)) if type(e) != ArmNavError or not e.error_code: if not checked_side and self._at_side(arm_name): return checked_side = True elif e.error_code.val == e.error_code.START_STATE_IN_COLLISION: self._arm_mover.move_out_of_collision(arm_name) handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0) if handle.reached_goal(): return rospy.loginfo('Even after moving out of collision, we are unable to move the arm to the side') break elif e.error_code.val == e.error_code.JOINT_LIMITS_VIOLATED: self._arm_mover.move_into_joint_limits(arm_name) handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0) if handle.reached_goal(): return rospy.loginfo('Moved into joint limits but still unable to move the arm to the side') break # now try to move to each point on our open loop trajectories # when there, see if we can move to side for p in range(len(self.side_joint_trajectory[arm_name])): joint_state.position = self.side_joint_trajectory[arm_name][p] self._arm_mover.move_to_goal(arm_name, joint_state, try_hard=True) if p < len(self.side_joint_trajectory[arm_name]) - 1: # try to go directly to side from here joint_state.position = self.side_joint_trajectory[arm_name][-1] handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0) if handle.reached_goal() or self._at_side(arm_name): return def move_arms_to_side(self): ''' Moves both arms to side by calling move_arm_to_side on each arm. ''' self.move_arm_to_side('right_arm') self.move_arm_to_side('left_arm') def _at_side(self, arm_name): # check if we're already pretty close to the side position # move arm only does this if we're not in collision (which is kind of dumb) joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state( self.side_joint_trajectory[arm_name][-1]) current_joint_state = self._joint_state_listener.get_joint_positions(self._arm_planners[arm_name].joint_names) already_there = True for p in range(len(current_joint_state)): if abs(gt.angular_distance(current_joint_state[p], joint_state.position[p])) > 0.2: already_there = False rospy.loginfo('Joint '+joint_state.name[p]+' is out of position by ' + str(gt.angular_distance(current_joint_state[p], joint_state.position[p]))) break return already_there