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')
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)
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:
#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))
