def sequence1(animation):
    
    #This function runs the robot through some motion forever.
    #It doesn't need to. It could just be a long-running motion
    #which does eventually stop.
    
    rospy.loginfo('Starting sequence 1')
    pr2.setTorso(.02, 4)
    animation.sleep_while_running(6)
    rospy.loginfo('End of sequence 1')
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')
def sequence2(animation):
    #Robot's reaction to someone entering his personal space to interact with him.
    rospy.loginfo('Starting sequence 2')

    pr2.rotateHead(humanAngle, 2,wait=False)
    pr2.setTorso(.04 + pr2.getSensorReading("torso_lift_joint"), 1.0,  wait=False)
    #pr2.moveBase(rotation=new_PS_Angle, duration=7.0)
    joints = ['r_forearm_roll_joint', 'r_shoulder_lift_joint','r_wrist_roll_joint', 'r_wrist_flex_joint', 'r_upper_arm_roll_joint','r_shoulder_pan_joint'  ]
    value = [60, 40, 90, 0, 0, 0]
    pr2.moveArmJoint(joints, value, duration=2.0, wait=False)
    joints = ['l_forearm_roll_joint', 'l_shoulder_lift_joint','l_wrist_roll_joint', 'l_wrist_flex_joint', 'l_upper_arm_roll_joint','l_shoulder_pan_joint'  ]
    value = [60, 70, 0, -80, 0, 15]
    pr2.moveArmJoint(joints, value, duration=2.0)
    pr2.moveArmJoint(['r_upper_arm_roll_joint'], [3], duration=1)
    pr2.moveArmJoint(['r_upper_arm_roll_joint'], [-3], duration=1)
    pr2.moveArmJoint(['r_upper_arm_roll_joint'], [3], duration=1)
    pr2.moveArmJoint(['r_upper_arm_roll_joint'], [-3], duration=1.25)
    pr2.moveArmJoint(['r_upper_arm_roll_joint'], [0], duration=1.5)

    animation.sleep_while_running(7)
    pr2.rotateHead(2, .5)
    animation.sleep_while_running(2)
    pr2.tiltHead(-5, .5)
    animation.sleep_while_running(6)
    pr2.rotateHead(0, .5)
    animation.sleep_while_running(14)
    
    pr2.setTorso(-.01 + pr2.getSensorReading("torso_lift_joint"), 4.0)
    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=3.0, wait=False)
    joints = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint','l_upper_arm_roll_joint']
    values = [humanAngle *.2 , 70, 0]
    pr2.moveArmJoint(joints,values, duration=3.0)
    
    rospy.loginfo('End of sequence 2')
if aboutEq('l_shoulder_pan_joint', 90):
    pr2.moveArmJoint('l_shoulder_pan_joint', 0, duration=2.0)
    pr2.moveArmJoint(['l_forearm_roll_joint', 'l_elbow_flex_joint'], [30, 0], duration=2.0)
else:
    pr2.moveArmJoint('l_shoulder_pan_joint', 90, duration=2.0)
    pr2.moveArmJoint(['l_forearm_roll_joint', 'l_elbow_flex_joint'], [-30, -130], duration=2.0)    

pr2.moveBase(place=(0.3,0.0,0.0))
pr2.moveBase(place=(-0.3,0.0,0.0))
pr2.moveBase(rotation=90)
pr2.moveBase(rotation=-90)

torsoState = pr2.getSensorReading('torso_lift_joint')

if aboutEq('torso_lift_joint', 0):
    pr2.setTorso(0.3, wait=True)
    print "Torso done"
else:
    pr2.setTorso(0, duration=5, wait=True)
    print "Torso done"


# Advanced: event simulators that generate events via callbacks:

from event_simulators.event_simulator import EventSimulator
from Queue import Empty
from collections import OrderedDict
import sys

def printWord(word):
    print(word)
def sequence4(animation):
    rospy.loginfo('Starting sequence 4')
    pr2.setTorso(.02, 4)
    animation.sleep_while_running(3)
    rospy.loginfo('End of sequence 4')