sample4legrobot_stabilizer.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 try:
4  from hrpsys.hrpsys_config import *
5  import OpenHRP
6 except:
7  print "import without hrpsys"
8  import rtm
9  from rtm import *
10  from OpenHRP import *
11  import waitInput
12  from waitInput import *
13  import socket
14  import time
15 
16 import math
17 
18 def init ():
19  global hcf, initial_pose, hrpsys_version
20  hcf = HrpsysConfigurator()
21  hcf.getRTCList = hcf.getRTCListUnstable
22  hcf.init ("Sample4LegRobot(Robot)0", "$(PROJECT_DIR)/../model/sample_4leg_robot.wrl")
23  initial_pose = [0, -0.378613, 0, 0.832038, -0.452564, 0,
24  0, -0.378613, 0, 0.832038, -0.452564, 0,
25  0, -0.378613, 0, 0.832038, -0.452564, 0,
26  0, -0.378613, 0, 0.832038, -0.452564, 0,]
27  hcf.seq_svc.setJointAngles(initial_pose, 2.0)
28  hcf.waitInterpolation()
29  # decrease zmp weight for arms
30  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
31  abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
32  hcf.abc_svc.setAutoBalancerParam(abcp)
33  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
34  ggp.zmp_weight_map = [1.0]*4
35  ggp.default_step_height = 0.05
36  hcf.abc_svc.setGaitGeneratorParam(ggp)
37  hcf.startAutoBalancer(['rleg', 'lleg', 'rarm', 'larm'])
38  hrpsys_version = hcf.seq.ref.get_component_profile().version
39  print("hrpsys_version = %s"%hrpsys_version)
40 
42  print >> sys.stderr, "1. setParameter"
43  stp_org = hcf.st_svc.getParameter()
44  # for tpcc
45  stp_org.k_tpcc_p=[0.2, 0.2]
46  stp_org.k_tpcc_x=[4.0, 4.0]
47  stp_org.k_brot_p=[0.0, 0.0]
48  # for eefm
49  tmp_leg_inside_margin=71.12*1e-3
50  tmp_leg_outside_margin=71.12*1e-3
51  tmp_leg_front_margin=182.0*1e-3
52  tmp_leg_rear_margin=72.0*1e-3
53  rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
54  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
55  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
56  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
57  lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
58  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
59  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
60  OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
61  rarm_vertices = rleg_vertices
62  larm_vertices = lleg_vertices
63  stp_org.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
64  stp_org.eefm_leg_inside_margin=tmp_leg_inside_margin
65  stp_org.eefm_leg_outside_margin=tmp_leg_outside_margin
66  stp_org.eefm_leg_front_margin=tmp_leg_front_margin
67  stp_org.eefm_leg_rear_margin=tmp_leg_rear_margin
68  stp_org.eefm_k1=[-1.39899,-1.39899]
69  stp_org.eefm_k2=[-0.386111,-0.386111]
70  stp_org.eefm_k3=[-0.175068,-0.175068]
71  stp_org.eefm_rot_damping_gain = [[20*1.6*1.5, 20*1.6*1.5, 1e5]]*4
72  stp_org.eefm_pos_damping_gain = [[3500*50, 3500*50, 3500*1.0*1.5]]*4
73  stp_org.is_ik_enable = [True]*4
74  stp_org.is_feedback_control_enable = [True]*4
75  stp_org.is_zmp_calc_enable = [True]*4
76  stp_org.eefm_use_force_difference_control=False
77  stp_org.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP
78  hcf.st_svc.setParameter(stp_org)
79  hcf.startStabilizer ()
80 
82  print >> sys.stderr,"2. setFootSteps"
83  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.00,-0.19,0], [1,0,0,0], "rleg"),
84  OpenHRP.AutoBalancerService.Footstep([0.7+0.00,+0.19,0], [1,0,0,0], "larm")]),
85  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.15,+0.19,0], [1,0,0,0], "lleg"),
86  OpenHRP.AutoBalancerService.Footstep([0.7+0.15,-0.19,0], [1,0,0,0], "rarm")]),
87  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.30,-0.19,0], [1,0,0,0], "rleg"),
88  OpenHRP.AutoBalancerService.Footstep([0.7+0.30,+0.19,0], [1,0,0,0], "larm")]),
89  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,+0.19,0], [1,0,0,0], "lleg"),
90  OpenHRP.AutoBalancerService.Footstep([0.7+0.45,-0.19,0], [1,0,0,0], "rarm")]),
91  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.0+0.45,-0.19,0], [1,0,0,0], "rleg"),
92  OpenHRP.AutoBalancerService.Footstep([0.7+0.45,+0.19,0], [1,0,0,0], "larm")])])
93  hcf.abc_svc.waitFootSteps()
94 
95 def demo():
96  init()
99 
100 if __name__ == '__main__':
101  demo()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51