def init_arm_pose(self, force_ready=False, which_arm='l'):
'''
Move the arm to the initial pose to be out of the way for viewing the
tabletop
'''
if which_arm == 'l':
push_arm = self.left_arm_move
robot_arm = self.robot.left
robot_gripper = self.robot.left_gripper
ready_joints = LEFT_ARM_READY_JOINTS
setup_joints = LEFT_ARM_SETUP_JOINTS
else:
push_arm = self.right_arm_move
robot_arm = self.robot.right
robot_gripper = self.robot.right_gripper
ready_joints = RIGHT_ARM_READY_JOINTS
setup_joints = RIGHT_ARM_SETUP_JOINTS
ready_diff = np.linalg.norm(
pr2.diff_arm_pose(robot_arm.pose(), ready_joints))
push_arm.set_movement_mode_ik()
rospy.loginfo('Moving %s_arm to setup pose' % which_arm)
robot_arm.set_pose(setup_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to setup pose' % which_arm)
rospy.loginfo('Closing %s_gripper' % which_arm)
res = robot_gripper.close(block=True)
rospy.loginfo('Closed %s_gripper' % which_arm)
push_arm.pressure_listener.rezero()
def init_arm_pose(self, force_ready=False, which_arm='l'):
'''
Move the arm to the initial pose to be out of the way for viewing the
tabletop
'''
if which_arm == 'l':
push_arm = self.left_arm_move
robot_arm = self.robot.left
robot_gripper = self.robot.left_gripper
ready_joints = LEFT_ARM_READY_JOINTS
setup_joints = LEFT_ARM_SETUP_JOINTS
else:
push_arm = self.right_arm_move
robot_arm = self.robot.right
robot_gripper = self.robot.right_gripper
ready_joints = RIGHT_ARM_READY_JOINTS
setup_joints = RIGHT_ARM_SETUP_JOINTS
ready_diff = np.linalg.norm(pr2.diff_arm_pose(robot_arm.pose(),
ready_joints))
push_arm.set_movement_mode_ik()
rospy.loginfo('Moving %s_arm to setup pose' % which_arm)
robot_arm.set_pose(setup_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to setup pose' % which_arm)
rospy.loginfo('Closing %s_gripper' % which_arm)
res = robot_gripper.close(block=True)
rospy.loginfo('Closed %s_gripper' % which_arm)
push_arm.pressure_listener.rezero()
def reset_arm_pose(self, force_ready=False, which_arm='l'):
'''
Move the arm to the initial pose to be out of the way for viewing the
tabletop
'''
if which_arm == 'l':
push_arm = self.left_arm_move
robot_arm = self.robot.left
robot_gripper = self.robot.left_gripper
ready_joints = LEFT_ARM_READY_JOINTS
setup_joints = LEFT_ARM_SETUP_JOINTS
else:
push_arm = self.right_arm_move
robot_arm = self.robot.right
robot_gripper = self.robot.right_gripper
ready_joints = RIGHT_ARM_READY_JOINTS
setup_joints = RIGHT_ARM_SETUP_JOINTS
push_arm.pressure_listener.rezero()
ready_diff = np.linalg.norm(pr2.diff_arm_pose(robot_arm.pose(),
ready_joints))
push_arm.set_movement_mode_ik()
# Choose to move to ready first, if it is closer, then move to init
moved_ready = False
if force_ready or ready_diff > READY_POSE_MOVE_THRESH or force_ready:
rospy.loginfo('Moving %s_arm to ready pose' % which_arm)
robot_arm.set_pose(ready_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to ready pose' % which_arm)
moved_ready = True
else:
rospy.loginfo('Arm in ready pose')
rospy.loginfo('Moving %s_arm to setup pose' % which_arm)
robot_arm.set_pose(setup_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to setup pose' % which_arm)
def reset_arm_pose(self, force_ready=False, which_arm='l'):
'''
Move the arm to the initial pose to be out of the way for viewing the
tabletop
'''
if which_arm == 'l':
push_arm = self.left_arm_move
robot_arm = self.robot.left
robot_gripper = self.robot.left_gripper
ready_joints = LEFT_ARM_READY_JOINTS
setup_joints = LEFT_ARM_SETUP_JOINTS
else:
push_arm = self.right_arm_move
robot_arm = self.robot.right
robot_gripper = self.robot.right_gripper
ready_joints = RIGHT_ARM_READY_JOINTS
setup_joints = RIGHT_ARM_SETUP_JOINTS
push_arm.pressure_listener.rezero()
ready_diff = np.linalg.norm(
pr2.diff_arm_pose(robot_arm.pose(), ready_joints))
push_arm.set_movement_mode_ik()
# Choose to move to ready first, if it is closer, then move to init
moved_ready = False
if force_ready or ready_diff > READY_POSE_MOVE_THRESH or force_ready:
rospy.loginfo('Moving %s_arm to ready pose' % which_arm)
robot_arm.set_pose(ready_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to ready pose' % which_arm)
moved_ready = True
else:
rospy.loginfo('Arm in ready pose')
rospy.loginfo('Moving %s_arm to setup pose' % which_arm)
robot_arm.set_pose(setup_joints, nsecs=2.0, block=True)
rospy.loginfo('Moved %s_arm to setup pose' % which_arm)
