samplerobot_sequence_player.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 try:
00004     from hrpsys.hrpsys_config import *
00005     import OpenHRP
00006 except:
00007     print "import without hrpsys"
00008     import rtm
00009     from rtm import *
00010     from OpenHRP import *
00011     import waitInput
00012     from waitInput import *
00013     import socket
00014     import time
00015 
00016 from distutils.version import StrictVersion
00017 
00018 def init ():
00019     global hcf, hrpsys_version
00020     hcf = HrpsysConfigurator()
00021     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00022     hcf.connectLoggerPort(hcf.sh, 'optionalDataOut') # Just for checking
00023     hcf.connectLoggerPort(hcf.el, 'servoStateOut') # Just for checking
00024     global reset_pose_doc, move_base_pose_doc, doc
00025     # doc for patterns.
00026     #  torque and wrenches are non-realistic values, just for testing.
00027     dof = 29
00028     reset_pose_doc = {'pos':[-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0],
00029                       'vel':[0]*dof,
00030                       'zmp':[-0.00081, 1.712907e-05, -0.66815],
00031 #                      'gsens':[0,0,0],
00032                       'waist':[0.000234, 0.000146, 0.66815, -0.000245, -0.000862, 0.000195],
00033                       'waist_acc':[0]*3,
00034                       'torque':[0]*dof, # non realistic value
00035                       'wrenches':[0]*24, # non realistic value
00036                       'optionaldata':[1,1,0,0,1,1,1,1]
00037                       }
00038     move_base_pose_doc = {'pos':[8.251963e-05,-0.980029,-0.000384,1.02994,-0.398115,-0.000111,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,8.252625e-05,-0.980033,-0.000384,1.02986,-0.398027,-0.000111,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0],
00039                           'vel':[0]*dof,
00040                           'zmp':[0.302518, 0.000153, -0.562325],
00041 #                          'gsens':[0,0,0],
00042                           'waist':[-0.092492, -6.260780e-05, 0.6318, -0.000205, 0.348204, 0.000268],
00043                           'waist_acc':[0]*3,
00044                           'torque':range(dof), # non realistic value
00045                           'wrenches':[1]*6+[-2]*6+[3]*6+[-4]*6, # non realistic value
00046                           'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value
00047                           }
00048     hrpsys_version = hcf.seq.ref.get_component_profile().version
00049     print("hrpsys_version = %s"%hrpsys_version)
00050     hcf.seq_svc.removeJointGroup('larm')
00051     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00052     hcf.seq_svc.waitInterpolation();
00053 
00054 
00055 def dumpLoadPatternTestFile (basename, var_doc, tm):
00056     for key in var_doc.keys():
00057         f=open(basename+"."+key, "w")
00058         dumpstr=" ".join(map(str, [tm]+var_doc[key]))
00059         f.writelines(dumpstr)
00060         f.close()
00061 
00062 def checkArrayEquality (arr1, arr2, eps=1e-7):
00063     return all(map(lambda x,y : abs(x-y)<eps, arr1, arr2))
00064 
00065 def checkArrayBetween (arr1, arr2, arr3, eps=1e-7):
00066     return all(map(lambda x,y,z : (z-y)*(x-y) <= eps, arr1, arr2, arr3))
00067 
00068 def clearLogForCheckParameter(log_length=1):
00069     hcf.setMaxLogLength(log_length)
00070     hcf.clearLog()
00071 
00072 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-sequence-player-check-param"):
00073     clearLogForCheckParameter(log_length=1)
00074     time.sleep(0.1);hcf.saveLog(log_fname)
00075 
00076 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-sequence-player-check-param", save_log=True, rtc_name="sh"):
00077     if save_log:
00078         saveLogForCheckParameter(log_fname)
00079     return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
00080 
00081 def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-check-param"):
00082     hcf.saveLog(log_fname)
00083     ret = True
00084     fp = open(log_fname+'.el_servoStateOut', "r")
00085     for l in fp:
00086         for s in map(int, l.split()[1:-1]):
00087             if s != 7:
00088                 print l
00089                 ret = False
00090     return ret
00091 
00092 def checkJointAngles (var_doc, eps=1e-7):
00093     if isinstance(var_doc, list):
00094         p = var_doc
00095     else:
00096         p = var_doc['pos']
00097     ret = checkArrayEquality(hcf.sh_svc.getCommand().jointRefs, p, eps)
00098     print "  pos => ", ret
00099     assert(ret is True)
00100 
00101 def checkJointAnglesBetween(from_doc, to_doc, eps=1e-7):
00102     p0 =  from_doc if isinstance(from_doc, list) else from_doc['pos']
00103     p1 =    to_doc if isinstance(  to_doc, list) else   to_doc['pos']
00104     ret = checkArrayBetween(p0, hcf.sh_svc.getCommand().jointRefs, p1, eps)
00105     print "  pos => ", ret
00106     assert(ret is True)
00107 
00108 def checkZmp(var_doc):
00109     zmp=hcf.sh_svc.getCommand().zmp
00110     ret = checkArrayEquality([zmp[0], zmp[1], zmp[2]], var_doc['zmp'])
00111     print "  zmp => ", ret
00112     assert(ret is True)
00113 
00114 def checkWaist(var_doc, save_log=True):
00115     bpos=checkParameterFromLog("basePosOut", save_log=save_log)
00116     brpy=checkParameterFromLog("baseRpyOut", save_log=False)
00117     ret = checkArrayEquality([bpos[0], bpos[1], bpos[2], brpy[0], brpy[1], brpy[2]], var_doc['waist'], eps=1e-5)
00118     print "  waist => ", ret
00119     assert(ret is True)
00120 
00121 def checkTorque (var_doc, save_log=True):
00122     ret = checkArrayEquality(checkParameterFromLog("tqOut", save_log=save_log), var_doc['torque'], eps=1e-5)
00123     print "  torque => ", ret
00124     assert(ret is True)
00125 
00126 def checkWrenches (var_doc, save_log=True):
00127     if save_log:
00128         saveLogForCheckParameter()
00129     ret = checkArrayEquality(reduce(lambda x,y:x+y, map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])), var_doc['wrenches'], eps=1e-5)
00130     print "  wrenches => ", ret
00131     assert(ret is True)
00132 
00133 def checkOptionalData (var_doc, save_log=True):
00134     ret = checkArrayEquality(checkParameterFromLog("optionalDataOut", save_log=save_log), var_doc['optionaldata'], eps=1e-5)
00135     print "  optionaldata => ", ret
00136     assert(ret is True)
00137 
00138 def checkRobotState (var_doc):
00139     checkJointAngles(var_doc)
00140     checkZmp(var_doc)
00141     checkWaist(var_doc)
00142     checkTorque(var_doc, save_log=False)
00143     if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'):
00144         checkWrenches(var_doc, save_log=False)
00145         checkOptionalData(var_doc, save_log=False)
00146 
00147 # demo functions
00148 def demoSetJointAngles():
00149     print >> sys.stderr, "1. setJointAngles"
00150     hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 1.0);
00151     hcf.seq_svc.waitInterpolation();
00152     checkJointAngles(move_base_pose_doc)
00153     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00154     hcf.seq_svc.waitInterpolation();
00155     checkJointAngles(reset_pose_doc)
00156     # check override
00157     print >> sys.stderr, "   check override"
00158     hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0);
00159     time.sleep(2.5)
00160     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00161     hcf.seq_svc.waitInterpolation();
00162     checkJointAngles(reset_pose_doc)
00163     # check clear
00164     if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'):
00165         return
00166     print >> sys.stderr, "   check clear"
00167     hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0);
00168     time.sleep(2.5)
00169     hcf.seq_svc.clearJointAngles()
00170     checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
00171 
00172 def demoSetJointAnglesSequence():
00173     print >> sys.stderr, "2. setJointAnglesSequence"
00174     hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,1.0]);
00175     hcf.seq_svc.waitInterpolation();
00176     checkJointAngles(move_base_pose_doc)
00177     hcf.seq_svc.setJointAnglesSequence([reset_pose_doc['pos']], [1.0]);
00178     hcf.seq_svc.waitInterpolation();
00179     checkJointAngles(reset_pose_doc)
00180     # check override
00181     print >> sys.stderr, "   check override"
00182     hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0])
00183     time.sleep(3.5)
00184     hcf.seq_svc.setJointAnglesSequence([reset_pose_doc['pos'],move_base_pose_doc['pos'],reset_pose_doc['pos']], [1.0,1.0,1.0]);
00185     hcf.seq_svc.waitInterpolation();
00186     checkJointAngles(reset_pose_doc)
00187     # check clear
00188     print >> sys.stderr, "   check clear"
00189     hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0])
00190     time.sleep(3.5)
00191     hcf.seq_svc.clearJointAngles()
00192     checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
00193 
00194 def demoSetJointAngle():
00195     print >> sys.stderr, "3. setJointAngle"
00196     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00197     hcf.seq_svc.waitInterpolation();
00198     hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 1.0);
00199     hcf.seq_svc.waitInterpolation();
00200     p = list(reset_pose_doc['pos']) # copy object
00201     p[27] = 10*3.14159/180.0
00202     checkJointAngles(p)
00203     hcf.seq_svc.setJointAngle("WAIST_R", 0*3.14159/180.0, 1.0);
00204     hcf.seq_svc.waitInterpolation();
00205     checkJointAngles(reset_pose_doc)
00206     # # check override
00207     # print "   check override"
00208     # hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00209     # hcf.seq_svc.waitInterpolation();
00210     # hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 5.0);
00211     # time.sleep(2.5)
00212     # hcf.seq_svc.setJointAngle("WAIST_R", 0*3.14159/180.0, 5.0);
00213     # hcf.seq_svc.waitInterpolation();
00214     # checkJointAngles(reset_pose_doc)
00215     # # check clear
00216     # print "   check clear"
00217     # hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 5.0);
00218     # time.sleep(2.5)
00219     # hcf.seq_svc.clearJointAngles()
00220     # checkJointAnglesBetween(reset_pose_doc,p)
00221 
00222 def demoLoadPattern():
00223     print >> sys.stderr, "4. loadPattern"
00224     # dump pattern doc as loadPattern file
00225     dumpLoadPatternTestFile("/tmp/test-samplerobot-move-base-pose", move_base_pose_doc, 2.0);
00226     dumpLoadPatternTestFile("/tmp/test-samplerobot-reset-pose", reset_pose_doc, 2.0);
00227     # execute loadPattern and check final values
00228     hcf.seq_svc.loadPattern("/tmp/test-samplerobot-move-base-pose", 1.0)
00229     hcf.seq_svc.waitInterpolation()
00230     checkRobotState(move_base_pose_doc)
00231     hcf.seq_svc.loadPattern("/tmp/test-samplerobot-reset-pose", 1.0)
00232     hcf.seq_svc.waitInterpolation()
00233     checkRobotState(reset_pose_doc)
00234 
00235 def demoSetZmp ():
00236     print >> sys.stderr, "5. setZmp"
00237     hcf.seq_svc.setZmp(move_base_pose_doc['zmp'], 1.0);
00238     hcf.seq_svc.waitInterpolation();
00239     checkZmp(move_base_pose_doc)
00240     hcf.seq_svc.setZmp(reset_pose_doc['zmp'], 1.0);
00241     hcf.seq_svc.waitInterpolation();
00242     checkZmp(reset_pose_doc)
00243 
00244 def demoSetBasePosRpy ():
00245     print >> sys.stderr, "6. setBasePos and setBaseRpy"
00246     hcf.seq_svc.setBasePos(move_base_pose_doc['waist'][0:3], 1.0);
00247     hcf.seq_svc.setBaseRpy(move_base_pose_doc['waist'][3:6], 1.0);
00248     hcf.seq_svc.waitInterpolation();
00249     checkWaist(move_base_pose_doc)
00250     hcf.seq_svc.setBasePos(reset_pose_doc['waist'][0:3], 1.0);
00251     hcf.seq_svc.setBaseRpy(reset_pose_doc['waist'][3:6], 1.0);
00252     hcf.seq_svc.waitInterpolation();
00253     checkWaist(reset_pose_doc)
00254 
00255 def demoSetWrenches ():
00256     print >> sys.stderr, "7. setWrenches"
00257     hcf.seq_svc.setWrenches(move_base_pose_doc['wrenches'], 1.0);
00258     hcf.seq_svc.waitInterpolation();
00259     checkWrenches(move_base_pose_doc)
00260     hcf.seq_svc.setWrenches(reset_pose_doc['wrenches'], 1.0);
00261     hcf.seq_svc.waitInterpolation();
00262     checkWrenches(reset_pose_doc)
00263 
00264 def demoSetJointAnglesOfGroup():
00265     print >> sys.stderr, "8. setJointAnglesOfGroup"
00266     hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
00267     larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
00268     larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0]
00269     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00270     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 1.0);
00271     hcf.seq_svc.waitInterpolationOfGroup('larm');
00272     p0 = list(reset_pose_doc['pos']) # copy
00273     for i in range(len(larm_pos0)):
00274         p0[i+19] = larm_pos0[i]
00275     checkJointAngles(p0)
00276     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
00277     hcf.seq_svc.waitInterpolationOfGroup('larm');
00278     p1 = list(reset_pose_doc['pos']) # copy
00279     for i in range(len(larm_pos1)):
00280         p1[i+19] = larm_pos1[i]
00281     checkJointAngles(p1)
00282     # check override
00283     print >> sys.stderr, "   check override"
00284     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00285     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
00286     time.sleep(2.5)
00287     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
00288     hcf.seq_svc.waitInterpolationOfGroup('larm');
00289     checkJointAngles(p1)
00290     # check clear
00291     if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'):
00292         return
00293     print >> sys.stderr, "   check clear (clearJointAnglesOfGroup)"
00294     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
00295     time.sleep(2.5)
00296     hcf.seq_svc.clearJointAnglesOfGroup('larm')
00297     checkJointAnglesBetween(p1, p0)
00298 
00299     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0);
00300     hcf.seq_svc.waitInterpolationOfGroup('larm')
00301     checkJointAngles(p1)
00302 
00303     print >> sys.stderr, "   check clear clearOfGroup"
00304     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
00305     time.sleep(2.5)
00306     hcf.seq_svc.clearOfGroup('larm', 0.0)
00307     checkJointAnglesBetween(p1, p0)
00308 
00309     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0);
00310     hcf.seq_svc.waitInterpolationOfGroup('larm')
00311     checkJointAngles(p1)
00312 
00313 def demoSetJointAnglesSequenceOfGroup():
00314     print >> sys.stderr, "9. setJointAnglesOfGroup"
00315     hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
00316     larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
00317     larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0]
00318     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00319     hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 1.0]);
00320     hcf.seq_svc.waitInterpolationOfGroup('larm');
00321     p0 = list(reset_pose_doc['pos']) # copy
00322     for i in range(len(larm_pos0)):
00323         p0[i+19] = larm_pos0[i]
00324     checkJointAngles(p0)
00325     hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos1, larm_pos0, larm_pos1], [1.0, 1.0, 1.0]);
00326     hcf.seq_svc.waitInterpolationOfGroup('larm');
00327     p1 = list(reset_pose_doc['pos']) # copy
00328     for i in range(len(larm_pos1)):
00329         p1[i+19] = larm_pos1[i]
00330     checkJointAngles(p1)
00331     # check override
00332     print >> sys.stderr, "   check override"
00333     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00334     hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]);
00335     time.sleep(3.5)
00336     hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos1, larm_pos0, larm_pos1], [1.0, 1.0, 1.0]);
00337     hcf.seq_svc.waitInterpolationOfGroup('larm');
00338     checkJointAngles(p1)
00339     # check clear
00340     print >> sys.stderr, "   check clear"
00341     hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]);
00342     time.sleep(3.5)
00343     hcf.seq_svc.clearJointAnglesOfGroup('larm')
00344     checkJointAnglesBetween(p1, p0)
00345     hcf.seq_svc.removeJointGroup('larm')
00346 
00347 def demoSetJointAnglesSequenceFull():
00348     print >> sys.stderr, "10. setJointAnglesSequenceFull"
00349     hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
00350                                            [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
00351                                            [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
00352                                            [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
00353                                            [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
00354                                            [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
00355                                            [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
00356                                            [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
00357                                            [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
00358                                            [1.0,1.0,1.0]);
00359     hcf.waitInterpolation();
00360     checkRobotState(move_base_pose_doc)
00361     hcf.seq_svc.setJointAnglesSequenceFull([reset_pose_doc['pos']],
00362                                            [reset_pose_doc['vel']],
00363                                            [reset_pose_doc['torque']],
00364                                            [reset_pose_doc['waist'][0:3]],
00365                                            [reset_pose_doc['waist'][3:6]],
00366                                            [reset_pose_doc['waist_acc']],
00367                                            [reset_pose_doc['zmp']],
00368                                            [reset_pose_doc['wrenches']],
00369                                            [reset_pose_doc['optionaldata']],
00370                                            [1.0])
00371     hcf.waitInterpolation();
00372     checkRobotState(reset_pose_doc)
00373     # check override
00374     print >> sys.stderr, "   check override"
00375     hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
00376                                            [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
00377                                            [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
00378                                            [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
00379                                            [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
00380                                            [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
00381                                            [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
00382                                            [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
00383                                            [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
00384                                            [1.0,1.0,5.0]);
00385     time.sleep(3.5)
00386     hcf.seq_svc.setJointAnglesSequenceFull([reset_pose_doc['pos'],move_base_pose_doc['pos'],reset_pose_doc['pos']],
00387                                            [reset_pose_doc['vel'],move_base_pose_doc['vel'],reset_pose_doc['vel']],
00388                                            [reset_pose_doc['torque'],move_base_pose_doc['torque'],reset_pose_doc['torque']],
00389                                            [reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3]],
00390                                            [reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6]],
00391                                            [reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc']],
00392                                            [reset_pose_doc['zmp'],move_base_pose_doc['zmp'],reset_pose_doc['zmp']],
00393                                            [reset_pose_doc['wrenches'],move_base_pose_doc['wrenches'],reset_pose_doc['wrenches']],
00394                                            [reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata']],
00395                                            [1.0,1.0,1.0]);
00396     hcf.waitInterpolation()
00397     checkRobotState(reset_pose_doc)
00398     # check clear
00399     print >> sys.stderr, "   check clear"
00400     hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
00401                                            [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
00402                                            [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
00403                                            [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
00404                                            [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
00405                                            [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
00406                                            [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
00407                                            [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
00408                                            [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
00409                                            [1.0,1.0,5.0]);
00410     time.sleep(3.5)
00411     hcf.seq_svc.clearJointAngles()
00412     checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
00413 
00414 def demoSetTargetPose():
00415     # reset wait position
00416     hcf.seq_svc.setBasePos([0.000000, 0.000000, 0.723500], 1.0);
00417     hcf.seq_svc.setBaseRpy([0.000000, 0.000000, 0.000000], 1.0);
00418     print >> sys.stderr, "11. setTargetPose"
00419     hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
00420     larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
00421     larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.785395, -0.636277, 0.0, 0.0]
00422     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00423     hcf.seq_svc.waitInterpolation();
00424     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 1.0);
00425     hcf.seq_svc.waitInterpolationOfGroup('larm');
00426     pos0 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
00427     rpy0 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
00428     pos0_world = hcf.getCurrentPosition('LARM_WRIST_R')
00429     rpy0_world = hcf.getCurrentRPY('LARM_WRIST_R')
00430     p0 = list(reset_pose_doc['pos']) # copy
00431     for i in range(len(larm_pos0)):
00432         p0[i+19] = larm_pos0[i]
00433     checkJointAngles(p0)
00434 
00435     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
00436     hcf.seq_svc.waitInterpolationOfGroup('larm');
00437     pos1 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
00438     rpy1 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
00439     pos1_world = hcf.getCurrentPosition('LARM_WRIST_R')
00440     rpy1_world = hcf.getCurrentRPY('LARM_WRIST_R')
00441     p1 = list(reset_pose_doc['pos']) # copy
00442     for i in range(len(larm_pos1)):
00443         p1[i+19] = larm_pos1[i]
00444     checkJointAngles(p1)
00445 
00446     print >> sys.stderr, "   check setTargetPose"
00447     hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0)
00448     hcf.seq_svc.waitInterpolationOfGroup('larm');
00449     print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)
00450     checkJointAngles(p0, 0.01)
00451 
00452     clearLogForCheckParameter(5000)
00453     hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0)
00454     hcf.seq_svc.waitInterpolationOfGroup('larm');
00455     print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
00456     checkJointAngles(p1, 0.01)
00457     assert(checkServoStateFromLog() is True)
00458 
00459     hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
00460     hcf.seq_svc.waitInterpolationOfGroup('larm');
00461 
00462     print >> sys.stderr, "   check setTargetPose without giving reference frame"
00463     hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0)
00464     hcf.seq_svc.waitInterpolationOfGroup('larm');
00465     print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)
00466     checkJointAngles(p0, 0.01)
00467 
00468     clearLogForCheckParameter(5000)
00469     hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0)
00470     hcf.seq_svc.waitInterpolationOfGroup('larm');
00471     print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
00472     checkJointAngles(p1, 0.01)
00473     assert(checkServoStateFromLog() is True)
00474 
00475     print >> sys.stderr, "   check clear clearOfGroup"
00476     hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0)
00477     time.sleep(0.5)
00478     hcf.seq_svc.clearOfGroup('larm', 0.0)
00479     checkJointAnglesBetween(p0, p1, 0.01)
00480 
00481     clearLogForCheckParameter(5000)
00482     hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0)
00483     hcf.seq_svc.waitInterpolationOfGroup('larm')
00484     print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
00485     checkJointAngles(p1, 0.1)
00486     assert(checkServoStateFromLog() is True)
00487     hcf.seq_svc.removeJointGroup('larm')
00488     hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
00489     hcf.seq_svc.waitInterpolation();
00490 
00491 def demo():
00492     init()
00493     demoSetJointAngles()
00494     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00495         demoSetJointAnglesSequence()
00496     demoSetJointAngle()
00497     demoLoadPattern()
00498     demoSetZmp()
00499     demoSetBasePosRpy()
00500     if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'):
00501         demoSetWrenches()
00502     demoSetJointAnglesOfGroup()
00503     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00504         demoSetJointAnglesSequenceOfGroup()
00505         demoSetJointAnglesSequenceFull()
00506     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00507         demoSetTargetPose()
00508 
00509 if __name__ == '__main__':
00510     demo()
00511

hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18