Merge pull request #788 from eisoku9618/add-four-leg-test-program

Add four leg test program
fkanehiro · Sep 3, 2015 · 30a55d5 · 30a55d5
2 parents 89b3d5a + 510e194
commit 30a55d5
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 3 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1158,7 +1158,7 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo
         for (size_t i = 0; i < fs_vec_list.size(); i++) {
             if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
                 std::vector<step_node> tmp_fns;
-                for (size_t j = 0; j < fs_vec_list.at(j).size(); j++) {
+                for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
                     tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
                 }
                 fnsl.push_back(tmp_fns);

diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py
@@ -14,7 +14,7 @@
     import time
 
 def init ():
-    global hcf, initial_pose, arm_front_pose, half_sitting_pose, root_rot_x_pose, root_rot_y_pose, pose_list, hrpsys_version
+    global hcf, initial_pose, arm_front_pose, half_sitting_pose, root_rot_x_pose, root_rot_y_pose, pose_list, hrpsys_version, four_legs_mode_pose
     hcf = HrpsysConfigurator()
     hcf.getRTCList = hcf.getRTCListUnstable
     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
@@ -24,7 +24,8 @@ def init ():
     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]
     root_rot_x_pose = [-0.241557,-0.634167,0.011778,1.30139,-0.668753,0.074236,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.233491,-0.555191,0.011181,1.13468,-0.580942,0.065086,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0]
     root_rot_y_pose = [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]
-    pose_list=[half_sitting_pose, root_rot_x_pose, root_rot_y_pose, arm_front_pose]
+    four_legs_mode_pose = [0.0, -0.349066, 0.0, 0.820305, -0.471239, 0.0, 0.493231, 0.008013, 0.000304, -1.608, 0.008019, -0.456023, 0.637045, 0.0, -0.349066, 0.0, 0.820305, -0.471239, 0.0, 0.493231, -0.008013, -0.000304, -1.608, -0.008019, -0.456023, -0.637045, 0.0, 0.0, 0.0]
+    pose_list=[half_sitting_pose, root_rot_x_pose, root_rot_y_pose, arm_front_pose, four_legs_mode_pose]
     hcf.seq_svc.setJointAngles(initial_pose, 2.0)
     hcf.waitInterpolation()
     hrpsys_version = hcf.seq.ref.get_component_profile().version
@@ -130,6 +131,23 @@ def demoAutoBalancerBalanceAgainstHandForce():
     hcf.stopAutoBalancer();
     checkActualBaseAttitude()
 
+def demoAutoBalancerBalanceWithArms():
+    print >> sys.stderr, "8. balance with arms"
+    hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0)
+    hcf.waitInterpolation()
+    abcp=hcf.abc_svc.getAutoBalancerParam()[1]
+    abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
+    hcf.abc_svc.setAutoBalancerParam(abcp)
+    hcf.startAutoBalancer();
+    print >> sys.stderr, "  startAutoBalancer with arms"
+    hcf.stopAutoBalancer();
+    print >> sys.stderr, "  stopAutoBalancer"
+    abcp.leg_names = ['rleg', 'lleg']
+    hcf.abc_svc.setAutoBalancerParam(abcp)
+    checkActualBaseAttitude()
+    hcf.seq_svc.setJointAngles(initial_pose, 1.0)
+    hcf.waitInterpolation()
+
 def demoGaitGeneratorGoPos():
     print >> sys.stderr, "1. goPos"
     hcf.startAutoBalancer();
@@ -370,6 +388,35 @@ 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
+    orig_abcp = abcp =hcf.abc_svc.getAutoBalancerParam()[1]
+    abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
+    hcf.abc_svc.setAutoBalancerParam(abcp)
+    # decrease zmp weight for arms
+    orig_ggp = ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
+    ggp.zmp_weight_map = [1.0, 1.0, 0.01, 0.01]
+    ggp.default_step_height = 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"),
+                                                             OpenHRP.AutoBalancerService.Footstep([0.23,-0.21,0.86], [1,0,0,0], "rarm")])])
+    hcf.abc_svc.waitFootSteps()
+    checkActualBaseAttitude()
+    print >> sys.stderr, "  setFootSteps()=>OK"
+    # reset params
+    hcf.stopAutoBalancer()
+    hcf.abc_svc.setAutoBalancerParam(orig_abcp)
+    hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
+    hcf.startAutoBalancer()
+
 def demo():
     init()
     if hrpsys_version >= '315.5.0':
@@ -381,6 +428,7 @@ def demo():
         demoAutoBalancerTestPoses()
         demoAutoBalancerStartStopCheck()
         demoAutoBalancerBalanceAgainstHandForce()
+        demoAutoBalancerBalanceWithArms()
         # sample for walk pattern generation by AutoBalancer RTC
         demoGaitGeneratorGoPos()
         demoGaitGeneratorGoVelocity()
@@ -397,6 +445,7 @@ def demo():
         demoGaitGeneratorOverwriteFootsteps()
         demoGaitGeneratorOverwriteFootsteps(2)
         # demoGaitGeneratorHandFixWalking()
+        demoGaitGeneratorSetFootStepsWithArms()
 
 if __name__ == '__main__':
     demo()