samplerobot_stabilizer.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 import math
00017 from subprocess import check_output
00018 from distutils.version import StrictVersion
00019 
00020 def init ():
00021     global hcf, initial_pose, half_sitting_pose, hrpsys_version
00022     hcf = HrpsysConfigurator()
00023     hcf.getRTCList = hcf.getRTCListUnstable
00024     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00025     # set initial pose from sample/controller/SampleController/etc/Sample.pos
00026     initial_pose = [-7.779e-005,  -0.378613,  -0.000209793,  0.832038,  -0.452564,  0.000244781,  0.31129,  -0.159481,  -0.115399,  -0.636277,  0,  0,  0,  -7.77902e-005,  -0.378613,  -0.000209794,  0.832038,  -0.452564,  0.000244781,  0.31129,  0.159481,  0.115399,  -0.636277,  0,  0,  0,  0,  0,  0]
00027     half_sitting_pose = [-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00028     hrpsys_version = hcf.seq.ref.get_component_profile().version
00029     print("hrpsys_version = %s"%hrpsys_version)
00030     if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00031         # on < 315.5.0 this outputs huge error log message
00032         hcf.seq_svc.setJointAngles(initial_pose, 2.0)
00033         hcf.seq_svc.waitInterpolation()
00034         # Remove offset
00035         for sen in ["rfsensor", "lfsensor"]:
00036             ofp=hcf.rmfo_svc.getForceMomentOffsetParam(sen)[1];
00037             ofp.link_offset_mass=1.9;ofp.link_offset_centroid=[0.08, 0, -0.03];
00038             hcf.rmfo_svc.setForceMomentOffsetParam(sen, ofp);
00039 
00040 def calcCOP ():
00041     cop_info=rtm.readDataPort(hcf.st.port("COPInfo")).data
00042     lcopx=cop_info[1]/cop_info[2];lcopy=cop_info[0]/cop_info[2]
00043     rcopx=cop_info[1+3]/cop_info[2+3];rcopy=cop_info[0+3]/cop_info[2+3]
00044     return [[lcopx, lcopx], [rcopx, rcopy], # l cop, r cop
00045             [(cop_info[1]+cop_info[1+3])/(cop_info[2]+cop_info[2+3]),(cop_info[0]+cop_info[0+3])/(cop_info[2]+cop_info[2+3])]] # total ZMP
00046 
00047 def demoGetParameter():
00048     print >> sys.stderr, "1. getParameter"
00049     stp = hcf.st_svc.getParameter()
00050     print >> sys.stderr, "  getParameter() => OK"
00051 
00052 def demoSetParameter():
00053     print >> sys.stderr, "2. setParameter"
00054     stp_org = hcf.st_svc.getParameter()
00055     # for tpcc
00056     stp_org.k_tpcc_p=[0.2, 0.2]
00057     stp_org.k_tpcc_x=[4.0, 4.0]
00058     stp_org.k_brot_p=[0.0, 0.0]
00059     # for eefm
00060     tmp_leg_inside_margin=71.12*1e-3
00061     tmp_leg_outside_margin=71.12*1e-3
00062     tmp_leg_front_margin=182.0*1e-3
00063     tmp_leg_rear_margin=72.0*1e-3
00064     rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
00065                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
00066                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
00067                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
00068     lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
00069                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
00070                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
00071                      OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
00072     rarm_vertices = rleg_vertices
00073     larm_vertices = lleg_vertices
00074     stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
00075     stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin
00076     stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin
00077     stp_org.eefm_leg_front_margin=tmp_leg_front_margin
00078     stp_org.eefm_leg_rear_margin=tmp_leg_rear_margin
00079     stp_org.eefm_k1=[-1.39899,-1.39899]
00080     stp_org.eefm_k2=[-0.386111,-0.386111]
00081     stp_org.eefm_k3=[-0.175068,-0.175068]
00082     stp_org.eefm_rot_damping_gain = [[20*1.6*1.5, 20*1.6*1.5, 1e5]]*4
00083     stp_org.eefm_pos_damping_gain = [[3500*50, 3500*50, 3500*1.0*1.5]]*4
00084     stp_org.eefm_swing_rot_damping_gain = stp_org.eefm_rot_damping_gain[0]
00085     stp_org.eefm_swing_pos_damping_gain = stp_org.eefm_pos_damping_gain[0]
00086     stp_org.eefm_use_swing_damping=True
00087     hcf.st_svc.setParameter(stp_org)
00088     stp = hcf.st_svc.getParameter()
00089     vcheck = stp.k_tpcc_p == stp_org.k_tpcc_p and stp.k_tpcc_x == stp_org.k_tpcc_x and stp.k_brot_p == stp_org.k_brot_p
00090     if vcheck:
00091         print >> sys.stderr, "  setParameter() => OK", vcheck
00092     assert(vcheck)
00093 
00094 def changeContactDecisionThre (thre):
00095     stp = hcf.st_svc.getParameter()
00096     stp.contact_decision_threshold=thre
00097     hcf.st_svc.setParameter(stp)
00098 
00099 def mimicInTheAir ():
00100     changeContactDecisionThre(10000) # [N]
00101 
00102 def mimicOnTheGround ():
00103     changeContactDecisionThre(50) # [N], default
00104 
00105 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-stabilizer-check-param"):
00106     hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
00107 
00108 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-stabilizer-check-param", save_log=True, rtc_name="SampleRobot(Robot)0"):
00109     if save_log:
00110         saveLogForCheckParameter(log_fname)
00111     return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
00112 
00113 def checkActualBaseAttitude(thre=5.0): # degree
00114     '''Check whether the robot falls down based on actual robot base-link attitude.
00115     '''
00116     act_rpy = checkParameterFromLog("WAIST")[3:]
00117     ret = abs(math.degrees(act_rpy[0])) < thre and abs(math.degrees(act_rpy[1])) < thre
00118     print >> sys.stderr, "  ret = ", ret, ", actual base rpy = (", act_rpy, ")"
00119     return ret
00120 
00121 def printActualBase():
00122     '''Print actual base pos and rot
00123     '''
00124     act_base = checkParameterFromLog("WAIST")
00125     print >> sys.stderr, "  actual base pos = ", act_base[0:3], "[m], actual base rpy = ", act_base[3:], "[rad]"
00126 
00127 def changeSTAlgorithm (new_st_alg):
00128     stp = hcf.st_svc.getParameter()
00129     if stp.st_algorithm != new_st_alg:
00130         hcf.stopStabilizer()
00131         stp.st_algorithm = new_st_alg
00132         hcf.st_svc.setParameter(stp)
00133         hcf.startStabilizer ()
00134         # Wait for osscilation being samall
00135         hcf.setJointAngles(hcf.getJointAngles(), 2.0);
00136         hcf.waitInterpolation()
00137 
00138 def demoSTLoadPattern ():
00139     print >> sys.stderr, "3. EEFMQP st + SequencePlayer loadPattern"
00140     if hcf.pdc:
00141         # Set initial pose of samplerobot-gopos000 before starting of ST
00142         hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss
00143                                                [[0]*29], # vels
00144                                                [[0]*29], # torques
00145                                                [[-0.014759, -4.336272e-05, 0.668138]], # poss
00146                                                [[-0.000245, -0.000862, 0.000171]], # rpys
00147                                                [[0]*3], # accs
00148                                                [[0.014052, 0.000203, -0.66798]], # zmps
00149                                                [[0]*6*4], # wrenchs
00150                                                [[1,1,0,0,1,1,1,1]], # optionals
00151                                                [0.5]); # tms
00152         hcf.stopAutoBalancer()
00153         hcf.seq_svc.waitInterpolation()
00154         stp = hcf.st_svc.getParameter()
00155         stp.emergency_check_mode=OpenHRP.StabilizerService.NO_CHECK # Disable checking of emergency error because currently this error checker does not work correctly during walking.
00156         hcf.st_svc.setParameter(stp)
00157         #changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP)
00158         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQP)
00159         hcf.startStabilizer()
00160         # Exec loadPattern
00161         HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip()
00162         hcf.loadPattern(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/data/samplerobot-gopos000', 0.0)
00163         hcf.waitInterpolation()
00164         ret = checkActualBaseAttitude()
00165         if ret:
00166             print >> sys.stderr, "  ST + loadPattern => OK"
00167         assert(ret)
00168     else:
00169         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00170 
00171 def demoStartStopTPCCST ():
00172     print >> sys.stderr, "4. start and stop TPCC st"
00173     if hcf.pdc:
00174         # setup controllers
00175         hcf.startAutoBalancer()
00176         changeSTAlgorithm (OpenHRP.StabilizerService.TPCC)
00177         hcf.startStabilizer ()
00178         #hcf.abc_svc.goPos(0.5, 0.1, 10)
00179         #hcf.abc_svc.waitFootSteps()
00180         ret = checkActualBaseAttitude()
00181         if ret:
00182             print >> sys.stderr, "  Start and Stop Stabilizer => OK"
00183         assert(ret)
00184     else:
00185         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00186 
00187 def demoStartStopEEFMQPST ():
00188     print >> sys.stderr, "5. start and stop EEFMQP st"
00189     if hcf.pdc:
00190         # setup controllers
00191         hcf.startAutoBalancer()
00192         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQP)
00193         hcf.startStabilizer()
00194         hcf.abc_svc.goPos(0.3, 0, 0)
00195         hcf.abc_svc.waitFootSteps()
00196         ret = checkActualBaseAttitude()
00197         if ret:
00198             print >> sys.stderr, "  Start and Stop Stabilizer => OK"
00199         assert(ret)
00200     else:
00201         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00202 
00203 def demoSTStairWalk ():
00204     print >> sys.stderr, "6. EEFMQPCOP + stair"
00205     if hcf.pdc:
00206         # setup controllers
00207         printActualBase()
00208         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP)
00209         hcf.startStabilizer()
00210         hcf.startAutoBalancer()
00211         hcf.seq_svc.setJointAngles(half_sitting_pose, 1.0);
00212         hcf.waitInterpolation();
00213         printActualBase()
00214         # set gg param
00215         ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00216         org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00217         ggp.default_orbit_type = OpenHRP.AutoBalancerService.STAIR
00218         ggp.swing_trajectory_time_offset_xy2z=0.1
00219         ggp.swing_trajectory_delay_time_offset=0.2
00220         ggp.toe_heel_phase_ratio=[0.05, 0.25, 0.20, 0.0, 0.18, 0.23, 0.09]
00221         ggp.toe_pos_offset_x = 1e-3*182.0;
00222         ggp.heel_pos_offset_x = 1e-3*-72.0;
00223         ggp.toe_zmp_offset_x = 1e-3*182.0;
00224         ggp.heel_zmp_offset_x = 1e-3*-72.0;
00225         ggp.use_toe_heel_transition=True
00226         ggp.use_toe_heel_auto_set = True
00227         ggp.toe_angle = 20;
00228         ggp.heel_angle = 10;
00229         hcf.abc_svc.setGaitGeneratorParam(ggp)
00230         printActualBase()
00231         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
00232                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.27,0.09,0.1], [1,0,0,0], "lleg")]),
00233                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.27,-0.09,0.1], [1,0,0,0], "rleg")]),
00234                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.54,0.09,0], [1,0,0,0], "lleg")]),
00235                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.54,-0.09,0], [1,0,0,0], "rleg")])]);
00236         printActualBase()
00237         hcf.abc_svc.waitFootSteps();
00238         printActualBase()
00239         # finish
00240         hcf.abc_svc.setGaitGeneratorParam(org_ggp)
00241         ret = checkActualBaseAttitude()
00242         printActualBase()
00243         if ret:
00244             print >> sys.stderr, "  ST + Stair => OK"
00245         assert(ret)
00246     else:
00247         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00248 
00249 def demoSTToeHeelWalk ():
00250     print >> sys.stderr, "7. EEFMQPCOP + toeheel"
00251     if hcf.pdc:
00252         # setup controllers
00253         hcf.startAutoBalancer()
00254         hcf.co_svc.disableCollisionDetection()
00255         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP)
00256         hcf.startStabilizer()
00257         hcf.seq_svc.setJointAngles(initial_pose, 2.0);
00258         hcf.waitInterpolation();
00259         # set gg param
00260         ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00261         org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00262         ggp.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE
00263         ggp.swing_trajectory_time_offset_xy2z=0.1
00264         ggp.swing_trajectory_delay_time_offset=0.2
00265         ggp.toe_heel_phase_ratio=[0.05, 0.35, 0.20, 0.0, 0.13, 0.13, 0.14]
00266         ggp.toe_pos_offset_x = 1e-3*182.0;
00267         ggp.heel_pos_offset_x = 1e-3*-72.0;
00268         ggp.toe_zmp_offset_x = 1e-3*182.0;
00269         ggp.heel_zmp_offset_x = 1e-3*-72.0;
00270         ggp.use_toe_heel_transition=True
00271         ggp.use_toe_heel_auto_set=True
00272         # test setFootStepsWithParam
00273         ggp.default_double_support_ratio=0.7
00274         hcf.abc_svc.setGaitGeneratorParam(ggp)
00275         for sgn in [1, -1]:
00276             hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
00277                                        OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.22,0.09,0], [1,0,0,0], "lleg")]),
00278                                        OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.44,-0.09,0], [1,0,0,0], "rleg")]),
00279                                        OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.44,0.09,0], [1,0,0,0], "lleg")])],
00280                                       [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
00281                                        OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)]),
00282                                        OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)]),
00283                                        OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)])])
00284             hcf.abc_svc.waitFootSteps();
00285         # test goPos
00286         ggp.default_double_support_ratio=0.2
00287         ggp.stride_parameter=[0.22,0.1,20.0,0.22]
00288         ggp.toe_angle = 20;
00289         ggp.heel_angle = 10;
00290         hcf.abc_svc.setGaitGeneratorParam(ggp)
00291         for sgn in [1, -1]:
00292             hcf.abc_svc.goPos(sgn*0.66,sgn*0.2,sgn*40);
00293             hcf.abc_svc.waitFootSteps();
00294         # finish
00295         hcf.abc_svc.setGaitGeneratorParam(org_ggp)
00296         hcf.co_svc.enableCollisionDetection()
00297         ret = checkActualBaseAttitude()
00298         if ret:
00299             print >> sys.stderr, "  ST + ToeHeel => OK"
00300         assert(ret)
00301     else:
00302         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00303 
00304 def demoSTTurnWalk ():
00305     print >> sys.stderr, "8. EEFMQPCOP st + Turn walk"
00306     if hcf.pdc:
00307         hcf.startAutoBalancer()
00308         hcf.co_svc.disableCollisionDetection()
00309         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP)
00310         hcf.startStabilizer()
00311         ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00312         org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
00313         ggp.stride_parameter=[0.15, 0.15, 90.0, 0.05]
00314         hcf.abc_svc.setGaitGeneratorParam(ggp)
00315         hcf.abc_svc.goPos(0,-0.2,0);
00316         hcf.abc_svc.waitFootSteps();
00317         hcf.abc_svc.goPos(0,0,175);
00318         hcf.abc_svc.waitFootSteps();
00319         hcf.abc_svc.goPos(0.4,0.15,40);
00320         hcf.abc_svc.waitFootSteps();
00321         # Wait for non-st osscilation being samalpl
00322         hcf.abc_svc.setGaitGeneratorParam(org_ggp)
00323         hcf.co_svc.enableCollisionDetection()
00324         ret = checkActualBaseAttitude()
00325         if ret:
00326             print >> sys.stderr, "  ST + Turnwalk => OK"
00327         assert(ret)
00328     else:
00329         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00330 
00331 
00332 def demoSTTransitionAirGround ():
00333     # This example is from YoheiKakiuchi's comment : https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102#issuecomment-284609203
00334     print >> sys.stderr, "9. ST Transition (in the air and on the ground)"
00335     if hcf.pdc:
00336         # Init
00337         stp_org = hcf.st_svc.getParameter()
00338         stp = hcf.st_svc.getParameter()
00339         stp.transition_time = 0.1; # for fast checking
00340         hcf.st_svc.setParameter(stp)
00341         # Tests
00342         print >> sys.stderr, "  9-1. Check in the air"
00343         hcf.startStabilizer()
00344         mimicInTheAir()
00345         hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition
00346         cmode1 = hcf.st_svc.getParameter().controller_mode
00347         vcheck1 = (cmode1 == OpenHRP.StabilizerService.MODE_AIR)
00348         print >> sys.stderr, "  9-2. Check on the ground"
00349         mimicOnTheGround()
00350         hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition
00351         cmode2 = hcf.st_svc.getParameter().controller_mode
00352         vcheck2 = (cmode2 == OpenHRP.StabilizerService.MODE_ST)
00353         print >> sys.stderr, "  9-3. Check in the air and then stopST"
00354         mimicInTheAir()
00355         hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until in the air flag is invoked in onExecute
00356         hcf.stopStabilizer()
00357         cmode3 = hcf.st_svc.getParameter().controller_mode
00358         vcheck3 = (cmode3 == OpenHRP.StabilizerService.MODE_IDLE)
00359         print >> sys.stderr, "  9-4. Check on the ground"
00360         mimicOnTheGround()
00361         hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until on the ground flag is invoked in onExecute
00362         hcf.startStabilizer()
00363         cmode4 = hcf.st_svc.getParameter().controller_mode
00364         vcheck4 = (cmode4 == OpenHRP.StabilizerService.MODE_ST)
00365         # Finsh
00366         hcf.st_svc.setParameter(stp_org)
00367         vcheck_list = [vcheck1, vcheck2, vcheck3, vcheck4]
00368         print >> sys.stderr, "  ST Transition Air Ground vcheck = ", vcheck_list, ", cmode = ", [cmode1, cmode2, cmode3, cmode4]
00369         if all(vcheck_list):
00370             print >> sys.stderr, "  ST Transition Air Ground => OK"
00371         assert(all(vcheck_list))
00372     else:
00373         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00374 
00375 def demoSTRootRotChange ():
00376     print >> sys.stderr, "10. ST root rot change"
00377     if hcf.pdc:
00378         # 10deg
00379         root_rot_x_pose=[-0.240857,-0.634561,0.012382,1.30211,-0.669201,0.073893,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.232865,-0.555515,0.011753,1.1356,-0.581653,0.06476,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00380         # 35deg
00381         root_rot_y_pose=[-1.706033e-05,-1.04708,-0.000479,0.497763,-0.060719,-0.000105,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-1.690260e-05,-1.04693,-0.000479,0.497318,-0.060422,-0.000105,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00382         # 25deg
00383         root_rot_z_pose=[-0.261382,-0.479591,-0.490714,1.26471,-0.722778,0.018041,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.313108,-0.610397,-0.535653,1.24943,-0.571839,-0.013257,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00384         # all 10deg
00385         root_rot_xyz_pose=[-0.378611,-0.81283,-0.238181,1.23534,-0.577915,0.061071,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.351695,-0.768514,-0.225097,1.05221,-0.442267,0.050849,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00386         hcf.startAutoBalancer();
00387         changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP)
00388         print >> sys.stderr, "  init"
00389         checkActualBaseAttitude()
00390         hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation();
00391         hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation(); # dummy for wait
00392         print >> sys.stderr, "  root rot x done."
00393         checkActualBaseAttitude()
00394         hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation();
00395         hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation(); # dummy for wait
00396         print >> sys.stderr, "  root rot y done."
00397         checkActualBaseAttitude()
00398         hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation();
00399         hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation(); # dummy for wait
00400         print >> sys.stderr, "  root rot z done."
00401         checkActualBaseAttitude()
00402         hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation();
00403         hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation(); # dummy for wait
00404         hcf.seq_svc.setJointAngles(initial_pose, 1.0);hcf.waitInterpolation();
00405         print >> sys.stderr, "  root rot xyz done."
00406         ret = checkActualBaseAttitude()
00407         if ret:
00408             print >> sys.stderr, "  ST root rot change => OK"
00409         assert(ret)
00410     else:
00411         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00412 
00413 def demoSTMimicRouchTerrainWalk (terrain_height_diff = 0.04):
00414     print >> sys.stderr, "11. ST mimic rough terrain walk"
00415     if hcf.pdc:
00416         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
00417                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.22,0.09,terrain_height_diff], [1,0,0,0], "lleg")]),
00418                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,-0.09,0], [1,0,0,0], "rleg")]),
00419                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,0.09,0], [1,0,0,0], "lleg")])]);
00420         hcf.abc_svc.waitFootSteps();
00421         hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
00422                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.22,0.09,-1*terrain_height_diff], [1,0,0,0], "lleg")]),
00423                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,-0.09,0], [1,0,0,0], "rleg")]),
00424                           OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,0.09,0], [1,0,0,0], "lleg")])]);
00425         hcf.abc_svc.waitFootSteps();
00426         ret = checkActualBaseAttitude()
00427         if ret:
00428             print >> sys.stderr, "  ST mimic rough terrain walk => OK"
00429         assert(ret)
00430     else:
00431         print >> sys.stderr, "  This sample is neglected in High-gain mode simulation"
00432 
00433 
00434 def demo():
00435     OPENHRP3_DIR=check_output(['pkg-config', 'openhrp3.1', '--variable=prefix']).rstrip()
00436     if os.path.exists(OPENHRP3_DIR+"/share/OpenHRP-3.1/sample/model/sample1_bush.wrl"):
00437         init()
00438         if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
00439             demoGetParameter()
00440             demoSetParameter()
00441             demoSTLoadPattern()
00442             demoStartStopTPCCST()
00443             demoStartStopEEFMQPST()
00444             demoSTStairWalk()
00445             demoSTToeHeelWalk()
00446             demoSTTurnWalk()
00447             demoSTTransitionAirGround()
00448             demoSTRootRotChange()
00449             demoSTMimicRouchTerrainWalk()
00450     else:
00451         print >> sys.stderr, "Skip st test because of missing sample1_bush.wrl"
00452 
00453 if __name__ == '__main__':
00454     demo()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56