def sequence3(animation):
    #a distance robot will notice someone.
    rospy.loginfo('Starting sequence 3')
    pr2.tiltHead(-8,1)
    pr2.rotateHead(humanAngle, 1.5)
    #pr2.setTorso(.01, 4)
    rospy.loginfo('End of sequence 3')
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 headKeepAlive(animation, motionPauseSecs):
    rospy.loginfo('Starting Head Keep Alive')
    #pr2.rotateHead(pr2.getSensorReading("head_pan_joint") +5, .5)
    #animation.sleep_while_running(1)
    #pr2.tiltHead(pr2.getSensorReading("head_tilt_joint") -5, .85)
    #animation.sleep_while_running(3)
    pr2.rotateHead(25, 1)
    animation.sleep_while_running(motionPauseSecs)
    pr2.rotateHead(-25, 1)
    animation.sleep_while_running(motionPauseSecs)
    rospy.loginfo('End of Head Keep Alive')

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:
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')
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 main():
    __init__()
       
    #HeadKeepAliveAnim.start()
    #rospy.timer.sleep(45)
    #HeadKeepAliveAnim.stop()
    
    
    # Obtain a queue from which human distances are obtained as
    # they change:
    eventQueue = human.getEventQueue()
    
    # Motions, like the function "longMotionSequence" above are 
    # started using the following statement. Note that you 
    # can currently only have one motion sequence running at
    # the same time. You can stop a motion anytime by calling
    # myMotion.stop(). For pausing a motion, see below:
    
    
    NO_HUMANS = 'NO_HUMANS'
    VISIBLE_HUMANS = 'VISIBLE_HUMANS'
    CLOSE_HUMANS = 'CLOSE_HUMANS'
    
    state = NO_HUMANS
    
    defaulPoseAnim.start()
    while not defaulPoseAnim.is_done():
        time.sleep(0.01)
    defaulPoseAnim.stop()
    
    #headKeepAlive(None)
    # Two seconds between the two head motions:
    # This is an example for a parallel function
    # taking an argument:
    
    #headKeepAliveAnim.start(5)
    
    while (True):
        # Wait for a new position report from the human:
        
        newHumanLocation = eventQueue.get()
        humanDistance = pr2.getPlanarDistance(newHumanLocation)
        humanAngle = pr2.getAngle(newHumanLocation)
        
        pr2.displayInfo('Robot state:' + state)
        
        ## UPDATE THE STATE
        
        if state == NO_HUMANS:
            ## In NO_HUMANS 
            pr2.lookAtPoint(newHumanLocation, 1.0, wait=False)
    
            if humanDistance < 4:
                state = VISIBLE_HUMANS
                print('Current state ' + state)
                ## NO_HUMANS --> VISIBLE_HUMANS
                animation4.stop()
                animation3.start()
        
        elif state == VISIBLE_HUMANS:
            ## In CLOSE_HUMANS
            pr2.lookAtPoint(newHumanLocation, 1.0, wait=False)
            
            if humanDistance < 2:
                state = CLOSE_HUMANS
                print('Current state ' + state)
                ## VISIBLE_HUMANS --> CLOSE_HUMANS
                animation3.stop()
                animation2.start()
                
            elif humanDistance > 4.5:
                state = NO_HUMANS
                print('Current state ' + state)
                ## VISIBLE_HUMANS --> NO_HUMANS
                animation3.stop()
                animation4.start()
        
        elif state == CLOSE_HUMANS:
            ## In CLOSE_HUMANS 
            
            
            if humanDistance > 2.5:
                state = VISIBLE_HUMANS
                print('Current state ' + state)
                
                ## CLOSE_HUMANS --> VISIBLE_HUMANS
                animation2.stop()
                animation3.start()
        else:
            print 'ERROR: Unknown state.'
def sequence4(animation):
    rospy.loginfo('Starting sequence 4')
    pr2.setTorso(.02, 4)
    animation.sleep_while_running(3)
    rospy.loginfo('End of sequence 4')
Esempio n. 9
0

#pr2.moveBase(rotation=10)
#pr2.moveBase(rotation=0)        
#pr2.moveBase(rotation=90, duration=0.5)
#pr2.moveBase(rotation=-90, duration=0.5)
#pr2.moveBase(rotation=359)
#pr2.moveBase(rotation=135)
#pr2.moveBase(rotation=-135)

#
#pr2.moveBase(place=(1.0,0.0,0.0))    
#pr2.moveBase(place=(-1.0,0.0,0.0))    
#pr2.moveBase(place=(0.0,1.0,0.0))    
#pr2.moveBase(place=(0.0,-1.0,0.0))    

pr2.moveBase(place=(1.0,-3.0,0.0), rotation=135)    
#pr2.moveBase(place=(1.0,-3.0,0.0))
#pr2.moveBase(rotation=135)    



#from tf.transformations import euler_from_quaternion
#from geometry_msgs.msg import Quaternion
#
#for deg in range(360):
#    rad = pr2.degree2rad(deg)
#    quat = (0.0,0.0,1.0,rad)
#    (roll,pitch,yaw) = euler_from_quaternion(quat)
#    print("Yaw " + str(deg) + " deg (" + str(rad) + "rad): " + str(yaw))