def defaultPose(animation):
    rospy.loginfo('Starting Default Pose')
    pr2.closeGripper(LEFT)
    rospy.loginfo('close 1')
    pr2.closeGripper(RIGHT)
    rospy.loginfo('close 2')
    pr2.moveBase(rotation=0, duration=0.5)    
    rospy.loginfo('moved base')
    pr2.tiltHead(20,2, wait=False)
    rospy.loginfo('tilted')
    pr2.rotateHead(-25, 2, wait=False)
    joints = ['l_shoulder_pan_joint','l_elbow_flex_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint','l_wrist_flex_joint','l_upper_arm_roll_joint', 'l_wrist_roll_joint']
    values = [10,-125,70,90,-70,15, 0]
    pr2.moveArmJoint(joints,values, duration=2.0, wait=False)
    joints = ['r_shoulder_pan_joint','r_elbow_flex_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint','r_wrist_flex_joint','r_upper_arm_roll_joint', 'r_wrist_roll_joint']
    values = [-10,-125,70,-90,-70,-15, 0]
    pr2.moveArmJoint(joints,values, duration=2.0)
    pr2.setTorso(.02, 2.0)
    animation.sleep_while_running(5)
    rospy.loginfo('End of Default Pose')
The many 'if' statements below just ensure that the robot will do something
different each time you run this file.

'''

from robot_scripting import PR2RobotScript as pr2
from robot_scripting import aboutEq

LEFT  = pr2.LEFT
RIGHT = pr2.RIGHT
BOTH  = pr2.BOTH

if aboutEq("l_gripper_joint", 0):
    pr2.openGripper(LEFT)
else:
    pr2.closeGripper(LEFT)   
if aboutEq("r_gripper_joint", 0):
    pr2.openGripper(RIGHT)
else:
    pr2.closeGripper(RIGHT)   

if aboutEq('head_tilt_joint', 30):
    pr2.tiltHead(-30, duration=0.5)
else:
    pr2.tiltHead(30, duration=0.5)
    
if aboutEq('head_pan_joint', 60):
    pr2.panHead(-60)
else:
    pr2.panHead(60)