samplerobot_auto_balancer.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 def defJointGroups ():
17  rleg_6dof_group = ['rleg', ['RLEG_HIP_R', 'RLEG_HIP_P', 'RLEG_HIP_Y', 'RLEG_KNEE', 'RLEG_ANKLE_P', 'RLEG_ANKLE_R']]
18  lleg_6dof_group = ['lleg', ['LLEG_HIP_R', 'LLEG_HIP_P', 'LLEG_HIP_Y', 'LLEG_KNEE', 'LLEG_ANKLE_P', 'LLEG_ANKLE_R']]
19  torso_group = ['torso', ['WAIST_P', 'WAIST_R', 'CHEST']]
20  head_group = ['head', []]
21  rarm_group = ['rarm', ['RARM_SHOULDER_P', 'RARM_SHOULDER_R', 'RARM_SHOULDER_Y', 'RARM_ELBOW', 'RARM_WRIST_Y', 'RARM_WRIST_P', 'RARM_WRIST_R']]
22  larm_group = ['larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']]
23  return [rleg_6dof_group, lleg_6dof_group, torso_group, head_group, rarm_group, larm_group]
24 
25 def init ():
26  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
27  hcf = HrpsysConfigurator()
28  hcf.getRTCList = hcf.getRTCListUnstable
29  hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
30  hcf.connectLoggerPort(hcf.abc, 'baseRpyOut') # Just for checking
31  hcf.Groups = defJointGroups()
32  # set initial pose from sample/controller/SampleController/etc/Sample.pos
33  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.637045, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, -0.637045, 0, 0, 0]
34  arm_front_pose = [-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,-1.5708,-0.159481,-0.115399,-0.349066,0.0,0.0,0.0,-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,-1.5708,0.159481,0.115399,-0.349066,0.0,0.0,0.0,0.0,0.0,0.0]
35  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]
36  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]
37  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]
38  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]
39  pose_list=[half_sitting_pose, root_rot_x_pose, root_rot_y_pose, arm_front_pose, four_legs_mode_pose]
40  hcf.seq_svc.setJointAngles(initial_pose, 2.0)
41  hcf.waitInterpolation()
42  hrpsys_version = hcf.seq.ref.get_component_profile().version
43  print("hrpsys_version = %s"%hrpsys_version)
44 
45 def testPoseList(pose_list, initial_pose):
46  for pose in pose_list:
47  hcf.seq_svc.setJointAngles(pose, 1.0)
48  hcf.waitInterpolation()
49  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
50  hcf.waitInterpolation()
51 
52 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-auto-balancer-check-param"):
53  hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
54 
55 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-auto-balancer-check-param", save_log=True, rtc_name="SampleRobot(Robot)0"):
56  if save_log:
57  saveLogForCheckParameter(log_fname)
58  return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
59 
60 def checkActualBaseAttitude(ref_rpy = None, thre=0.1): # degree
61  '''Check whether the robot falls down based on actual robot base-link attitude.
62  '''
63  act_rpy = checkParameterFromLog("WAIST")[3:]
64  if ref_rpy == None:
65  ref_rpy = checkParameterFromLog("baseRpyOut", rtc_name="sh", save_log=False)
66  ret = abs(math.degrees(act_rpy[0]-ref_rpy[0])) < thre and abs(math.degrees(act_rpy[1]-ref_rpy[1])) < thre
67  print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, "), ", "reference base rpy = (", ref_rpy, ")"
68  assert (ret)
69  return ret
70 
71 def Quaternion2Angle(q):
72  w, v = q[0], q[1:]
73  theta = math.acos(w) * 2.0
74  return theta
75 
76 def Quaternion2RotMatrixZ(q):
77  theta = Quaternion2Angle(q)
78  return numpy.array([[numpy.cos(theta), -numpy.sin(theta), 0],
79  [numpy.sin(theta), numpy.cos(theta), 0],
80  [ 0, 0, 1]])
81 
82 
83 def calcDiffFootMidCoords (prev_dst_foot_midcoords):
84  '''Calculate difference from previous dst_foot_midcoords and current dst_foot_midcoords.
85  Returns difx, dify, difth, which are gopos parameters
86  '''
87  new_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
88  # Check diff
89  difxy = (Quaternion2RotMatrixZ(prev_dst_foot_midcoords.rot).transpose()).dot((numpy.array([new_dst_foot_midcoords.pos])-numpy.array([prev_dst_foot_midcoords.pos])).transpose())
90  difth = math.degrees(Quaternion2Angle(new_dst_foot_midcoords.rot)-Quaternion2Angle(prev_dst_foot_midcoords.rot))
91  return [difxy[0,0], difxy[1,0], difth]
92 
93 def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords):
94  '''Check whether goPos argument are correctly achieved based on dst_foot_midcoords values.
95  goPos params should be "new_dst_foot_midcoords - prev_dst_foot_midcoords"
96  '''
97  # Check diff
98  [difx, dify, difth] = calcDiffFootMidCoords(prev_dst_foot_midcoords)
99  ret = (abs(difx-goalx) < 5e-5 and abs(dify-goaly) < 5e-5 and abs(difth-goalth) < 1e-2)
100  print >> sys.stderr, " Check goPosParam (diff = ", (difx-goalx), "[m], ", (dify-goaly), "[m], ", (difth-goalth), "[deg])"
101  print >> sys.stderr, " => ", ret
102  assert(ret)
103  return ret
104 
105 def calcVelListFromPosList(pos_list, dt):
106  '''Calculate velocity list from position list.
107  Element of pos_list and vel_list should be list like [0,0,0].
108  '''
109  vel_list=[]
110  ppos=pos_list[0]
111  for pos in pos_list:
112  vel_list.append(map(lambda x, y: (x-y)/dt, pos, ppos));
113  ppos=pos
114  return vel_list
115 
116 def checkTooLargeABCCogAcc (acc_thre = 5.0): # [m/s^2]
117  '''Check ABC too large cog acceleration.
118  This is used discontinuous cog trajectory.
119  '''
120  # Parse COG [m] and tm [s]
121  cog_list=[]
122  tm_list=[]
123  for line in open("/tmp/test-abc-log.abc_cogOut", "r"):
124  tm_list.append(float(line.split(" ")[0]));
125  cog_list.append(map(float, line.split(" ")[1:-1]));
126  cog_list=cog_list[:-1000] # ?? Neglect latter elements
127  dt = tm_list[1]-tm_list[0] # [s]
128  # Calculate velocity and acceleration
129  dcog_list=calcVelListFromPosList(cog_list, dt)
130  ddcog_list=calcVelListFromPosList(dcog_list, dt)
131  # Check max
132  max_cogx_acc = max(map(lambda x : abs(x[0]), ddcog_list))
133  max_cogy_acc = max(map(lambda x : abs(x[1]), ddcog_list))
134  ret = (max_cogx_acc < acc_thre) and (max_cogy_acc < acc_thre)
135  print >> sys.stderr, " Check acc x = ", max_cogx_acc, ", y = ", max_cogy_acc, ", thre = ", acc_thre, "[m/s^2], ret = ", ret
136  assert(ret)
137 
138 def demoAutoBalancerFixFeet ():
139  print >> sys.stderr, "1. AutoBalancer mode by fixing feet"
140  hcf.startAutoBalancer(["rleg", "lleg"]);
141  hcf.seq_svc.setJointAngles(arm_front_pose, 1.0)
142  hcf.waitInterpolation()
143  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
144  hcf.waitInterpolation()
145  hcf.stopAutoBalancer();
146  checkActualBaseAttitude()
147  print >> sys.stderr, " Start and Stop AutoBalancer by fixing feet=>OK"
148 
149 def demoAutoBalancerFixFeetHands ():
150  print >> sys.stderr, "2. AutoBalancer mode by fixing hands and feet"
151  hcf.startAutoBalancer()
152  hcf.seq_svc.setJointAngles(arm_front_pose, 1.0)
153  hcf.waitInterpolation()
154  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
155  hcf.waitInterpolation()
156  hcf.stopAutoBalancer();
157  checkActualBaseAttitude()
158  print >> sys.stderr, " Start and Stop AutoBalancer by fixing hands and feet=>OK"
159 
160 def demoAutoBalancerGetParam():
161  print >> sys.stderr, "3. getAutoBalancerParam"
162  ret = hcf.abc_svc.getAutoBalancerParam()
163  if ret[0]:
164  print >> sys.stderr, " getAutoBalancerParam() => OK"
165 
166 def demoAutoBalancerSetParam():
167  print >> sys.stderr, "4. setAutoBalancerParam"
168  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
169  abcp.default_zmp_offsets = [[0.1,0,0], [0.1,0,0], [0,0,0], [0,0,0]]
170  hcf.abc_svc.setAutoBalancerParam(abcp)
171  print >> sys.stderr, " default_zmp_offsets setting check in start and stop"
172  hcf.startAutoBalancer(["rleg", "lleg"]);
173  hcf.stopAutoBalancer();
174  ret=hcf.abc_svc.getAutoBalancerParam()
175  flag = (ret[0] and numpy.allclose(ret[1].default_zmp_offsets, abcp.default_zmp_offsets, 1e-6))
176  if flag:
177  print >> sys.stderr, " setAutoBalancerParam() => OK"
178  assert (flag), (ret[0], ret[1].default_zmp_offsets, abcp.default_zmp_offsets)
179  abcp.default_zmp_offsets = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]
180  hcf.abc_svc.setAutoBalancerParam(abcp)
181 
182 def demoAutoBalancerTestPoses():
183  print >> sys.stderr, "5. change base height, base rot x, base rot y, and upper body while AutoBalancer mode"
184  hcf.startAutoBalancer(["rleg", "lleg"]);
185  testPoseList(pose_list, initial_pose)
186  hcf.stopAutoBalancer();
187  checkActualBaseAttitude()
188 
189 def demoAutoBalancerStartStopCheck():
190  print >> sys.stderr, "6. start stop check"
191  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
192  abcp.default_zmp_offsets = [[-0.05,0.05,0], [-0.05,0.05,0], [0,0,0], [0,0,0]]
193  hcf.abc_svc.setAutoBalancerParam(abcp)
194  hcf.setMaxLogLength(1500)
195  for pose in pose_list:
196  hcf.seq_svc.setJointAngles(pose, 1.0)
197  hcf.waitInterpolation()
198  hcf.clearLog()
199  hcf.startAutoBalancer(["rleg", "lleg"]);
200  hcf.stopAutoBalancer();
201  hcf.saveLog("/tmp/test-samplerobot-abc-startstop-{0}".format(pose_list.index(pose)))
202  abcp.default_zmp_offsets = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]
203  hcf.abc_svc.setAutoBalancerParam(abcp)
204  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
205  hcf.waitInterpolation()
206  checkActualBaseAttitude()
207 
208 def demoAutoBalancerBalanceAgainstHandForce():
209  print >> sys.stderr, "7. balance against hand force"
210  hcf.startAutoBalancer(["rleg", "lleg"]);
211  hcf.seq_svc.setWrenches([0,0,0,0,0,0,
212  0,0,0,0,0,0,
213  0,0,0,0,0,0,
214  0,0,-50,0,0,0,], 1.0); # rhsensor
215  hcf.waitInterpolation();
216  hcf.seq_svc.setWrenches([0,0,0,0,0,0,
217  0,0,0,0,0,0,
218  0,0,0,0,0,0,
219  0,0,0,0,0,0,], 1.0);
220  hcf.waitInterpolation();
221  hcf.stopAutoBalancer();
222  checkActualBaseAttitude()
223 
224 def demoAutoBalancerBalanceWithArms():
225  print >> sys.stderr, "8. balance with arms"
226  hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0)
227  hcf.waitInterpolation()
228  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
229  abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
230  hcf.abc_svc.setAutoBalancerParam(abcp)
231  hcf.startAutoBalancer();
232  print >> sys.stderr, " startAutoBalancer with arms"
233  hcf.stopAutoBalancer();
234  print >> sys.stderr, " stopAutoBalancer"
235  abcp.leg_names = ['rleg', 'lleg']
236  hcf.abc_svc.setAutoBalancerParam(abcp)
237  checkActualBaseAttitude()
238  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
239  hcf.waitInterpolation()
240 
241 def demoGaitGeneratorBaseTformCheck ():
242  print >> sys.stderr, "0. baseTform check"
243  # Set parameter
244  orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
245  orig_abcp = hcf.abc_svc.getAutoBalancerParam()[1]
246  hcf.co_svc.disableCollisionDetection()
247  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
248  ggp.stride_parameter = [0.2, 0.1, 20, 0.15]
249  ggp.default_step_time = 0.5
250  hcf.abc_svc.setGaitGeneratorParam(ggp)
251  abcp = hcf.abc_svc.getAutoBalancerParam()[1]
252  abcp.transition_time = 0.1
253  hcf.abc_svc.setAutoBalancerParam(abcp)
254  btf0 = checkParameterFromLog("baseTformOut", rtc_name="abc")
255  # Check start ABC
256  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
257  [[0]*29], # vels
258  [[0]*29], # torques
259  [[btf0[0]+0.2+-0.014759, btf0[1]+-0.1+-4.336272e-05, 0.668138]], # poss
260  [[-0.000245, -0.000862, 0.000171]], # rpys
261  [[0]*3], # accs
262  [[0.014052, 0.000203, -0.66798]], # zmps
263  [[0]*6*4], # wrenchs
264  [[1,1,0,0,1,1,1,1]], # optionals
265  [0.5]); # tms
266  hcf.waitInterpolation()
267  btf1 = checkParameterFromLog("baseTformOut", rtc_name="abc")
268  hcf.startAutoBalancer()
269  btf2 = checkParameterFromLog("baseTformOut", rtc_name="abc")
270  # Check stop ABC
271  hcf.abc_svc.goPos(-0.2, 0.1, 0)
272  hcf.abc_svc.waitFootSteps()
273  btf3 = checkParameterFromLog("baseTformOut", rtc_name="abc")
274  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
275  [[0]*29], # vels
276  [[0]*29], # torques
277  [[btf0[0]+-0.014759, btf0[1]+-4.336272e-05, 0.668138]], # poss
278  [[-0.000245, -0.000862, 0.000171]], # rpys
279  [[0]*3], # accs
280  [[0.014052, 0.000203, -0.66798]], # zmps
281  [[0]*6*4], # wrenchs
282  [[1,1,0,0,1,1,1,1]], # optionals
283  [0.1]); # tms
284  hcf.waitInterpolation()
285  hcf.stopAutoBalancer()
286  btf4 = checkParameterFromLog("baseTformOut", rtc_name="abc")
287  # Finalize
288  hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
289  hcf.abc_svc.setAutoBalancerParam(orig_abcp)
290  hcf.co_svc.enableCollisionDetection()
291  # Check values (currently pos x,y only 1[mm])
292  startABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf1[0:3], btf2[0:3]))
293  stopABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf3[0:3], btf4[0:3]))
294  print >> sys.stderr, " before startABC = ", btf1[0:3], ", after startABC = ", btf2[0:3], ", diff = ", startABC_OK
295  print >> sys.stderr, " before stopABC = ", btf3[0:3], ", after stopABC = ", btf4[0:3], ", diff = ", stopABC_OK
296  assert(startABC_OK and stopABC_OK)
297 
298 def demoGaitGeneratorGoPos():
299  print >> sys.stderr, "1. goPos"
300  hcf.startAutoBalancer();
301  # initialize dst_foot_midcoords
302  hcf.abc_svc.goPos(0,0,0)
303  hcf.abc_svc.waitFootSteps()
304  # gopos check 1
305  goalx=0.1;goaly=0.1;goalth=20.0
306  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
307  hcf.abc_svc.goPos(goalx, goaly, goalth)
308  hcf.abc_svc.waitFootSteps()
309  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
310  # gopos check 2
311  goalx=-0.1;goaly=-0.1;goalth=-10.0
312  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
313  hcf.abc_svc.goPos(goalx, goaly, goalth)
314  hcf.abc_svc.waitFootSteps()
315  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
316  checkActualBaseAttitude()
317  print >> sys.stderr, " goPos()=>OK"
318 
319 def demoGaitGeneratorGoVelocity():
320  print >> sys.stderr, "2. goVelocity and goStop"
321  print >> sys.stderr, " goVelocity few steps"
322  hcf.abc_svc.goVelocity(-0.1, -0.05, -20)
323  time.sleep(1)
324  hcf.abc_svc.goStop()
325  checkActualBaseAttitude()
326  print >> sys.stderr, " goVelocity few steps=>OK"
327  print >> sys.stderr, " Check discontinuity of COG by checking too large COG acc."
328  hcf.setMaxLogLength(10000)
329  hcf.clearLog()
330  hcf.abc_svc.goVelocity(0,0,0) # One step overwrite
331  hcf.abc_svc.goStop()
332  hcf.saveLog("/tmp/test-abc-log");
333  checkTooLargeABCCogAcc()
334 
335 def demoGaitGeneratorSetFootSteps():
336  print >> sys.stderr, "3. setFootSteps"
337  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
338  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])])
339  hcf.abc_svc.waitFootSteps()
340  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
341  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")]),
342  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,-0.09,0], [1,0,0,0], "rleg")]),
343  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], "lleg")])])
344  hcf.abc_svc.waitFootSteps()
345  checkActualBaseAttitude()
346  print >> sys.stderr, " setFootSteps()=>OK"
347 
348 def demoGaitGeneratorChangePoseWhileWalking():
349  print >> sys.stderr, "4. Change base height, base rot x, base rot y, and upper body while walking"
350  hcf.abc_svc.waitFootSteps()
351  hcf.abc_svc.goVelocity(0,0,0)
352  testPoseList(pose_list, initial_pose)
353  hcf.abc_svc.goStop()
354  checkActualBaseAttitude()
355 
356 def demoGaitGeneratorGetParam():
357  print >> sys.stderr, "5. getGaitGeneratorParam"
358  ret = hcf.abc_svc.getGaitGeneratorParam()
359  if ret[0]:
360  print >> sys.stderr, " getGaitGeneratorParam() => OK"
361 
362 def demoGaitGeneratorSetParam():
363  print >> sys.stderr, "6. setGaitGeneratorParam"
364  ggp_org = hcf.abc_svc.getGaitGeneratorParam()[1]
365  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
366  ggp.default_step_time = 0.9
367  ggp.default_step_height = 0.15
368  ggp.default_double_support_ratio = 0.4
369  ggp.swing_trajectory_delay_time_offset = 0.20
370  ggp.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE;
371  hcf.abc_svc.setGaitGeneratorParam(ggp)
372  ret = hcf.abc_svc.getGaitGeneratorParam()
373  if ret[0] and ret[1].default_step_time == ggp.default_step_time and ret[1].default_step_height == ggp.default_step_height and ret[1].default_double_support_ratio == ggp.default_double_support_ratio and ret[1].default_orbit_type == ggp.default_orbit_type:
374  print >> sys.stderr, " setGaitGeneratorParam() => OK"
375  hcf.abc_svc.goPos(0.2,0,0)
376  hcf.abc_svc.waitFootSteps()
377  hcf.abc_svc.setGaitGeneratorParam(ggp_org) # revert parameter
378 
379 def demoGaitGeneratorNonDefaultStrideStop():
380  print >> sys.stderr, "7. non-default stride"
381  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
382  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")])])
383  hcf.abc_svc.waitFootSteps()
384  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
385  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])])
386  hcf.abc_svc.waitFootSteps()
387  checkActualBaseAttitude()
388  print >> sys.stderr, " Non default Stride()=>OK"
389 
390 def demoGaitGeneratorToeHeelContact():
391  print >> sys.stderr, "8. Use toe heel contact"
392  ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
393  ggp.toe_pos_offset_x = 1e-3*182.0;
394  ggp.heel_pos_offset_x = 1e-3*-72.0;
395  ggp.toe_zmp_offset_x = 1e-3*182.0;
396  ggp.heel_zmp_offset_x = 1e-3*-72.0;
397  ggp.toe_angle = 20;
398  ggp.heel_angle = 10;
399  hcf.abc_svc.setGaitGeneratorParam(ggp);
400  hcf.abc_svc.goPos(0.3, 0, 0);
401  hcf.abc_svc.waitFootSteps()
402  ggp.toe_angle = 0;
403  ggp.heel_angle = 0;
404  hcf.abc_svc.setGaitGeneratorParam(ggp);
405  checkActualBaseAttitude()
406  print >> sys.stderr, " Toe heel contact=>OK"
407 
408 def demoGaitGeneratorStopStartSyncCheck():
409  print >> sys.stderr, "9. Stop and start auto balancer sync check2"
410  print >> sys.stderr, " Check 9-1 Sync after setFootSteps"
411  hcf.startAutoBalancer();
412  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
413  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]);
414  hcf.abc_svc.waitFootSteps();
415  hcf.stopAutoBalancer();
416  print >> sys.stderr, " Sync after setFootSteps => OK"
417  print >> sys.stderr, " Check 9-2 Sync from setJointAngles at the beginning"
418  open_stride_pose = [0.00026722677758058496, -0.3170503560247552, -0.0002054613599000865, 0.8240549352035262, -0.5061434785447525, -8.67443660992421e-05, 0.3112899999999996, -0.15948099999999998, -0.11539900000000003, -0.6362769999999993, 0.0, 0.0, 0.0, 0.00023087433689200683, -0.4751295978345554, -0.00021953834197007937, 0.8048588066686679, -0.3288687069275527, -8.676469399681631e-05, 0.3112899999999996, 0.15948099999999998, 0.11539900000000003, -0.6362769999999993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
419  hcf.seq_svc.setJointAngles(open_stride_pose, 2.0);
420  hcf.waitInterpolation();
421  hcf.startAutoBalancer();
422  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
423  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]);
424  hcf.abc_svc.waitFootSteps();
425  hcf.stopAutoBalancer();
426  print >> sys.stderr, " Sync from setJointAngle at the beginning => OK"
427  print >> sys.stderr, " Check 9-3 Sync from setJointAngles"
428  hcf.startAutoBalancer();
429  hcf.seq_svc.setJointAngles(initial_pose, 2.0);
430  hcf.waitInterpolation();
431  hcf.stopAutoBalancer();
432  print >> sys.stderr, " Sync from setJointAngle => OK"
433 
434 def demoGaitGeneratorEmergencyStop():
435  print >> sys.stderr, "10. Emergency stop"
436  hcf.startAutoBalancer();
437  hcf.abc_svc.goPos(0,0,90);
438  print >> sys.stderr, " Start goPos and wait for 4 steps"
439  for idx in range(4): # Wait for 4 steps including initial double support phase
440  # Wait for 1 steps
441  hcf.seq_svc.setJointAngles(initial_pose, hcf.abc_svc.getGaitGeneratorParam()[1].default_step_time);
442  hcf.waitInterpolation();
443  print >> sys.stderr, " Emergency stoping"
444  hcf.abc_svc.emergencyStop();
445  print >> sys.stderr, " Align foot steps"
446  hcf.abc_svc.goPos(0,0,0);
447  checkActualBaseAttitude()
448 
449 def demoGaitGeneratorGetRemainingSteps():
450  print >> sys.stderr, "11. Get remaining foot steps"
451  hcf.abc_svc.goPos(0.3,0.1,15);
452  fslist=hcf.abc_svc.getRemainingFootstepSequence()[1]
453  while fslist != []:
454  fslist=hcf.abc_svc.getRemainingFootstepSequence()[1]
455  print >> sys.stderr, " Remaining footstep ", len(fslist)
456  # Wait for 1 step
457  hcf.seq_svc.setJointAngles(initial_pose, hcf.abc_svc.getGaitGeneratorParam()[1].default_step_time);
458  hcf.waitInterpolation();
459  checkActualBaseAttitude()
460 
461 def demoGaitGeneratorChangeStepParam():
462  print >> sys.stderr, "12. Change step param with setFootSteps"
463  ggp_org=hcf.abc_svc.getGaitGeneratorParam()[1];
464  # dummy setting
465  ggp=hcf.abc_svc.getGaitGeneratorParam()[1];
466  ggp.toe_angle = 50;
467  ggp.heel_angle = 50;
468  hcf.abc_svc.setGaitGeneratorParam(ggp);
469  hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
470  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]),
471  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]),
472  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])],
473  [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
474  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0)]),
475  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
476  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0)])])
477  hcf.abc_svc.waitFootSteps()
478  hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
479  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]),
480  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]),
481  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])],
482  [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
483  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
484  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
485  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0)])])
486  hcf.abc_svc.waitFootSteps()
487  hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
488  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]),
489  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]),
490  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])],
491  [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
492  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]),
493  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=20.0, heel_angle=5.0)]),
494  OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=10.0, heel_angle=10.0)])])
495  hcf.abc_svc.waitFootSteps()
496  hcf.abc_svc.setGaitGeneratorParam(ggp_org);
497  checkActualBaseAttitude()
498 
499 def demoGaitGeneratorOverwriteFootsteps(overwrite_offset_idx = 1):
500  print >> sys.stderr, "13. Overwrite footsteps during walking."
501  hcf.startAutoBalancer()
502  demoGaitGeneratorOverwriteFootstepsBase("x", overwrite_offset_idx, True) # Overwrite by X direction foot steps
503  hcf.seq_svc.setJointAngles(initial_pose, 1.0*overwrite_offset_idx)
504  hcf.seq_svc.waitInterpolation()
505  demoGaitGeneratorOverwriteFootstepsBase("y", overwrite_offset_idx, True) # Overwrite by Y direction foot steps
506  hcf.seq_svc.setJointAngles(initial_pose, 1.0*overwrite_offset_idx)
507  hcf.seq_svc.waitInterpolation()
508  demoGaitGeneratorOverwriteFootstepsBase("x", overwrite_offset_idx, True) # Overwrite by X direction foot steps
509  hcf.abc_svc.waitFootSteps()
510  checkActualBaseAttitude()
511 
512 def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init_fs = False):
513  if init_fs:
514  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0, -0.09,0], [1,0,0,0], "rleg")]),
515  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1, 0.09,0], [1,0,0,0], "lleg")]),
516  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]),
517  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3, 0.09,0], [1,0,0,0], "lleg")]),
518  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4,-0.09,0], [1,0,0,0], "rleg")]),
519  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4, 0.09,0], [1,0,0,0], "lleg")])]);
520  print >> sys.stderr, " Overwrite footsteps ", overwrite_offset_idx
521  # Get remaining footstep
522  [remain_fs, current_fs_idx]=hcf.abc_svc.getRemainingFootstepSequence()[1:]
523  #print >> sys.stderr, remain_fs
524  print >> sys.stderr, " Remaining legs = ", map(lambda fs : fs.leg, remain_fs)
525  print >> sys.stderr, " Remaining idx = ", map(lambda idx : current_fs_idx+idx, range(len(remain_fs)))
526  # Footstep index to be overwritten
527  overwrite_fs_idx = current_fs_idx + overwrite_offset_idx
528  print >> sys.stderr, " Overwrite index = ",overwrite_fs_idx, ", leg = ", remain_fs[overwrite_offset_idx].leg
529  # Calc new footsteps
530  import numpy
531  support_fs = remain_fs[overwrite_offset_idx-1] # support fs before overwritten fs
532  if axis == "x":
533  pos_offset = [0.1, 0, 0]
534  pos_offset2 = [0.2, 0, 0]
535  else:
536  pos_offset = [0, (0.1 if support_fs.leg =='rleg' else -0.1), 0]
537  pos_offset2 = pos_offset
538  fpos1=list(numpy.array(support_fs.pos) + numpy.array([0, 2.0*(0.09 if support_fs.leg =='rleg' else -0.09) ,0]) + numpy.array(pos_offset))
539  fpos2=list(numpy.array(support_fs.pos) + numpy.array(pos_offset))
540  fpos3=list(numpy.array(support_fs.pos) + numpy.array([0, 2.0*(0.09 if support_fs.leg =='rleg' else -0.09) ,0]) + numpy.array(pos_offset2))
541  fpos4=list(numpy.array(support_fs.pos) + numpy.array(pos_offset2))
542  new_fs =[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(support_fs.pos, [1,0,0,0], support_fs.leg)]),
543  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos1, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg")]),
544  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos2, [1,0,0,0], support_fs.leg)]),
545  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos3, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg")]),
546  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos4, [1,0,0,0], support_fs.leg)])]
547  hcf.abc_svc.setFootSteps(new_fs, overwrite_fs_idx);
548 
549 def demoGaitGeneratorFixHand():
550  print >> sys.stderr, "14. Fix arm walking"
551  hcf.stopAutoBalancer()
552  hcf.startAutoBalancer()
553  # Set pose
554  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
555  abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # Setting default_zmp_offsets is not necessary for fix mode. Just for debugging for default_zmp_offsets in hand fix mode.
556  hcf.abc_svc.setAutoBalancerParam(abcp)
557  dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163]
558  hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
559  hcf.waitInterpolation()
560  print >> sys.stderr, " Walk without fixing arm"
561  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
562  abcp.is_hand_fix_mode=False
563  hcf.abc_svc.setAutoBalancerParam(abcp)
564  hcf.abc_svc.goPos(0.3,0,0)
565  hcf.abc_svc.waitFootSteps()
566  hcf.abc_svc.goPos(0,0.2,0)
567  hcf.abc_svc.waitFootSteps()
568  hcf.abc_svc.goPos(0,0,30)
569  hcf.abc_svc.waitFootSteps()
570  print >> sys.stderr, " Walk with fixing arm"
571  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
572  abcp.is_hand_fix_mode=True
573  hcf.abc_svc.setAutoBalancerParam(abcp)
574  hcf.abc_svc.goPos(0.3,0,0)
575  hcf.abc_svc.waitFootSteps()
576  hcf.abc_svc.goPos(0,-0.2,0)
577  hcf.abc_svc.waitFootSteps()
578  hcf.abc_svc.goPos(0,0,-30)
579  hcf.abc_svc.waitFootSteps()
580  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
581  abcp.is_hand_fix_mode=False
582  abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
583  hcf.abc_svc.setAutoBalancerParam(abcp)
584  ref_rpy = checkParameterFromLog("baseRpyOut", rtc_name="abc")
585  hcf.stopAutoBalancer()
586  checkActualBaseAttitude(ref_rpy)
587  print >> sys.stderr, " Fix hand=>OK"
588 
589 def demoGaitGeneratorOverwriteCurrentFootstep():
590  print >> sys.stderr, "15. Overwrite current footstep"
591  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
592  hcf.waitInterpolation()
593  hcf.startAutoBalancer()
594  # decrease zmp weight for arms
595  orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
596  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
597  ggp.overwritable_footstep_index_offset = 0
598  ggp.default_orbit_type=OpenHRP.AutoBalancerService.RECTANGLE
599  hcf.abc_svc.setGaitGeneratorParam(ggp)
600  # start walking
601  hcf.abc_svc.goVelocity(0,0,0);
602  hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation
603  hcf.abc_svc.goVelocity(0.1,0,0);
604  hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation
605  hcf.abc_svc.goVelocity(0,0.1,0);
606  hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation
607  hcf.abc_svc.goStop()
608  checkActualBaseAttitude()
609  print >> sys.stderr, " Overwrite current footstep=>OK"
610  # reset params
611  hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
612 
613 def demoGaitGeneratorGoPosOverwrite():
614  print >> sys.stderr, "16. goPos overwriting"
615  hcf.startAutoBalancer();
616  print >> sys.stderr, " Overwrite goPos by goPos"
617  # Initialize dst_foot_midcoords
618  hcf.abc_svc.goPos(0,0.001,0);
619  hcf.abc_svc.waitFootSteps();
620  goalx=0.3;goaly=0.1;goalth=15.0
621  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
622  hcf.abc_svc.goPos(0.2,-0.1,-5) # initial gopos
623  hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation
624  hcf.abc_svc.goPos(goalx,goaly,goalth) # overwrite gopos
625  hcf.abc_svc.waitFootSteps()
626  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
627  print >> sys.stderr, " Overwrite setFootSteps by goPos"
628  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
629  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]),
630  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]),
631  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]),
632  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], "lleg")]),
633  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,-0.09,0], [1,0,0,0], "rleg")])
634  ]) # initial setfootsteps
635  hcf.seq_svc.setJointAngles(initial_pose, 2.0);hcf.waitInterpolation() # wait 2 step using dummy waitInterpolation
636  hcf.abc_svc.goPos(goalx,goaly,goalth) # overwrite gopos
637  hcf.abc_svc.waitFootSteps()
638  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
639 
640 def demoGaitGeneratorGrasplessManipMode():
641  print >> sys.stderr, "17. Graspless manip mode"
642  hcf.startAutoBalancer();
643  # Initialize and pose define
644  dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163]
645  hcf.seq_svc.setJointAngles(dualarm_push_pose, 0.5)
646  hcf.waitInterpolation()
647  # hands 50[mm] fwd from dualarm_push_pose
648  av_fwd = [-5.579249e-05,-0.760285,-0.000277,1.44619,-0.660772,-2.615057e-05,-0.7752,-0.080815,0.116555,-0.935667,-1.70514,-0.045373,1.309,-5.577374e-05,-0.760232,-0.000277,1.44608,-0.660715,-2.613350e-05,-0.77525,0.080663,-0.116463,-0.935597,1.70494,-0.045325,-1.309,0.157668,0.000123,-0.000152]
649  # hands 50[mm] bwd from dualarm_push_pose
650  av_bwd = [-1.901820e-05,-0.641174,-0.00025,1.36927,-0.717047,-2.260319e-05,-0.305537,-0.099557,0.134675,-1.04208,-1.72497,-0.065256,1.309,-1.900236e-05,-0.641122,-0.00025,1.36915,-0.71698,-2.258509e-05,-0.305624,0.099383,-0.134605,-1.04197,1.72476,-0.06517,-1.309,-0.22394,5.625198e-05,-0.000165]
651  # hands 50[mm] right from dualarm_push_pose
652  av_right = [-0.005678,-0.711398,0.006148,1.40852,-0.678974,0.011103,-0.575284,-0.179786,0.092155,-0.999366,-1.74805,0.048205,1.309,-0.005686,-0.710309,0.006143,1.4098,-0.681345,0.011103,-0.520691,0.002033,-0.154878,-1.05585,1.6731,-0.161177,-1.309,0.015053,0.024788,-0.023196]
653  # hands 50[mm] left from dualarm_push_pose
654  av_left = [0.005607,-0.71036,-0.006671,1.40991,-0.681404,-0.011151,-0.52064,-0.002193,0.154967,-1.05593,-1.67329,-0.161246,1.309,0.005598,-0.711343,-0.006677,1.4084,-0.67891,-0.011151,-0.575342,0.179622,-0.092066,-0.999287,1.74785,0.04827,-1.309,0.015056,-0.024583,0.02287]
655  # hands 10[deg] right turn from dualarm_push_pose
656  av_rturn = [0.023512,-0.71245,0.014216,1.40899,-0.677868,-0.01575,-0.462682,0.040789,0.154221,-1.10667,-1.66067,-0.2349,1.309,0.023442,-0.708029,0.014161,1.40823,-0.68153,-0.015763,-0.61987,0.217174,-0.105089,-0.949927,1.75163,0.120793,-1.309,0.013747,-0.058774,-0.084435]
657  # hands 10[deg] left turn from dualarm_push_pose
658  av_lturn = [-0.023522,-0.708079,-0.014689,1.40835,-0.681597,0.015717,-0.619803,-0.217337,0.105179,-0.950004,-1.75184,0.120733,1.309,-0.02359,-0.712393,-0.014744,1.40889,-0.677813,0.0157,-0.462722,-0.040951,-0.154135,-1.10659,1.66048,-0.234826,-1.309,0.013715,0.058979,0.084109]
659  # parameter setting
660  org_abcp=hcf.abc_svc.getAutoBalancerParam()[1]
661  abcp=hcf.abc_svc.getAutoBalancerParam()[1]
662  abcp.graspless_manip_mode=True
663  abcp.is_hand_fix_mode=True
664  abcp.graspless_manip_reference_trans_rot=[1.0, 0.0, 0.0, 1.365307e-06] # trans rot for dualarm_push_pose
665  abcp.graspless_manip_reference_trans_pos=[0.450037, 1.049436e-06, 0.869818] # trans pos for dualarm_push_pose
666  abcp.graspless_manip_p_gain=[1,1,1]
667  hcf.abc_svc.setAutoBalancerParam(abcp)
668  hcf.co_svc.disableCollisionDetection()
669  # Check one foot_midcoords movement
670  gv_pose_list = [av_fwd, av_bwd, av_left, av_right, av_lturn, av_rturn]
671  ref_footmid_diff = [[50*1e-3,0,0],
672  [-50*1e-3,0,0],
673  [0, 50*1e-3,0],
674  [0,-50*1e-3,0],
675  [0,0, 10],
676  [0,0,-10]]
677  ret=True
678  hcf.abc_svc.waitFootSteps()
679  for idx in range(len(gv_pose_list)):
680  pose = gv_pose_list[idx]
681  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
682  yvel = -0.0001 if (idx%2==0) else 0.0001 # Iff even number test, start with rleg. Otherwise, lleg.
683  hcf.abc_svc.goVelocity(0,yvel,0);
684  hcf.seq_svc.setJointAngles(pose, 0.4)
685  hcf.waitInterpolation()
686  hcf.seq_svc.setJointAngles(pose, 1.6);hcf.waitInterpolation() # Dummy 2step
687  hcf.abc_svc.goStop()
688  diff=numpy.array(ref_footmid_diff[idx])-numpy.array(calcDiffFootMidCoords(prev_dst_foot_midcoords))
689  if idx == 4 or idx == 5:
690  tmpret = abs(diff[2]) < 1.0 # TODO, check pos
691  else:
692  tmpret = abs(diff[0]) < 1e-3 and abs(diff[1]) < 1e-3 and abs(diff[2]) < 1.0
693  ret = ret and tmpret
694  print >> sys.stderr, " ret = ", tmpret, ", diff = ", diff
695  # Finishing
696  if ret:
697  print >> sys.stderr, " total is OK"
698  assert(ret)
699  hcf.co_svc.enableCollisionDetection()
700  hcf.seq_svc.setJointAngles(dualarm_push_pose, 0.5)
701  hcf.waitInterpolation()
702  hcf.abc_svc.setAutoBalancerParam(org_abcp)
703 
704 def demoGaitGeneratorSetFootStepsWithArms():
705  print >> sys.stderr, "18. Trot Walking"
706  hcf.stopAutoBalancer()
707  hcf.seq_svc.setJointAngles(four_legs_mode_pose, 1.0)
708  hcf.waitInterpolation()
709  # use arms
710  orig_abcp = hcf.abc_svc.getAutoBalancerParam()[1]
711  abcp = hcf.abc_svc.getAutoBalancerParam()[1]
712  abcp.leg_names = ['rleg', 'lleg', 'rarm', 'larm']
713  hcf.abc_svc.setAutoBalancerParam(abcp)
714  # decrease zmp weight for arms
715  orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
716  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
717  ggp.zmp_weight_map = [1.0, 1.0, 0.01, 0.01]
718  ggp.default_step_height = 0.01
719  hcf.abc_svc.setGaitGeneratorParam(ggp)
720  # start walking
721  hcf.startAutoBalancer()
722  hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"),
723  OpenHRP.AutoBalancerService.Footstep([0.23,0.21,0.86], [1,0,0,0], "larm")]),
724  OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg"),
725  OpenHRP.AutoBalancerService.Footstep([0.23,-0.21,0.86], [1,0,0,0], "rarm")])])
726  hcf.abc_svc.waitFootSteps()
727  checkActualBaseAttitude()
728  print >> sys.stderr, " setFootSteps()=>OK"
729  # reset params
730  hcf.stopAutoBalancer()
731  hcf.abc_svc.setAutoBalancerParam(orig_abcp)
732  hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
733  hcf.startAutoBalancer()
734 
735 def demoGaitGeneratorChangeStrideLimitationType():
736  print >> sys.stderr, "19. Change stride limitation type to CIRCLE"
737  hcf.startAutoBalancer();
738  # initialize dst_foot_midcoords
739  hcf.abc_svc.goPos(0,0,0)
740  hcf.abc_svc.waitFootSteps()
741  # set params
742  orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
743  ggp = hcf.abc_svc.getGaitGeneratorParam()[1]
744  ggp.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE
745  ggp.stride_limitation_for_circle_type = [0.15, 0.25, 10, 0.1, 0.1]
746  ggp.leg_margin = [182.0*1e-3, 72.0*1e-3, 71.12*1e-3, 71.12*1e-3]
747  hcf.abc_svc.setGaitGeneratorParam(ggp)
748  # gopos check 1
749  goalx=0.3;goaly=0.3;goalth=20.0
750  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
751  hcf.abc_svc.goPos(goalx, goaly, goalth)
752  hcf.abc_svc.waitFootSteps()
753  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
754  # gopos check 2
755  goalx=-0.3;goaly=-0.3;goalth=-10.0
756  prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords
757  hcf.abc_svc.goPos(goalx, goaly, goalth)
758  hcf.abc_svc.waitFootSteps()
759  checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
760  checkActualBaseAttitude()
761  print >> sys.stderr, " Change stride limitation type to CIRCLE=>OK"
762  # reset params
763  hcf.abc_svc.setGaitGeneratorParam(orig_ggp)
764 
765 def demoStandingPosResetting():
766  print >> sys.stderr, "demoStandingPosResetting"
767  hcf.abc_svc.goPos(0,0,math.degrees(-1*checkParameterFromLog("WAIST")[5])); # Rot yaw
768  hcf.abc_svc.waitFootSteps()
769  hcf.abc_svc.goPos(0,-1*checkParameterFromLog("WAIST")[1],0); # Pos Y
770  hcf.abc_svc.waitFootSteps()
771 
772 def demo():
773  start_time = time.time()
774  init()
775  from distutils.version import StrictVersion
776  if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
777  # sample for AutoBalancer mode
778  demoAutoBalancerFixFeet()
779  demoAutoBalancerFixFeetHands()
780  demoAutoBalancerGetParam()
781  demoAutoBalancerSetParam()
782  demoAutoBalancerTestPoses()
783  demoAutoBalancerStartStopCheck()
784  demoAutoBalancerBalanceAgainstHandForce()
785  demoAutoBalancerBalanceWithArms()
786  # sample for walk pattern generation by AutoBalancer RTC
787  demoGaitGeneratorBaseTformCheck()
788  demoGaitGeneratorGoPos()
789  demoGaitGeneratorGoVelocity()
790  demoGaitGeneratorSetFootSteps()
791  demoGaitGeneratorChangePoseWhileWalking()
792  demoGaitGeneratorGetParam()
793  demoGaitGeneratorSetParam()
794  demoGaitGeneratorNonDefaultStrideStop()
795  demoGaitGeneratorToeHeelContact()
796  demoGaitGeneratorStopStartSyncCheck()
797  demoGaitGeneratorEmergencyStop()
798  demoGaitGeneratorGetRemainingSteps()
799  demoGaitGeneratorChangeStepParam()
800  demoGaitGeneratorOverwriteFootsteps()
801  demoGaitGeneratorOverwriteFootsteps(2)
802  #demoStandingPosResetting()
803  demoGaitGeneratorFixHand()
804  demoGaitGeneratorOverwriteCurrentFootstep()
805  demoGaitGeneratorGoPosOverwrite()
806  demoGaitGeneratorGrasplessManipMode()
807  demoGaitGeneratorSetFootStepsWithArms()
808  demoGaitGeneratorChangeStrideLimitationType()
809  print >> sys.stderr, "All samplerobot_auto_balancer.py demo time ", (time.time()-start_time), "[s]"
810 
811 if __name__ == '__main__':
812  demo()
813 
max
#define max(a, b)
Definition: GraspController.cpp:21
hrpsys_config
samplerobot_auto_balancer.demoGaitGeneratorSetParam
def demoGaitGeneratorSetParam()
Definition: samplerobot_auto_balancer.py:362
samplerobot_auto_balancer.demoGaitGeneratorGetRemainingSteps
def demoGaitGeneratorGetRemainingSteps()
Definition: samplerobot_auto_balancer.py:449
samplerobot_auto_balancer.checkTooLargeABCCogAcc
def checkTooLargeABCCogAcc(acc_thre=5.0)
Definition: samplerobot_auto_balancer.py:116
samplerobot_auto_balancer.demoGaitGeneratorChangeStepParam
def demoGaitGeneratorChangeStepParam()
Definition: samplerobot_auto_balancer.py:461
samplerobot_auto_balancer.demoGaitGeneratorNonDefaultStrideStop
def demoGaitGeneratorNonDefaultStrideStop()
Definition: samplerobot_auto_balancer.py:379
samplerobot_auto_balancer.testPoseList
def testPoseList(pose_list, initial_pose)
Definition: samplerobot_auto_balancer.py:45
samplerobot_auto_balancer.demoGaitGeneratorGoPos
def demoGaitGeneratorGoPos()
Definition: samplerobot_auto_balancer.py:298
samplerobot_auto_balancer.demoGaitGeneratorOverwriteFootsteps
def demoGaitGeneratorOverwriteFootsteps(overwrite_offset_idx=1)
Definition: samplerobot_auto_balancer.py:499
samplerobot_auto_balancer.demoGaitGeneratorToeHeelContact
def demoGaitGeneratorToeHeelContact()
Definition: samplerobot_auto_balancer.py:390
samplerobot_auto_balancer.defJointGroups
def defJointGroups()
Definition: samplerobot_auto_balancer.py:16
samplerobot_auto_balancer.checkGoPosParam
def checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords)
Definition: samplerobot_auto_balancer.py:93
samplerobot_auto_balancer.demo
def demo()
Definition: samplerobot_auto_balancer.py:772
samplerobot_auto_balancer.demoAutoBalancerTestPoses
def demoAutoBalancerTestPoses()
Definition: samplerobot_auto_balancer.py:182
samplerobot_auto_balancer.demoGaitGeneratorGetParam
def demoGaitGeneratorGetParam()
Definition: samplerobot_auto_balancer.py:356
samplerobot_auto_balancer.demoGaitGeneratorSetFootStepsWithArms
def demoGaitGeneratorSetFootStepsWithArms()
Definition: samplerobot_auto_balancer.py:704
samplerobot_auto_balancer.init
def init()
Definition: samplerobot_auto_balancer.py:25
samplerobot_auto_balancer.demoAutoBalancerGetParam
def demoAutoBalancerGetParam()
Definition: samplerobot_auto_balancer.py:160
samplerobot_auto_balancer.demoGaitGeneratorOverwriteCurrentFootstep
def demoGaitGeneratorOverwriteCurrentFootstep()
Definition: samplerobot_auto_balancer.py:589
samplerobot_auto_balancer.demoGaitGeneratorSetFootSteps
def demoGaitGeneratorSetFootSteps()
Definition: samplerobot_auto_balancer.py:335
samplerobot_auto_balancer.demoGaitGeneratorBaseTformCheck
def demoGaitGeneratorBaseTformCheck()
Definition: samplerobot_auto_balancer.py:241
samplerobot_auto_balancer.demoGaitGeneratorFixHand
def demoGaitGeneratorFixHand()
Definition: samplerobot_auto_balancer.py:549
samplerobot_auto_balancer.demoAutoBalancerStartStopCheck
def demoAutoBalancerStartStopCheck()
Definition: samplerobot_auto_balancer.py:189
samplerobot_auto_balancer.demoAutoBalancerFixFeet
def demoAutoBalancerFixFeet()
Definition: samplerobot_auto_balancer.py:138
samplerobot_auto_balancer.demoAutoBalancerBalanceAgainstHandForce
def demoAutoBalancerBalanceAgainstHandForce()
Definition: samplerobot_auto_balancer.py:208
samplerobot_auto_balancer.saveLogForCheckParameter
def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-auto-balancer-check-param")
Definition: samplerobot_auto_balancer.py:52
samplerobot_auto_balancer.checkParameterFromLog
def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-auto-balancer-check-param", save_log=True, rtc_name="SampleRobot(Robot)0")
Definition: samplerobot_auto_balancer.py:55
samplerobot_auto_balancer.demoGaitGeneratorEmergencyStop
def demoGaitGeneratorEmergencyStop()
Definition: samplerobot_auto_balancer.py:434
samplerobot_auto_balancer.demoGaitGeneratorGoVelocity
def demoGaitGeneratorGoVelocity()
Definition: samplerobot_auto_balancer.py:319
samplerobot_auto_balancer.demoAutoBalancerSetParam
def demoAutoBalancerSetParam()
Definition: samplerobot_auto_balancer.py:166
samplerobot_auto_balancer.demoGaitGeneratorChangePoseWhileWalking
def demoGaitGeneratorChangePoseWhileWalking()
Definition: samplerobot_auto_balancer.py:348
samplerobot_auto_balancer.demoStandingPosResetting
def demoStandingPosResetting()
Definition: samplerobot_auto_balancer.py:765
samplerobot_auto_balancer.demoGaitGeneratorOverwriteFootstepsBase
def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx=1, init_fs=False)
Definition: samplerobot_auto_balancer.py:512
samplerobot_auto_balancer.checkActualBaseAttitude
def checkActualBaseAttitude(ref_rpy=None, thre=0.1)
Definition: samplerobot_auto_balancer.py:60
samplerobot_auto_balancer.demoGaitGeneratorChangeStrideLimitationType
def demoGaitGeneratorChangeStrideLimitationType()
Definition: samplerobot_auto_balancer.py:735
samplerobot_auto_balancer.calcVelListFromPosList
def calcVelListFromPosList(pos_list, dt)
Definition: samplerobot_auto_balancer.py:105
samplerobot_auto_balancer.calcDiffFootMidCoords
def calcDiffFootMidCoords(prev_dst_foot_midcoords)
Definition: samplerobot_auto_balancer.py:83
samplerobot_auto_balancer.demoAutoBalancerFixFeetHands
def demoAutoBalancerFixFeetHands()
Definition: samplerobot_auto_balancer.py:149
samplerobot_auto_balancer.demoGaitGeneratorStopStartSyncCheck
def demoGaitGeneratorStopStartSyncCheck()
Definition: samplerobot_auto_balancer.py:408
samplerobot_auto_balancer.demoAutoBalancerBalanceWithArms
def demoAutoBalancerBalanceWithArms()
Definition: samplerobot_auto_balancer.py:224
samplerobot_auto_balancer.demoGaitGeneratorGrasplessManipMode
def demoGaitGeneratorGrasplessManipMode()
Definition: samplerobot_auto_balancer.py:640
samplerobot_auto_balancer.Quaternion2RotMatrixZ
def Quaternion2RotMatrixZ(q)
Definition: samplerobot_auto_balancer.py:76
samplerobot_auto_balancer.demoGaitGeneratorGoPosOverwrite
def demoGaitGeneratorGoPosOverwrite()
Definition: samplerobot_auto_balancer.py:613
samplerobot_auto_balancer.Quaternion2Angle
def Quaternion2Angle(q)
Definition: samplerobot_auto_balancer.py:71

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