[sample/SampleRobot/samplerobot_auto_balancer.py] add a sample progra…

…m of setFootSteps with arms
fkanehiro · Sep 2, 2015 · e7c6262 · e7c6262
1 parent c510312
commit e7c6262
Showing 1 changed file with 31 additions and 0 deletions.
diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py
@@ -386,6 +386,36 @@ def demoGaitGeneratorHandFixWalking():
     # abc_svc.waitFootSteps()
     # abc_svc.stopABC()
 
+def demoGaitGeneratorSetFootStepsWithArms():
+    print >> sys.stderr, "14. Trot Walking"
+    hcf.stopAutoBalancer()
+    hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0)
+    hcf.waitInterpolation()
+    # use arms
+    abcp=hcf.abc_svc.getAutoBalancerParam()[1]
+    abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
+    hcf.abc_svc.setAutoBalancerParam(abcp)
+    # decrease zmp weight for arms
+    ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
+    ggp.zmp_weight_map = [1.0, 1.0, 0.01, 0.01]
+    hcf.abc_svc.setGaitGeneratorParam(ggp)
+    # start walking
+    hcf.startAutoBalancer()
+    hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"),
+                                                             OpenHRP.AutoBalancerService.Footstep([0.23,0.21,0.86], [1,0,0,0], "larm")]),
+                      OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])])
+    # hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"),
+    #                                                          OpenHRP.AutoBalancerService.Footstep([0.23,0.21,0.86], [1,0,0,0], "larm")]),
+    #                   OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg"),
+    #                                                          OpenHRP.AutoBalancerService.Footstep([0.23,-0.21,0.86], [1,0,0,0], "rarm")])])
+    hcf.abc_svc.waitFootSteps()
+    checkActualBaseAttitude()
+    print >> sys.stderr, "  setFootSteps()=>OK"
+    hcf.stopAutoBalancer()
+    abcp.leg_names = ['rleg', 'lleg']
+    hcf.abc_svc.setAutoBalancerParam(abcp)
+    checkActualBaseAttitude()
+
 def demo():
     init()
     if hrpsys_version >= '315.5.0':
@@ -414,6 +444,7 @@ def demo():
         demoGaitGeneratorOverwriteFootsteps()
         demoGaitGeneratorOverwriteFootsteps(2)
         # demoGaitGeneratorHandFixWalking()
+        demoGaitGeneratorSetFootStepsWithArms()
 
 if __name__ == '__main__':
     demo()