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 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')
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)
if aboutEq('head_tilt_joint', 30):
pr2.lookAt(-100, -20 , 5, wait=False)
else:
pr2.lookAt(+100, +30 , 5, wait=False)
if aboutEq('l_shoulder_pan_joint', 90):
pr2.moveArmJoint('l_shoulder_pan_joint', 0, duration=2.0)