test_hironx_target.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 import math
5 import numpy
6 import re
7 
8 from tf.transformations import quaternion_from_euler
9 
10 from test_hironx import euler_from_matrix, TestHiro
11 
12 PKG = 'hironx_ros_bridge'
13 RTM_JOINTGRP_LEFT_ARM = 'larm'
14 RTM_JOINTGRP_RIGHT_ARM = 'rarm'
15 
16 
18 
20  tm = 10
21  self.robot.goInitial()
22  posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
23  posl2 = self.robot.getCurrentPosition('LARM_JOINT5')
24  posr1 = self.robot.getCurrentPosition('RARM_JOINT5')
25  posr2 = self.robot.getCurrentPosition('RARM_JOINT5')
26  rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
27  rpyr1 = self.robot.getCurrentRPY('RARM_JOINT5')
28  posr1[0] += 0.05
29  posr2[2] += 0.08
30  posl1[0] -= 0.09
31  posl2[2] -= 0.07
32  # Til here initializing.
33 
34  if not self.robot.setTargetPose(RTM_JOINTGRP_LEFT_ARM, posl1, rpyl1, tm):
35  assert(False)
36  if not self.robot.setTargetPose(RTM_JOINTGRP_RIGHT_ARM, posr1, rpyr1, tm):
37  assert(False)
38  print('Before waitInterpolationOfGroup(larm) begins.')
39  self.robot.waitInterpolationOfGroup(RTM_JOINTGRP_LEFT_ARM)
40  print('waitInterpolationOfGroup(larm) returned.')
41  self.robot.waitInterpolationOfGroup(RTM_JOINTGRP_RIGHT_ARM) # just to make sure
42  print('waitInterpolationOfGroup(rarm) returned.')
43  if not self.robot.setTargetPose(RTM_JOINTGRP_LEFT_ARM, posl2, rpyl1, tm):
44  assert(False)
45  if not self.robot.setTargetPose(RTM_JOINTGRP_RIGHT_ARM, posr2, rpyr1, tm):
46  assert(False)
47 
48  # Making sure if reached here. If any error occurred. If not reached
49  # assert false should be returned earlier.
50  assert(True)
51 
53  def print_pose(msg, pose):
54  print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
55  self.robot.goInitial()
56  posel1 = self.robot.getReferencePose('LARM_JOINT5')
57  poser1 = self.robot.getReferencePose('RARM_JOINT5')
58  try:
59  posel2 = self.robot.getReferencePose('LARM_JOINT5:WAIST')
60  poser2 = self.robot.getReferencePose('RARM_JOINT5:WAIST')
61  except RuntimeError as e:
62  if re.match(r'frame_name \(.+\) is not supported', e.message):
63  print(e.message + "...this is expected so pass the test")
64  return True
65  elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
66  print("target version is " + self.robot.fk.ref.get_component_profile().version)
67  print(e.message + "...this is expected so pass the test")
68  return True
69  else:
70  raise RuntimeError(e.message)
71  print_pose("robot.getReferencePose('LARM_JOINT5')", posel1);
72  print_pose("robot.getReferencePose('RARM_JOINT5')", poser1);
73  print_pose("robot.getReferencePose('LARM_JOINT5:WAIST')", posel2);
74  print_pose("robot.getReferencePose('RARM_JOINT5:WAIST')", poser2);
75  numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
76  numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
77 
78  posel1 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
79  poser1 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
80  print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel1);
81  print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser1);
82  self.robot.setJointAnglesOfGroup('torso', [45], 1)
83  self.robot.waitInterpolationOfGroup('torso')
84  posel2 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
85  poser2 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
86  print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel2);
87  print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser2);
88  numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
89  numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
90 
91  self.robot.setJointAnglesOfGroup('torso', [0], 1)
92  self.robot.waitInterpolationOfGroup('torso')
93  pos1 = self.robot.getReferencePosition('LARM_JOINT5')
94  rot1 = self.robot.getReferenceRotation('LARM_JOINT5')
95  rpy1 = self.robot.getReferenceRPY('LARM_JOINT5')
96 
97  self.robot.setJointAnglesOfGroup('torso', [0], 1)
98  self.robot.waitInterpolationOfGroup('torso')
99  pos2 = self.robot.getReferencePosition('LARM_JOINT5', 'CHEST_JOINT0')
100  rot2 = self.robot.getReferenceRotation('LARM_JOINT5', 'CHEST_JOINT0')
101  rpy2 = self.robot.getReferenceRPY('LARM_JOINT5', 'CHEST_JOINT0')
102  numpy.testing.assert_array_almost_equal(numpy.array(pos1), numpy.array(pos2), decimal=2)
103  numpy.testing.assert_array_almost_equal(numpy.array(rot1), numpy.array(rot2), decimal=2)
104  numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
105 
107  def print_pose(msg, pose):
108  print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
109  self.robot.goInitial()
110  posel1 = self.robot.getCurrentPose('LARM_JOINT5')
111  poser1 = self.robot.getCurrentPose('RARM_JOINT5')
112  try:
113  posel2 = self.robot.getCurrentPose('LARM_JOINT5:WAIST')
114  poser2 = self.robot.getCurrentPose('RARM_JOINT5:WAIST')
115  except RuntimeError as e:
116  if re.match(r'frame_name \(.+\) is not supported', e.message):
117  print(e.message + "...this is expected so pass the test")
118  return True
119  elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
120  print("target version is " + self.robot.fk.ref.get_component_profile().version)
121  print(e.message + "...this is expected so pass the test")
122  return True
123  else:
124  raise RuntimeError(e.message)
125  print_pose("robot.getCurrentPose('LARM_JOINT5')", posel1);
126  print_pose("robot.getCurrentPose('RARM_JOINT5')", poser1);
127  print_pose("robot.getCurrentPose('LARM_JOINT5:WAIST')", posel2);
128  print_pose("robot.getCurrentPose('RARM_JOINT5:WAIST')", poser2);
129  numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
130  numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
131 
132  posel1 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
133  poser1 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
134  print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel1);
135  print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser1);
136  self.robot.setJointAnglesOfGroup('torso', [45], 1)
137  self.robot.waitInterpolationOfGroup('torso')
138  posel2 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
139  poser2 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
140  print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel2);
141  print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser2);
142  numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
143  numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
144 
145  self.robot.setJointAnglesOfGroup('torso', [0], 1)
146  self.robot.waitInterpolationOfGroup('torso')
147  pos1 = self.robot.getCurrentPosition('LARM_JOINT5')
148  rot1 = self.robot.getCurrentRotation('LARM_JOINT5')
149  rpy1 = self.robot.getCurrentRPY('LARM_JOINT5')
150 
151  self.robot.setJointAnglesOfGroup('torso', [0], 1)
152  self.robot.waitInterpolationOfGroup('torso')
153  pos2 = self.robot.getCurrentPosition('LARM_JOINT5', 'CHEST_JOINT0')
154  rot2 = self.robot.getCurrentRotation('LARM_JOINT5', 'CHEST_JOINT0')
155  try:
156  rpy2 = self.robot.getCurrentRPY('LARM_JOINT5', 'CHEST_JOINT0')
157  except RuntimeError as e:
158  if re.match(r'frame_name \(.+\) is not supported', e.message):
159  print(e.message + "...this is expected so pass the test")
160  elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
161  print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
162  print(e.message + "...this is expected so pass the test")
163  return True
164  else:
165  raise RuntimeError(e.message)
166  numpy.testing.assert_array_almost_equal(numpy.array(pos1), numpy.array(pos2), decimal=2)
167  numpy.testing.assert_array_almost_equal(numpy.array(rot1), numpy.array(rot2), decimal=2)
168  numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
169 
170  def testGetterByFrame(self):
171  def print_pose(msg, pose):
172  print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
173 
174  self.robot.goInitial()
175 
176  posel1 = self.robot.getCurrentPose('LARM_JOINT5')
177  try:
178  posel2 = self.robot.getCurrentPose('LARM_JOINT5', 'WAIST')
179  except RuntimeError as e:
180  if re.match(r'frame_name \(.+\) is not supported', e.message):
181  print(e.message + "...this is expected so pass the test")
182  elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
183  print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
184  print(e.message + "...this is expected so pass the test")
185  return True
186  else:
187  raise RuntimeError(e.message)
188  numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
189 
190  print_pose("robot.getCurrentPose(LARM_JOINT5:DEFAULT)", posel1)
191  print_pose("robot.getCurrentPose(LARM_JOINT5:WAIST)", posel2)
192 
193  posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
194  posl2 = self.robot.getCurrentPosition('LARM_JOINT5', 'WAIST')
195  numpy.testing.assert_array_almost_equal(numpy.array(posl1), numpy.array(posl2), decimal=2)
196 
197  print "robot.getCurrentPosition(LARM_JOINT5:DEFAULT)", posl1
198  print "robot.getCurrentPosition(LARM_JOINT5:WAIST)", posl2
199 
200  rotl1 = self.robot.getCurrentRotation('LARM_JOINT5')
201  rotl2 = self.robot.getCurrentRotation('LARM_JOINT5', 'WAIST')
202  numpy.testing.assert_array_almost_equal(numpy.array(rotl1), numpy.array(rotl2), decimal=2)
203 
204  print "robot.getCurrentRotation(LARM_JOINT5:DEFAULT)", rotl1
205  print "robot.getCurrentRotation(LARM_JOINT5:WAIST)", rotl2
206 
207  rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
208  try:
209  rpyl2 = self.robot.getCurrentRPY('LARM_JOINT5', 'WAIST')
210  except RuntimeError as e:
211  if re.match(r'frame_name \(.+\) is not supported', e.message):
212  print(e.message + "...this is expected so pass the test")
213  elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
214  print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
215  print(e.message + "...this is expected so pass the test")
216  return True
217  else:
218  raise RuntimeError(e.message)
219  numpy.testing.assert_array_almost_equal(numpy.array(rpyl1), numpy.array(rpyl2), decimal=2)
220 
221  print "robot.getCurrentRPY(LARM_JOINT5:DEFAULT)", rpyl1
222  print "robot.getCurrentRPY(LARM_JOINT5:WAIST)", rpyl2
223 
224  ref_posel1 = self.robot.getReferencePose('LARM_JOINT5')
225  ref_posel2 = self.robot.getReferencePose('LARM_JOINT5', 'WAIST')
226  numpy.testing.assert_array_almost_equal(numpy.array(ref_posel1), numpy.array(ref_posel2), decimal=2)
227 
228  print "robot.getReferencePose(LARM_JOINT5:DEFAULT)", ref_posel1
229  print "robot.getReferencePose(LARM_JOINT5:WAIST)", ref_posel2
230 
231  ref_posl1 = self.robot.getReferencePosition('LARM_JOINT5')
232  ref_posl2 = self.robot.getReferencePosition('LARM_JOINT5', 'WAIST')
233  numpy.testing.assert_array_almost_equal(numpy.array(ref_posl1), numpy.array(ref_posl2), decimal=2)
234 
235  print "robot.getReferencePosition(LARM_JOINT5:DEFAULT)", ref_posl1
236  print "robot.getReferencePosition(LARM_JOINT5:WAIST)", ref_posl2
237 
238  ref_rotl1 = self.robot.getReferenceRotation('LARM_JOINT5')
239  ref_rotl2 = self.robot.getReferenceRotation('LARM_JOINT5', 'WAIST')
240  numpy.testing.assert_array_almost_equal(numpy.array(ref_rotl1), numpy.array(ref_rotl2), decimal=2)
241 
242  print "robot.getReferenceRotation(LARM_JOINT5:DEFAULT)", ref_rotl1
243  print "robot.getReferenceRotation(LARM_JOINT5:WAIST)", ref_rotl2
244 
245  ref_rpyl1 = self.robot.getReferenceRPY('LARM_JOINT5')
246  ref_rpyl2 = self.robot.getReferenceRPY('LARM_JOINT5', 'WAIST')
247  numpy.testing.assert_array_almost_equal(numpy.array(ref_rpyl1), numpy.array(ref_rpyl2), decimal=2)
248 
249  print "robot.getReferenceRPY(LARM_JOINT5:DEFAULT)", ref_rpyl1
250  print "robot.getReferenceRPY(LARM_JOINT5:WAIST)", ref_rpyl2
251 
253  '''
254  Test if with setTargetPoseRelative with RPY values the arm pose becomes as intended.
255  Contributed by Naoki Fuse (Daido Steel).
256  '''
257 
258  print "goInitial", self.robot.getCurrentRPY('RARM_JOINT5'), self.robot.getCurrentRPY('LARM_JOINT5')
259  l_eef = 'LARM_JOINT5'
260  r_eef = 'RARM_JOINT5'
261 
262  init_l = quaternion_from_euler(-3.073, -1.57002, 3.073) # goInit (not factory setting) pose
263  init_r = quaternion_from_euler(3.073, -1.57002, -3.073) # goInit (not factory setting) pose
264  roll_l_post = quaternion_from_euler(-1.502, -1.5700, 3.073) # dr=math.pi / 2 from init pose
265  roll_r_post = quaternion_from_euler(-1.639, -1.5700, -3.073) # dr=math.pi / 2 from init pose
266  pitch_l_post = quaternion_from_euler(-0.000, -0.787, -4.924e-05) # dp=math.pi / 4 from init pose
267  pitch_r_post = quaternion_from_euler(0.000, -0.787, 4.827e-05) # dp=math.pi / 4 from init pose
268  yaw_l_post = quaternion_from_euler(-1.573, -4.205e-05, 1.571) # dw=math.pi / 2 from init pose
269  yaw_r_post = quaternion_from_euler(-1.573, -0.000, 1.571) # dw=math.pi / 2 from init pose
270 
271  # roll motion
272  self.robot.goInitial(2)
273  for i in range(0, 5): # Repeat the same movement 5 times
274  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=math.pi / 2, tm=0.5, wait=False)
275  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=math.pi / 2, tm=0.5, wait=True)
276  roll_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
277  roll_l_post_now = quaternion_from_euler(roll_l_post_now_rpy[0], roll_l_post_now_rpy[1], roll_l_post_now_rpy[2])
278  roll_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
279  roll_r_post_now = quaternion_from_euler(roll_r_post_now_rpy[0], roll_r_post_now_rpy[1], roll_r_post_now_rpy[2])
280  numpy.testing.assert_array_almost_equal(roll_l_post, roll_l_post_now, decimal=2)
281  numpy.testing.assert_array_almost_equal(roll_r_post, roll_r_post_now, decimal=2)
282  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=False)
283  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=True)
284  init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
285  init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
286  init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
287  init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
288  numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
289  numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
290 
291  # pitch motion
292  self.robot.goInitial(2)
293  for i in range(0, 5):
294  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=math.pi / 4, tm=0.5, wait=False)
295  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=math.pi / 4, tm=0.5, wait=True)
296  pitch_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
297  pitch_l_post_now = quaternion_from_euler(pitch_l_post_now_rpy[0], pitch_l_post_now_rpy[1], pitch_l_post_now_rpy[2])
298  pitch_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
299  pitch_r_post_now = quaternion_from_euler(pitch_r_post_now_rpy[0], pitch_r_post_now_rpy[1], pitch_r_post_now_rpy[2])
300  numpy.testing.assert_array_almost_equal(pitch_l_post, pitch_l_post_now, decimal=2)
301  numpy.testing.assert_array_almost_equal(pitch_r_post, pitch_r_post_now, decimal=2)
302  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=False)
303  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=True)
304  init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
305  init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
306  init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
307  init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
308  numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
309  numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
310 
311  # yaw motion
312  self.robot.goInitial(2)
313  for i in range(0, 5):
314  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=math.pi / 2, tm=0.5, wait=False)
315  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=math.pi / 2, tm=0.5, wait=True)
316  yaw_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
317  yaw_l_post_now = quaternion_from_euler(yaw_l_post_now_rpy[0], yaw_l_post_now_rpy[1], yaw_l_post_now_rpy[2])
318  yaw_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
319  yaw_r_post_now = quaternion_from_euler(yaw_r_post_now_rpy[0], yaw_r_post_now_rpy[1], yaw_r_post_now_rpy[2])
320  numpy.testing.assert_array_almost_equal(yaw_l_post, yaw_l_post_now, decimal=2)
321  numpy.testing.assert_array_almost_equal(yaw_r_post, yaw_r_post_now, decimal=2)
322  self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=-math.pi / 2, tm=0.5, wait=False)
323  self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=-math.pi / 2, tm=0.5, wait=True)
324  init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
325  init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
326  init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
327  init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
328  numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
329  numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
330 
332  '''
333  @summary: What we call "geometry_methods" are supposed to raise
334  RuntimeError in a normal condition.
335 
336  geometry_methods are [
337  'getCurrentPose', 'getCurrentPosition',
338  'getCurrentRPY', 'getCurrentRPYRotation',
339  'getReferencePose', 'getReferencePosition',
340  'getReferenceRotation', 'getReferenceRPY']
341  '''
342  self.assertRaises(RuntimeError, lambda: self.robot.getCurrentPose())
343  self.assertRaises(RuntimeError, lambda: self.robot.getReferencePose())
344 
345 if __name__ == '__main__':
346  import rostest
347  rostest.rosrun(PKG, 'test_hronx_target', TestHiroTarget)


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