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
