samplerobot_sequence_player.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 from distutils.version import StrictVersion
17 
18 def init ():
19  global hcf, hrpsys_version
20  hcf = HrpsysConfigurator()
21  hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
22  hcf.connectLoggerPort(hcf.sh, 'optionalDataOut') # Just for checking
23  hcf.connectLoggerPort(hcf.el, 'servoStateOut') # Just for checking
24  global reset_pose_doc, move_base_pose_doc, doc
25  # doc for patterns.
26  # torque and wrenches are non-realistic values, just for testing.
27  dof = 29
28  reset_pose_doc = {'pos':[-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-7.778932e-05,-0.378613,-0.00021,0.832039,-0.452564,0.000245,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0],
29  'vel':[0]*dof,
30  'zmp':[-0.00081, 1.712907e-05, -0.66815],
31 # 'gsens':[0,0,0],
32  'waist':[0.000234, 0.000146, 0.66815, -0.000245, -0.000862, 0.000195],
33  'waist_acc':[0]*3,
34  'torque':[0]*dof, # non realistic value
35  'wrenches':[0]*24, # non realistic value
36  'optionaldata':[1,1,0,0,1,1,1,1]
37  }
38  move_base_pose_doc = {'pos':[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],
39  'vel':[0]*dof,
40  'zmp':[0.302518, 0.000153, -0.562325],
41 # 'gsens':[0,0,0],
42  'waist':[-0.092492, -6.260780e-05, 0.6318, -0.000205, 0.348204, 0.000268],
43  'waist_acc':[0]*3,
44  'torque':range(dof), # non realistic value
45  'wrenches':[1]*6+[-2]*6+[3]*6+[-4]*6, # non realistic value
46  'optionaldata':[0,1,0,0,0.1,0.1,0.1,0.1] # non realistic value
47  }
48  hrpsys_version = hcf.seq.ref.get_component_profile().version
49  print("hrpsys_version = %s"%hrpsys_version)
50  hcf.seq_svc.removeJointGroup('larm')
51  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
52  hcf.seq_svc.waitInterpolation();
53 
54 
55 def dumpLoadPatternTestFile (basename, var_doc, tm):
56  for key in var_doc.keys():
57  f=open(basename+"."+key, "w")
58  dumpstr=" ".join(map(str, [tm]+var_doc[key]))
59  f.writelines(dumpstr)
60  f.close()
61 
62 def checkArrayEquality (arr1, arr2, eps=1e-7):
63  return all(map(lambda x,y : abs(x-y)<eps, arr1, arr2))
64 
65 def checkArrayBetween (arr1, arr2, arr3, eps=1e-7):
66  return all(map(lambda x,y,z : (z-y)*(x-y) <= eps, arr1, arr2, arr3))
67 
68 def clearLogForCheckParameter(log_length=1):
69  hcf.setMaxLogLength(log_length)
70  hcf.clearLog()
71 
72 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-sequence-player-check-param"):
73  clearLogForCheckParameter(log_length=1)
74  time.sleep(0.1);hcf.saveLog(log_fname)
75 
76 def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-sequence-player-check-param", save_log=True, rtc_name="sh"):
77  if save_log:
78  saveLogForCheckParameter(log_fname)
79  return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1])
80 
81 def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-check-param"):
82  hcf.saveLog(log_fname)
83  ret = True
84  fp = open(log_fname+'.el_servoStateOut', "r")
85  for l in fp:
86  for s in map(int, l.split()[1:-1]):
87  if s != 7:
88  print l
89  ret = False
90  return ret
91 
92 def checkJointAngles (var_doc, eps=1e-7):
93  if isinstance(var_doc, list):
94  p = var_doc
95  else:
96  p = var_doc['pos']
97  ret = checkArrayEquality(hcf.sh_svc.getCommand().jointRefs, p, eps)
98  print " pos => ", ret
99  assert(ret is True)
100 
101 def checkJointAnglesBetween(from_doc, to_doc, eps=1e-7):
102  p0 = from_doc if isinstance(from_doc, list) else from_doc['pos']
103  p1 = to_doc if isinstance( to_doc, list) else to_doc['pos']
104  ret = checkArrayBetween(p0, hcf.sh_svc.getCommand().jointRefs, p1, eps)
105  print " pos => ", ret
106  assert(ret is True)
107 
108 def checkZmp(var_doc):
109  zmp=hcf.sh_svc.getCommand().zmp
110  ret = checkArrayEquality([zmp[0], zmp[1], zmp[2]], var_doc['zmp'])
111  print " zmp => ", ret
112  assert(ret is True)
113 
114 def checkWaist(var_doc, save_log=True):
115  bpos=checkParameterFromLog("basePosOut", save_log=save_log)
116  brpy=checkParameterFromLog("baseRpyOut", save_log=False)
117  ret = checkArrayEquality([bpos[0], bpos[1], bpos[2], brpy[0], brpy[1], brpy[2]], var_doc['waist'], eps=1e-5)
118  print " waist => ", ret
119  assert(ret is True)
120 
121 def checkTorque (var_doc, save_log=True):
122  ret = checkArrayEquality(checkParameterFromLog("tqOut", save_log=save_log), var_doc['torque'], eps=1e-5)
123  print " torque => ", ret
124  assert(ret is True)
125 
126 def checkWrenches (var_doc, save_log=True):
127  if save_log:
128  saveLogForCheckParameter()
129  ret = checkArrayEquality(reduce(lambda x,y:x+y, map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])), var_doc['wrenches'], eps=1e-5)
130  print " wrenches => ", ret
131  assert(ret is True)
132 
133 def checkOptionalData (var_doc, save_log=True):
134  ret = checkArrayEquality(checkParameterFromLog("optionalDataOut", save_log=save_log), var_doc['optionaldata'], eps=1e-5)
135  print " optionaldata => ", ret
136  assert(ret is True)
137 
138 def checkRobotState (var_doc):
139  checkJointAngles(var_doc)
140  checkZmp(var_doc)
141  checkWaist(var_doc)
142  checkTorque(var_doc, save_log=False)
143  if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'):
144  checkWrenches(var_doc, save_log=False)
145  checkOptionalData(var_doc, save_log=False)
146 
147 # demo functions
148 def demoSetJointAngles():
149  print >> sys.stderr, "1. setJointAngles"
150  hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 1.0);
151  hcf.seq_svc.waitInterpolation();
152  checkJointAngles(move_base_pose_doc)
153  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
154  hcf.seq_svc.waitInterpolation();
155  checkJointAngles(reset_pose_doc)
156  # check override
157  print >> sys.stderr, " check override"
158  hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0);
159  time.sleep(2.5)
160  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
161  hcf.seq_svc.waitInterpolation();
162  checkJointAngles(reset_pose_doc)
163  # check clear
164  if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'):
165  return
166  print >> sys.stderr, " check clear"
167  hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0);
168  time.sleep(2.5)
169  hcf.seq_svc.clearJointAngles()
170  checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
171 
172 def demoSetJointAnglesSequence():
173  print >> sys.stderr, "2. setJointAnglesSequence"
174  hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,1.0]);
175  hcf.seq_svc.waitInterpolation();
176  checkJointAngles(move_base_pose_doc)
177  hcf.seq_svc.setJointAnglesSequence([reset_pose_doc['pos']], [1.0]);
178  hcf.seq_svc.waitInterpolation();
179  checkJointAngles(reset_pose_doc)
180  # check override
181  print >> sys.stderr, " check override"
182  hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0])
183  time.sleep(3.5)
184  hcf.seq_svc.setJointAnglesSequence([reset_pose_doc['pos'],move_base_pose_doc['pos'],reset_pose_doc['pos']], [1.0,1.0,1.0]);
185  hcf.seq_svc.waitInterpolation();
186  checkJointAngles(reset_pose_doc)
187  # check clear
188  print >> sys.stderr, " check clear"
189  hcf.seq_svc.setJointAnglesSequence([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']], [1.0,1.0,5.0])
190  time.sleep(3.5)
191  hcf.seq_svc.clearJointAngles()
192  checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
193 
194 def demoSetJointAngle():
195  print >> sys.stderr, "3. setJointAngle"
196  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
197  hcf.seq_svc.waitInterpolation();
198  hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 1.0);
199  hcf.seq_svc.waitInterpolation();
200  p = list(reset_pose_doc['pos']) # copy object
201  p[27] = 10*3.14159/180.0
202  checkJointAngles(p)
203  hcf.seq_svc.setJointAngle("WAIST_R", 0*3.14159/180.0, 1.0);
204  hcf.seq_svc.waitInterpolation();
205  checkJointAngles(reset_pose_doc)
206  # # check override
207  # print " check override"
208  # hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
209  # hcf.seq_svc.waitInterpolation();
210  # hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 5.0);
211  # time.sleep(2.5)
212  # hcf.seq_svc.setJointAngle("WAIST_R", 0*3.14159/180.0, 5.0);
213  # hcf.seq_svc.waitInterpolation();
214  # checkJointAngles(reset_pose_doc)
215  # # check clear
216  # print " check clear"
217  # hcf.seq_svc.setJointAngle("WAIST_R", 10*3.14159/180.0, 5.0);
218  # time.sleep(2.5)
219  # hcf.seq_svc.clearJointAngles()
220  # checkJointAnglesBetween(reset_pose_doc,p)
221 
222 def demoLoadPattern():
223  print >> sys.stderr, "4. loadPattern"
224  # dump pattern doc as loadPattern file
225  dumpLoadPatternTestFile("/tmp/test-samplerobot-move-base-pose", move_base_pose_doc, 2.0);
226  dumpLoadPatternTestFile("/tmp/test-samplerobot-reset-pose", reset_pose_doc, 2.0);
227  # execute loadPattern and check final values
228  hcf.seq_svc.loadPattern("/tmp/test-samplerobot-move-base-pose", 1.0)
229  hcf.seq_svc.waitInterpolation()
230  checkRobotState(move_base_pose_doc)
231  hcf.seq_svc.loadPattern("/tmp/test-samplerobot-reset-pose", 1.0)
232  hcf.seq_svc.waitInterpolation()
233  checkRobotState(reset_pose_doc)
234 
235 def demoSetZmp ():
236  print >> sys.stderr, "5. setZmp"
237  hcf.seq_svc.setZmp(move_base_pose_doc['zmp'], 1.0);
238  hcf.seq_svc.waitInterpolation();
239  checkZmp(move_base_pose_doc)
240  hcf.seq_svc.setZmp(reset_pose_doc['zmp'], 1.0);
241  hcf.seq_svc.waitInterpolation();
242  checkZmp(reset_pose_doc)
243 
244 def demoSetBasePosRpy ():
245  print >> sys.stderr, "6. setBasePos and setBaseRpy"
246  hcf.seq_svc.setBasePos(move_base_pose_doc['waist'][0:3], 1.0);
247  hcf.seq_svc.setBaseRpy(move_base_pose_doc['waist'][3:6], 1.0);
248  hcf.seq_svc.waitInterpolation();
249  checkWaist(move_base_pose_doc)
250  hcf.seq_svc.setBasePos(reset_pose_doc['waist'][0:3], 1.0);
251  hcf.seq_svc.setBaseRpy(reset_pose_doc['waist'][3:6], 1.0);
252  hcf.seq_svc.waitInterpolation();
253  checkWaist(reset_pose_doc)
254 
255 def demoSetWrenches ():
256  print >> sys.stderr, "7. setWrenches"
257  hcf.seq_svc.setWrenches(move_base_pose_doc['wrenches'], 1.0);
258  hcf.seq_svc.waitInterpolation();
259  checkWrenches(move_base_pose_doc)
260  hcf.seq_svc.setWrenches(reset_pose_doc['wrenches'], 1.0);
261  hcf.seq_svc.waitInterpolation();
262  checkWrenches(reset_pose_doc)
263 
264 def demoSetJointAnglesOfGroup():
265  print >> sys.stderr, "8. setJointAnglesOfGroup"
266  hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
267  larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
268  larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0]
269  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
270  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 1.0);
271  hcf.seq_svc.waitInterpolationOfGroup('larm');
272  p0 = list(reset_pose_doc['pos']) # copy
273  for i in range(len(larm_pos0)):
274  p0[i+19] = larm_pos0[i]
275  checkJointAngles(p0)
276  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
277  hcf.seq_svc.waitInterpolationOfGroup('larm');
278  p1 = list(reset_pose_doc['pos']) # copy
279  for i in range(len(larm_pos1)):
280  p1[i+19] = larm_pos1[i]
281  checkJointAngles(p1)
282  # check override
283  print >> sys.stderr, " check override"
284  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
285  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
286  time.sleep(2.5)
287  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
288  hcf.seq_svc.waitInterpolationOfGroup('larm');
289  checkJointAngles(p1)
290  # check clear
291  if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'):
292  return
293  print >> sys.stderr, " check clear (clearJointAnglesOfGroup)"
294  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
295  time.sleep(2.5)
296  hcf.seq_svc.clearJointAnglesOfGroup('larm')
297  checkJointAnglesBetween(p1, p0)
298 
299  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0);
300  hcf.seq_svc.waitInterpolationOfGroup('larm')
301  checkJointAngles(p1)
302 
303  print >> sys.stderr, " check clear clearOfGroup"
304  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0);
305  time.sleep(2.5)
306  hcf.seq_svc.clearOfGroup('larm', 0.0)
307  checkJointAnglesBetween(p1, p0)
308 
309  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0);
310  hcf.seq_svc.waitInterpolationOfGroup('larm')
311  checkJointAngles(p1)
312 
313 def demoSetJointAnglesSequenceOfGroup():
314  print >> sys.stderr, "9. setJointAnglesOfGroup"
315  hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
316  larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
317  larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0]
318  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
319  hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 1.0]);
320  hcf.seq_svc.waitInterpolationOfGroup('larm');
321  p0 = list(reset_pose_doc['pos']) # copy
322  for i in range(len(larm_pos0)):
323  p0[i+19] = larm_pos0[i]
324  checkJointAngles(p0)
325  hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos1, larm_pos0, larm_pos1], [1.0, 1.0, 1.0]);
326  hcf.seq_svc.waitInterpolationOfGroup('larm');
327  p1 = list(reset_pose_doc['pos']) # copy
328  for i in range(len(larm_pos1)):
329  p1[i+19] = larm_pos1[i]
330  checkJointAngles(p1)
331  # check override
332  print >> sys.stderr, " check override"
333  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
334  hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]);
335  time.sleep(3.5)
336  hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos1, larm_pos0, larm_pos1], [1.0, 1.0, 1.0]);
337  hcf.seq_svc.waitInterpolationOfGroup('larm');
338  checkJointAngles(p1)
339  # check clear
340  print >> sys.stderr, " check clear"
341  hcf.seq_svc.setJointAnglesSequenceOfGroup('larm', [larm_pos0, larm_pos1, larm_pos0], [1.0, 1.0, 5.0]);
342  time.sleep(3.5)
343  hcf.seq_svc.clearJointAnglesOfGroup('larm')
344  checkJointAnglesBetween(p1, p0)
345  hcf.seq_svc.removeJointGroup('larm')
346 
347 def demoSetJointAnglesSequenceFull():
348  print >> sys.stderr, "10. setJointAnglesSequenceFull"
349  hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
350  [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
351  [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
352  [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
353  [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
354  [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
355  [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
356  [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
357  [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
358  [1.0,1.0,1.0]);
359  hcf.waitInterpolation();
360  checkRobotState(move_base_pose_doc)
361  hcf.seq_svc.setJointAnglesSequenceFull([reset_pose_doc['pos']],
362  [reset_pose_doc['vel']],
363  [reset_pose_doc['torque']],
364  [reset_pose_doc['waist'][0:3]],
365  [reset_pose_doc['waist'][3:6]],
366  [reset_pose_doc['waist_acc']],
367  [reset_pose_doc['zmp']],
368  [reset_pose_doc['wrenches']],
369  [reset_pose_doc['optionaldata']],
370  [1.0])
371  hcf.waitInterpolation();
372  checkRobotState(reset_pose_doc)
373  # check override
374  print >> sys.stderr, " check override"
375  hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
376  [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
377  [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
378  [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
379  [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
380  [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
381  [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
382  [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
383  [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
384  [1.0,1.0,5.0]);
385  time.sleep(3.5)
386  hcf.seq_svc.setJointAnglesSequenceFull([reset_pose_doc['pos'],move_base_pose_doc['pos'],reset_pose_doc['pos']],
387  [reset_pose_doc['vel'],move_base_pose_doc['vel'],reset_pose_doc['vel']],
388  [reset_pose_doc['torque'],move_base_pose_doc['torque'],reset_pose_doc['torque']],
389  [reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3]],
390  [reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6]],
391  [reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc']],
392  [reset_pose_doc['zmp'],move_base_pose_doc['zmp'],reset_pose_doc['zmp']],
393  [reset_pose_doc['wrenches'],move_base_pose_doc['wrenches'],reset_pose_doc['wrenches']],
394  [reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata']],
395  [1.0,1.0,1.0]);
396  hcf.waitInterpolation()
397  checkRobotState(reset_pose_doc)
398  # check clear
399  print >> sys.stderr, " check clear"
400  hcf.seq_svc.setJointAnglesSequenceFull([move_base_pose_doc['pos'],reset_pose_doc['pos'],move_base_pose_doc['pos']],
401  [move_base_pose_doc['vel'],reset_pose_doc['vel'],move_base_pose_doc['vel']],
402  [move_base_pose_doc['torque'],reset_pose_doc['torque'],move_base_pose_doc['torque']],
403  [move_base_pose_doc['waist'][0:3],reset_pose_doc['waist'][0:3],move_base_pose_doc['waist'][0:3]],
404  [move_base_pose_doc['waist'][3:6],reset_pose_doc['waist'][3:6],move_base_pose_doc['waist'][3:6]],
405  [move_base_pose_doc['waist_acc'],reset_pose_doc['waist_acc'],move_base_pose_doc['waist_acc']],
406  [move_base_pose_doc['zmp'],reset_pose_doc['zmp'],move_base_pose_doc['zmp']],
407  [move_base_pose_doc['wrenches'],reset_pose_doc['wrenches'],move_base_pose_doc['wrenches']],
408  [move_base_pose_doc['optionaldata'],reset_pose_doc['optionaldata'],move_base_pose_doc['optionaldata']],
409  [1.0,1.0,5.0]);
410  time.sleep(3.5)
411  hcf.seq_svc.clearJointAngles()
412  checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc)
413 
414 def demoSetTargetPose():
415  # reset wait position
416  hcf.seq_svc.setBasePos([0.000000, 0.000000, 0.723500], 1.0);
417  hcf.seq_svc.setBaseRpy([0.000000, 0.000000, 0.000000], 1.0);
418  print >> sys.stderr, "11. setTargetPose"
419  hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R'])
420  larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0]
421  larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.785395, -0.636277, 0.0, 0.0]
422  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
423  hcf.seq_svc.waitInterpolation();
424  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 1.0);
425  hcf.seq_svc.waitInterpolationOfGroup('larm');
426  pos0 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
427  rpy0 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
428  pos0_world = hcf.getCurrentPosition('LARM_WRIST_R')
429  rpy0_world = hcf.getCurrentRPY('LARM_WRIST_R')
430  p0 = list(reset_pose_doc['pos']) # copy
431  for i in range(len(larm_pos0)):
432  p0[i+19] = larm_pos0[i]
433  checkJointAngles(p0)
434 
435  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
436  hcf.seq_svc.waitInterpolationOfGroup('larm');
437  pos1 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
438  rpy1 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
439  pos1_world = hcf.getCurrentPosition('LARM_WRIST_R')
440  rpy1_world = hcf.getCurrentRPY('LARM_WRIST_R')
441  p1 = list(reset_pose_doc['pos']) # copy
442  for i in range(len(larm_pos1)):
443  p1[i+19] = larm_pos1[i]
444  checkJointAngles(p1)
445 
446  print >> sys.stderr, " check setTargetPose"
447  hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0)
448  hcf.seq_svc.waitInterpolationOfGroup('larm');
449  print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)
450  checkJointAngles(p0, 0.01)
451 
452  clearLogForCheckParameter(5000)
453  hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0)
454  hcf.seq_svc.waitInterpolationOfGroup('larm');
455  print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
456  checkJointAngles(p1, 0.01)
457  assert(checkServoStateFromLog() is True)
458 
459  hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
460  hcf.seq_svc.waitInterpolationOfGroup('larm');
461 
462  print >> sys.stderr, " check setTargetPose without giving reference frame"
463  hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0)
464  hcf.seq_svc.waitInterpolationOfGroup('larm');
465  print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)
466  checkJointAngles(p0, 0.01)
467 
468  clearLogForCheckParameter(5000)
469  hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0)
470  hcf.seq_svc.waitInterpolationOfGroup('larm');
471  print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
472  checkJointAngles(p1, 0.01)
473  assert(checkServoStateFromLog() is True)
474 
475  print >> sys.stderr, " check clear clearOfGroup"
476  hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0)
477  time.sleep(0.5)
478  hcf.seq_svc.clearOfGroup('larm', 0.0)
479  checkJointAnglesBetween(p0, p1, 0.01)
480 
481  clearLogForCheckParameter(5000)
482  hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0)
483  hcf.seq_svc.waitInterpolationOfGroup('larm')
484  print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
485  checkJointAngles(p1, 0.1)
486  assert(checkServoStateFromLog() is True)
487  hcf.seq_svc.removeJointGroup('larm')
488  hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0);
489  hcf.seq_svc.waitInterpolation();
490 
491 def demo():
492  init()
493  demoSetJointAngles()
494  if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
495  demoSetJointAnglesSequence()
496  demoSetJointAngle()
497  demoLoadPattern()
498  demoSetZmp()
499  demoSetBasePosRpy()
500  if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'):
501  demoSetWrenches()
502  demoSetJointAnglesOfGroup()
503  if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
504  demoSetJointAnglesSequenceOfGroup()
505  demoSetJointAnglesSequenceFull()
506  if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
507  demoSetTargetPose()
508 
509 if __name__ == '__main__':
510  demo()
511 
samplerobot_sequence_player.init
def init()
Definition: samplerobot_sequence_player.py:18
hrpsys_config
samplerobot_sequence_player.checkTorque
def checkTorque(var_doc, save_log=True)
Definition: samplerobot_sequence_player.py:121
samplerobot_sequence_player.checkJointAnglesBetween
def checkJointAnglesBetween(from_doc, to_doc, eps=1e-7)
Definition: samplerobot_sequence_player.py:101
samplerobot_sequence_player.checkOptionalData
def checkOptionalData(var_doc, save_log=True)
Definition: samplerobot_sequence_player.py:133
samplerobot_sequence_player.saveLogForCheckParameter
def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-sequence-player-check-param")
Definition: samplerobot_sequence_player.py:72
samplerobot_sequence_player.demoLoadPattern
def demoLoadPattern()
Definition: samplerobot_sequence_player.py:222
samplerobot_sequence_player.demoSetZmp
def demoSetZmp()
Definition: samplerobot_sequence_player.py:235
samplerobot_sequence_player.demo
def demo()
Definition: samplerobot_sequence_player.py:491
samplerobot_sequence_player.checkArrayEquality
def checkArrayEquality(arr1, arr2, eps=1e-7)
Definition: samplerobot_sequence_player.py:62
samplerobot_sequence_player.checkZmp
def checkZmp(var_doc)
Definition: samplerobot_sequence_player.py:108
samplerobot_sequence_player.checkWaist
def checkWaist(var_doc, save_log=True)
Definition: samplerobot_sequence_player.py:114
samplerobot_sequence_player.checkArrayBetween
def checkArrayBetween(arr1, arr2, arr3, eps=1e-7)
Definition: samplerobot_sequence_player.py:65
samplerobot_sequence_player.demoSetBasePosRpy
def demoSetBasePosRpy()
Definition: samplerobot_sequence_player.py:244
samplerobot_sequence_player.demoSetJointAnglesSequence
def demoSetJointAnglesSequence()
Definition: samplerobot_sequence_player.py:172
samplerobot_sequence_player.checkJointAngles
def checkJointAngles(var_doc, eps=1e-7)
Definition: samplerobot_sequence_player.py:92
samplerobot_sequence_player.demoSetJointAnglesSequenceOfGroup
def demoSetJointAnglesSequenceOfGroup()
Definition: samplerobot_sequence_player.py:313
samplerobot_sequence_player.clearLogForCheckParameter
def clearLogForCheckParameter(log_length=1)
Definition: samplerobot_sequence_player.py:68
samplerobot_sequence_player.checkRobotState
def checkRobotState(var_doc)
Definition: samplerobot_sequence_player.py:138
samplerobot_sequence_player.demoSetJointAngles
def demoSetJointAngles()
Definition: samplerobot_sequence_player.py:148
samplerobot_sequence_player.demoSetWrenches
def demoSetWrenches()
Definition: samplerobot_sequence_player.py:255
samplerobot_sequence_player.demoSetTargetPose
def demoSetTargetPose()
Definition: samplerobot_sequence_player.py:414
samplerobot_sequence_player.demoSetJointAngle
def demoSetJointAngle()
Definition: samplerobot_sequence_player.py:194
samplerobot_sequence_player.checkWrenches
def checkWrenches(var_doc, save_log=True)
Definition: samplerobot_sequence_player.py:126
samplerobot_sequence_player.checkServoStateFromLog
def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-check-param")
Definition: samplerobot_sequence_player.py:81
samplerobot_sequence_player.dumpLoadPatternTestFile
def dumpLoadPatternTestFile(basename, var_doc, tm)
Definition: samplerobot_sequence_player.py:55
samplerobot_sequence_player.demoSetJointAnglesSequenceFull
def demoSetJointAnglesSequenceFull()
Definition: samplerobot_sequence_player.py:347
samplerobot_sequence_player.demoSetJointAnglesOfGroup
def demoSetJointAnglesOfGroup()
Definition: samplerobot_sequence_player.py:264
samplerobot_sequence_player.checkParameterFromLog
def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-sequence-player-check-param", save_log=True, rtc_name="sh")
Definition: samplerobot_sequence_player.py:76

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