hrpsys: samplerobot_terrain_walk.py Source File

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 def init ():
00017     global hcf
00018     hcf = HrpsysConfigurator()
00019     hcf.getRTCList = hcf.getRTCListUnstable
00020     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
00021 
00022 def initPose():
00023     # set initial pose from base 90mm down pose of sample/controller/SampleController/etc/Sample.pos
00024     initial_pose=[-0.000181,-0.614916,-0.000239,1.36542,-0.749643,0.000288,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000181,-0.614916,-0.000239,1.36542,-0.749643,0.000288,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
00025     hcf.seq_svc.setJointAngles(initial_pose, 1.0)
00026     hcf.seq_svc.waitInterpolation()
00027     hcf.startAutoBalancer();
00028 
00029 def demo():
00030     init()
00031     initPose()
00032 
00033 def setupGaitGeneratorParam(set_step_height=False):
00034     ggp = hcf.abc_svc.getGaitGeneratorParam()
00035     ggp[1].default_double_support_ratio = 0.3
00036     ggp[1].default_step_time = 1.2
00037     if set_step_height:
00038         ggp[1].default_step_height = 0.1
00039     #ggp[1].swing_trajectory_delay_time_offset = 0.2;
00040     ggp[1].default_orbit_type = OpenHRP.AutoBalancerService.STAIR;
00041     hcf.abc_svc.setGaitGeneratorParam(ggp[1])
00042 
00043 def stairWalk(stair_height = 0.1524):
00044     stair_stride_x = 0.25
00045     floor_stride_x = 0.16
00046     init_step_x = 0
00047     init_step_z = 0
00048     ret = []
00049     setupGaitGeneratorParam()
00050     for step_idx in [1,2,3,4]:
00051         ret = ret + [OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg")]),
00052                      OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]),
00053                      OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")]),
00054                      OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])]
00055         init_step_x = init_step_x + stair_stride_x + floor_stride_x
00056         init_step_z = init_step_z + stair_height
00057     hcf.setFootSteps(ret)
00058     hcf.abc_svc.waitFootSteps()
00059 
00060 # sample for SampleRobot.TerrainFloor.SlopeUpDown.xml
00061 def demoSlopeUpDown():
00062     print "Start stlop up down"
00063     setupGaitGeneratorParam(True)
00064     hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]);
00065     fsList=[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg")]),
00066             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.0953,0.09,0.030712], [0.991445,0.0,-0.130526,0.0], "lleg")]),
00067             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.28848,-0.09,0.082475], [0.991445,0.0,-0.130526,0.0], "rleg")]),
00068             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.38508,0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "lleg")]),
00069             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.38508,-0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "rleg")]),
00070             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.54959,0.09,0.125863], [0.991445,0.0,0.130526,0.0], "lleg")]),
00071             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.74277,-0.09,0.074099], [0.991445,0.0,0.130526,0.0], "rleg")]),
00072             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.79107,0.09,0.061158], [0.991445,0.0,0.130526,0.0], "lleg")]),
00073             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([2.05,-0.09,0.0], [1.0,0.0,0.0,0.0], "rleg")]),
00074             OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([2.05,0.09,0.0], [1.0,0.0,0.0,0.0], "lleg")])]
00075     # set st Parameter
00076     # stp1 = hcf.st_svc.getParameter()
00077     # stp1.k_tpcc_p=[0.05, 0.05]
00078     # stp1.k_tpcc_x=[4.0, 4.0]
00079     # stp1.k_brot_p=[0.0, 0.0]
00080     # hcf.st_svc.setParameter(stp1)
00081     # hcf.st_svc.startStabilizer ()
00082     for idx in range(len(fsList)-1):
00083         hcf.setFootSteps([fsList[idx],fsList[idx+1]])
00084         hcf.abc_svc.waitFootSteps()
00085     hcf.abc_svc.stopAutoBalancer();
00086 
00087 # sample for SampleRobot.TerrainFloor.StairUp.xml
00088 def demoStairUp():
00089     print "Start stair up"
00090     stairWalk()
00091 
00092 # sample for SampleRobot.TerrainFloor.StairDown.xml
00093 def demoStairDown():
00094     print "Start stair down"
00095     hcf.abc_svc.goPos(0.05, 0.0, 0.0)
00096     hcf.abc_svc.waitFootSteps()
00097     stairWalk(-0.1524)
00098 
00099 def demoStairUpDown():
00100     print "Start stair up"
00101     stairWalk()
00102     hcf.abc_svc.goPos(0.05, 0.0, 0.0)
00103     hcf.abc_svc.waitFootSteps()
00104     print "Start stair down"
00105     stairWalk(-0.1524)
00106 
00107 if __name__ == '__main__':
00108     demo()