nao_driver: scan_pose_bending.py Source File

Go to the documentation of this file.
00001 import rospy
00002 
00003 
00004 def scanPoseBending(motionProxy,head_pitch,head_yaw, t):
00005     #print "This is scanPoseBending" 
00006     names = list()
00007     times = list()
00008     keys = list()
00009 
00010     #t=2.0000
00011 
00012     names.append("HeadYaw")
00013     times.append([ t])
00014     keys.append([ [ head_yaw, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00015 
00016     names.append("HeadPitch")
00017     times.append([ t])
00018     keys.append([ [ head_pitch, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00019 
00020     names.append("LShoulderPitch")
00021     times.append([ t])
00022     keys.append([ [ 2.00489, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00023 
00024     names.append("LShoulderRoll")
00025     times.append([ t])
00026     keys.append([ [ 0.35585, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00027 
00028     names.append("LElbowYaw")
00029     times.append([ t])
00030     keys.append([ [ -1.42666, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00031 
00032     names.append("LElbowRoll")
00033     times.append([ t])
00034     keys.append([ [ -0.24540, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00035 
00036     names.append("LWristYaw")
00037     times.append([ t])
00038     keys.append([ [ 0.00609, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00039 
00040     #names.append("LHand")
00041     #times.append([ t])
00042     #keys.append([ [ 0.00840, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00043 
00044     names.append("RShoulderPitch")
00045     times.append([ t])
00046     keys.append([ [ 2.02645, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00047 
00048     names.append("RShoulderRoll")
00049     times.append([ t])
00050     keys.append([ [ -0.34673, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00051 
00052     names.append("RElbowYaw")
00053     times.append([ t])
00054     keys.append([ [ 1.42351, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00055 
00056     names.append("RElbowRoll")
00057     times.append([ t])
00058     keys.append([ [ 0.24702, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00059 
00060     names.append("RWristYaw")
00061     times.append([ t])
00062     keys.append([ [ -0.00464, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00063 
00064     #names.append("RHand")
00065     #times.append([ t])
00066     #keys.append([ [ 0.01469, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00067 
00068     #names.append("LHipYawPitch")
00069     #times.append([ t])
00070     #keys.append([ [ 0.0, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00071 
00072     #names.append("LHipRoll")
00073     #times.append([ t])
00074     #keys.append([ [ -0.01683, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00075 
00076     #hp = -1.32533
00077     #kp = 1.13205
00078     
00079     hp = -1.32533*0.85
00080     kp = 1.13205*0.85
00081     names.append("LHipPitch")
00082     times.append([ t])
00083     keys.append([ [ hp, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00084 
00085     names.append("LKneePitch")
00086     times.append([ t])
00087     keys.append([ [ kp, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00088 
00089     #names.append("LAnklePitch")
00090     #times.append([ t])
00091     #keys.append([ [ -0.30531, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00092 
00093     #names.append("LAnkleRoll")
00094     #times.append([ t])
00095     #keys.append([ [ 0.01078, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00096 
00097     #names.append("RHipRoll")
00098     #times.append([ t])
00099     #keys.append([ [ 0.00004, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00100 
00101     names.append("RHipPitch")
00102     times.append([ t])
00103     keys.append([ [ hp, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00104 
00105     names.append("RKneePitch")
00106     times.append([ t])
00107     keys.append([ [ kp, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00108 
00109     #names.append("RAnklePitch")
00110     #times.append([ t])
00111     #keys.append([ [ -0.25460, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00112 
00113     #names.append("RAnkleRoll")
00114     #times.append([ t])
00115     #keys.append([ [ -0.01837, [ 2, -1.00000, 0.00000], [ 2, 0.00000, 0.00000]]])
00116 
00117 
00118     check = True
00119     if check:
00120         numJoints = len(motionProxy.getJointNames('Body'))
00121         if (numJoints == 26):
00122             names2 = names
00123             times2 = times
00124             keys2 = keys
00125         elif (numJoints == 22):  # no hands (e.g. simulator)
00126             names2 = names[0:6] + names[8:24]
00127             times2 = times[0:6] + times[8:24]
00128             keys2  = keys[0:6] + keys[8:24]
00129         else:
00130             rospy.logerr("Unkown number of joints: %d", numJoints)
00131             return 
00132     else:
00133         names2 = names
00134         times2 = times
00135         keys2 = keys
00136     try:
00137         #motion = ALProxy("ALMotion")
00138         moveId = motionProxy.post.angleInterpolationBezier(names2, times2, keys2);
00139     except BaseException, err:
00140         print "An error has been caught"
00141         print err
00142         pass
00143