test_hironx_limb.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 from distutils.version import StrictVersion
5 
6 from test_hironx import *
7 
8 class TestHiroLimb(TestHiro):
9 
10  def limbbody_init (self):
11  self.filenames = []
12  self.robot.setSelfGroups()
13  self.filename_base = tempfile.mkstemp()[1]
14 
15  def test_goInitial(self):
16  self.limbbody_init()
17  self.robot.goInitial()
18 
19 # def test_goOffPose(self):
20 # self.limbbody_init()
21 # self.robot.goOffPose()
22 
23  def test_rarm_setJointAngles_Wait (self):
24  self.limbbody_init()
25  # move to initiali position
26  self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
27  self.robot.waitInterpolationOfGroup("rarm")
28  self.robot.clearLog()
29 
30  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
31  self.robot.waitInterpolationOfGroup("rarm")
32  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
33  self.robot.waitInterpolationOfGroup("rarm")
34  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
35  self.robot.waitInterpolationOfGroup("rarm")
36 
37  filename = self.filename_base + "-wait"
38  self.robot.saveLog(filename)
39  q_filename = filename+"."+self.robot.rh.name()+"_q"
40  data = self.load_log_data(q_filename)
41 
42  print "check setJointAnglesOfGroup(wait=True)"
43  self.check_log_data(data, 6, 15.0, -140.0, -100.0)
44  return True
45 
46  def test_rarm_setJointAngles_NoWait (self):
47  self.limbbody_init()
48  clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
49  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
50  self.robot.waitInterpolationOfGroup("rarm")
51  for i in range(len(clear_time)):
52  self.robot.clearLog()
53 
54  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
55  self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);# time.sleep(clear_time[i]);
56  self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
57  self.robot.waitInterpolationOfGroup("rarm")
58 
59  filename = self.filename_base + "-no-wait-"+str(clear_time[i])
60  self.robot.saveLog(filename)
61  q_filename = filename+"."+self.robot.rh.name()+"_q"
62  data = self.load_log_data(q_filename)
63 
64  print "check setJointAnglesOfGroup(wait=True) "+str(clear_time[i])
65  self.check_log_data(data, 6, (10.0 - (5 - clear_time[i])), [(-140+i*40/len(clear_time)),20], -100.0)
66 
67 
68  def test_rarm_setJointAngles_Clear (self):
69  if StrictVersion(self.robot.hrpsys_version) < StrictVersion('315.5.0'):
70  # clearOfGroup is not available until '315.5.0'
71  self.assertTrue(True)
72  return
73 
74  self.limbbody_init()
75  # check if clear stops interpolation
76  clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
77  for i in range(len(clear_time)):
78  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
79  self.robot.waitInterpolationOfGroup("rarm")
80  self.robot.clearLog()
81 
82  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
83  self.robot.waitInterpolationOfGroup("rarm")
84  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
85  self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);# time.sleep(clear_time[i]);
86  self.robot.clearOfGroup("rarm")
87  self.robot.waitInterpolationOfGroup("rarm")
88 
89  filename = self.filename_base + "-clear-"+str(clear_time[i])
90  self.robot.saveLog(filename)
91  q_filename = filename+"."+self.robot.rh.name()+"_q"
92  data = self.load_log_data(q_filename)
93 
94  print "check setJointAnglesOfGroup(clear) "+str(clear_time[i])
95  self.check_log_data(data, 6, (5 + clear_time[i]), [(-140+i*40/len(clear_time)),20], -100.0)
96 
97  def test_rarm_setJointAnglesOfGroup_Override_Acceleration (self):
98  self.limbbody_init()
99  #disconnectPorts(self.robot.rh.port("q"), self.robot.log.port("q"))
100  #connectPorts(self.robot.el.port("q"), self.robot.log.port("q"))
101 
102  #self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157, 45, 0, 0], 3, wait=False);
103  #self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
104  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
105  self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
106  self.robot.waitInterpolationOfGroup("rarm")
107  self.robot.clearLog()
108  #self.robot.log_svc.maxLength(200*30)
109 
110  # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 3, wait=False);
111  # time.sleep(1.5);
112  # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
113  # self.robot.waitInterpolationOfGroup("rarm")
114  # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
115  # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=False);
116  # time.sleep(1.5);
117  # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
118  # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
119  # self.robot.waitInterpolationOfGroup("rarm")
120  # self.robot.waitInterpolationOfGroup("larm")
121  # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.1, wait=False);
122  # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 0.1, wait=False);
123  # self.robot.waitInterpolationOfGroup("rarm")
124  # self.robot.waitInterpolationOfGroup("larm")
125  # self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157, 45, 0, 0], 3, wait=False);
126  # self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
127  # time.sleep(2.5);
128  # for i in range(1,10):
129  # time.sleep(i/30.0)
130  # self.robot.setJointAnglesOfGroup("larm",
131  # [ 0.6, 0, -120,-15.2, 9.4,-3.2]+numpy.random.normal(0,1,6),
132  # 3, wait=False);
133 
134  for i in range(3):
135  self.robot.setJointAnglesOfGroup("rarm",
136  [-0.6, 0, -100, 15.2, 9.4, 3.2]+numpy.random.normal(0,1,6),
137  3, wait=False);
138  self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 1.5, wait=True);# time.sleep(clear_time[i]);
139 
140  #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
141  #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
142  #self.robot.waitInterpolationOfGroup("rarm")
143  #self.robot.waitInterpolationOfGroup("larm")
144  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 3, wait=False);
145  self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 1.5, wait=True);# time.sleep(clear_time[i]);
146  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
147  #time.sleep(1.5)
148  #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=False);
149  #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 0, wait=False);
150  self.robot.waitInterpolationOfGroup("rarm")
151 
152  filename = self.filename_base + "-no-wait2"
153  self.robot.saveLog(filename)
154 
155  # write pdf file
156  q_filename = filename+"."+self.robot.rh.name()+"_q"
157  self.write_all_joint_pdf(q_filename, "rarm_accel_check.pdf")
158 
159  # assertion
160  data = self.load_log_data(q_filename)
161  print "check setJointAnglesOfGroup(Override_acceleratoin)"
162  self.check_log_data(data, 6, 9, -135, -100.0)
163 
164 
165  def test_rarm_setJointAnglesOfGroup_minus(self):
166  self.limbbody_init()
167  self.robot.el_svc.setServoErrorLimit("all", 0.01) # default is 0.18
168 
169  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
170  self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
171  self.robot.waitInterpolationOfGroup("rarm")
172  self.robot.clearLog()
173  self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
174  self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
175  self.robot.waitInterpolationOfGroup("rarm")
176 
177  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], -1, wait=True);
178 
179  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], -1, wait=True);
180  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=True);
181  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=True);
182  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
183  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 0.1, wait=True);
184  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
185  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.05, wait=True);
186  self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
187  #self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
188 
189  filename = self.filename_base + "-minus"
190  self.robot.saveLog(filename)
191  # write pdf file
192  q_filename = filename+"."+self.robot.rh.name()+"_q"
193  self.write_all_joint_pdf(q_filename, "rarm_minus_check.pdf")
194 
195  # assertion
196  data = self.load_log_data(q_filename)
197  print "check setJointAnglesOfGroup(minus)"
198  self.check_log_data(data, 6, 7.19, -140, -120.0, acc_thre = 1.5, tm_thre = 0.3)
199 
200  self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18
201 
202  def test_movejoints_neck_waist(self):
203  '''
204  Move neck and waist joints to the positional limit defined in
205  the user's manual (with an arbitrary safety coefficient)
206  using hrpsys commands.
207 
208  This test is originally made to catch issues like
209  https://github.com/start-jsk/rtmros_hironx/issues/411
210  '''
211  safety_coeffiecient = 0.8
212  durtion_operation = 10
213  # Values claimed by manufacturer
214  max_neck_pitch = 0.63
215  min_neck_pitch = -0.4
216  max_neck_yaw = 1.22
217  min_neck_yaw = -1.22
218  max_waist_yaw = 2.84
219  min_waist_yaw = -2.84
220 
221  self.limbbody_init()
222  # Here multiplied by safety coefficient to avoid the robot tries
223  # the maximum angles, in case this test is used with real robots.
224  self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=max_neck_pitch*safety_coeffiecient, tm=durtion_operation))
225  self.robot.goInitial()
226  self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=min_neck_pitch*safety_coeffiecient, tm=durtion_operation))
227  self.robot.goInitial()
228  self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dw=max_neck_yaw*safety_coeffiecient, tm=durtion_operation))
229  self.robot.goInitial()
230  self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dw=min_neck_yaw*safety_coeffiecient, tm=durtion_operation))
231  self.robot.goInitial()
232  self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=max_waist_yaw*safety_coeffiecient, tm=durtion_operation))
233  self.robot.goInitial()
234  self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=min_waist_yaw*safety_coeffiecient, tm=durtion_operation))
235 
236 if __name__ == '__main__':
237  import rostest
238  rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)
test_hironx_limb.TestHiroLimb.test_rarm_setJointAngles_NoWait
def test_rarm_setJointAngles_NoWait(self)
Definition: test_hironx_limb.py:46
test_hironx.TestHiro.check_log_data
def check_log_data(self, data, idx, tm_data, min_data, max_data, acc_thre=0.06, tm_thre=0.1)
Definition: test_hironx.py:111
test_hironx_limb.TestHiroLimb.test_movejoints_neck_waist
def test_movejoints_neck_waist(self)
Definition: test_hironx_limb.py:202
test_hironx_limb.TestHiroLimb
Definition: test_hironx_limb.py:8
test_hironx_limb.TestHiroLimb.limbbody_init
def limbbody_init(self)
Definition: test_hironx_limb.py:10
test_hironx_limb.TestHiroLimb.test_rarm_setJointAngles_Wait
def test_rarm_setJointAngles_Wait(self)
Definition: test_hironx_limb.py:23
test_hironx_limb.TestHiroLimb.test_rarm_setJointAnglesOfGroup_Override_Acceleration
def test_rarm_setJointAnglesOfGroup_Override_Acceleration(self)
Definition: test_hironx_limb.py:97
test_hironx.TestHiro.assertTrue
def assertTrue(self, a)
Definition: test_hironx.py:226
test_hironx.TestHiro.write_all_joint_pdf
def write_all_joint_pdf(self, name, pdf_name)
Definition: test_hironx.py:175
test_hironx.TestHiro.load_log_data
def load_log_data(self, name)
Definition: test_hironx.py:99
test_hironx_limb.TestHiroLimb.filename_base
filename_base
Definition: test_hironx_limb.py:13
test_hironx.TestHiro
Definition: test_hironx.py:29
test_hironx_limb.TestHiroLimb.test_goInitial
def test_goInitial(self)
Definition: test_hironx_limb.py:15
test_hironx_limb.TestHiroLimb.test_rarm_setJointAnglesOfGroup_minus
def test_rarm_setJointAnglesOfGroup_minus(self)
Definition: test_hironx_limb.py:165
test_hironx_limb.TestHiroLimb.test_rarm_setJointAngles_Clear
def test_rarm_setJointAngles_Clear(self)
Definition: test_hironx_limb.py:68
test_hironx_limb.TestHiroLimb.filenames
filenames
Definition: test_hironx_limb.py:11

hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu May 14 2020 03:52:05