00001
00002
00003
00004 import math
00005 import numpy
00006 import re
00007
00008 from tf.transformations import quaternion_from_euler
00009
00010 from test_hironx import euler_from_matrix, TestHiro
00011
00012 PKG = 'hironx_ros_bridge'
00013 RTM_JOINTGRP_LEFT_ARM = 'larm'
00014 RTM_JOINTGRP_RIGHT_ARM = 'rarm'
00015
00016
00017 class TestHiroTarget(TestHiro):
00018
00019 def testSetTargetPoseBothArm(self):
00020 tm = 10
00021 self.robot.goInitial()
00022 posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
00023 posl2 = self.robot.getCurrentPosition('LARM_JOINT5')
00024 posr1 = self.robot.getCurrentPosition('RARM_JOINT5')
00025 posr2 = self.robot.getCurrentPosition('RARM_JOINT5')
00026 rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
00027 rpyr1 = self.robot.getCurrentRPY('RARM_JOINT5')
00028 posr1[0] += 0.05
00029 posr2[2] += 0.08
00030 posl1[0] -= 0.09
00031 posl2[2] -= 0.07
00032
00033
00034 if not self.robot.setTargetPose(RTM_JOINTGRP_LEFT_ARM, posl1, rpyl1, tm):
00035 assert(False)
00036 if not self.robot.setTargetPose(RTM_JOINTGRP_RIGHT_ARM, posr1, rpyr1, tm):
00037 assert(False)
00038 print('Before waitInterpolationOfGroup(larm) begins.')
00039 self.robot.waitInterpolationOfGroup(RTM_JOINTGRP_LEFT_ARM)
00040 print('waitInterpolationOfGroup(larm) returned.')
00041 self.robot.waitInterpolationOfGroup(RTM_JOINTGRP_RIGHT_ARM)
00042 print('waitInterpolationOfGroup(rarm) returned.')
00043 if not self.robot.setTargetPose(RTM_JOINTGRP_LEFT_ARM, posl2, rpyl1, tm):
00044 assert(False)
00045 if not self.robot.setTargetPose(RTM_JOINTGRP_RIGHT_ARM, posr2, rpyr1, tm):
00046 assert(False)
00047
00048
00049
00050 assert(True)
00051
00052 def testGetReferencePose(self):
00053 def print_pose(msg, pose):
00054 print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00055 self.robot.goInitial()
00056 posel1 = self.robot.getReferencePose('LARM_JOINT5')
00057 poser1 = self.robot.getReferencePose('RARM_JOINT5')
00058 try:
00059 posel2 = self.robot.getReferencePose('LARM_JOINT5:WAIST')
00060 poser2 = self.robot.getReferencePose('RARM_JOINT5:WAIST')
00061 except RuntimeError as e:
00062 if re.match(r'frame_name \(.+\) is not supported', e.message):
00063 print(e.message + "...this is expected so pass the test")
00064 return True
00065 elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00066 print("target version is " + self.robot.fk.ref.get_component_profile().version)
00067 print(e.message + "...this is expected so pass the test")
00068 return True
00069 else:
00070 raise RuntimeError(e.message)
00071 print_pose("robot.getReferencePose('LARM_JOINT5')", posel1);
00072 print_pose("robot.getReferencePose('RARM_JOINT5')", poser1);
00073 print_pose("robot.getReferencePose('LARM_JOINT5:WAIST')", posel2);
00074 print_pose("robot.getReferencePose('RARM_JOINT5:WAIST')", poser2);
00075 numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
00076 numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
00077
00078 posel1 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
00079 poser1 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
00080 print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel1);
00081 print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser1);
00082 self.robot.setJointAnglesOfGroup('torso', [45], 1)
00083 self.robot.waitInterpolationOfGroup('torso')
00084 posel2 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
00085 poser2 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
00086 print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel2);
00087 print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser2);
00088 numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
00089 numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
00090
00091 self.robot.setJointAnglesOfGroup('torso', [0], 1)
00092 self.robot.waitInterpolationOfGroup('torso')
00093 pos1 = self.robot.getReferencePosition('LARM_JOINT5')
00094 rot1 = self.robot.getReferenceRotation('LARM_JOINT5')
00095 rpy1 = self.robot.getReferenceRPY('LARM_JOINT5')
00096
00097 self.robot.setJointAnglesOfGroup('torso', [0], 1)
00098 self.robot.waitInterpolationOfGroup('torso')
00099 pos2 = self.robot.getReferencePosition('LARM_JOINT5', 'CHEST_JOINT0')
00100 rot2 = self.robot.getReferenceRotation('LARM_JOINT5', 'CHEST_JOINT0')
00101 rpy2 = self.robot.getReferenceRPY('LARM_JOINT5', 'CHEST_JOINT0')
00102 numpy.testing.assert_array_almost_equal(numpy.array(pos1), numpy.array(pos2), decimal=2)
00103 numpy.testing.assert_array_almost_equal(numpy.array(rot1), numpy.array(rot2), decimal=2)
00104 numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
00105
00106 def testGetCurrentPose(self):
00107 def print_pose(msg, pose):
00108 print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00109 self.robot.goInitial()
00110 posel1 = self.robot.getCurrentPose('LARM_JOINT5')
00111 poser1 = self.robot.getCurrentPose('RARM_JOINT5')
00112 try:
00113 posel2 = self.robot.getCurrentPose('LARM_JOINT5:WAIST')
00114 poser2 = self.robot.getCurrentPose('RARM_JOINT5:WAIST')
00115 except RuntimeError as e:
00116 if re.match(r'frame_name \(.+\) is not supported', e.message):
00117 print(e.message + "...this is expected so pass the test")
00118 return True
00119 elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00120 print("target version is " + self.robot.fk.ref.get_component_profile().version)
00121 print(e.message + "...this is expected so pass the test")
00122 return True
00123 else:
00124 raise RuntimeError(e.message)
00125 print_pose("robot.getCurrentPose('LARM_JOINT5')", posel1);
00126 print_pose("robot.getCurrentPose('RARM_JOINT5')", poser1);
00127 print_pose("robot.getCurrentPose('LARM_JOINT5:WAIST')", posel2);
00128 print_pose("robot.getCurrentPose('RARM_JOINT5:WAIST')", poser2);
00129 numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
00130 numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
00131
00132 posel1 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
00133 poser1 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
00134 print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel1);
00135 print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser1);
00136 self.robot.setJointAnglesOfGroup('torso', [45], 1)
00137 self.robot.waitInterpolationOfGroup('torso')
00138 posel2 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
00139 poser2 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
00140 print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel2);
00141 print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser2);
00142 numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
00143 numpy.testing.assert_array_almost_equal(numpy.array(poser1), numpy.array(poser2), decimal=2)
00144
00145 self.robot.setJointAnglesOfGroup('torso', [0], 1)
00146 self.robot.waitInterpolationOfGroup('torso')
00147 pos1 = self.robot.getCurrentPosition('LARM_JOINT5')
00148 rot1 = self.robot.getCurrentRotation('LARM_JOINT5')
00149 rpy1 = self.robot.getCurrentRPY('LARM_JOINT5')
00150
00151 self.robot.setJointAnglesOfGroup('torso', [0], 1)
00152 self.robot.waitInterpolationOfGroup('torso')
00153 pos2 = self.robot.getCurrentPosition('LARM_JOINT5', 'CHEST_JOINT0')
00154 rot2 = self.robot.getCurrentRotation('LARM_JOINT5', 'CHEST_JOINT0')
00155 try:
00156 rpy2 = self.robot.getCurrentRPY('LARM_JOINT5', 'CHEST_JOINT0')
00157 except RuntimeError as e:
00158 if re.match(r'frame_name \(.+\) is not supported', e.message):
00159 print(e.message + "...this is expected so pass the test")
00160 elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00161 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00162 print(e.message + "...this is expected so pass the test")
00163 return True
00164 else:
00165 raise RuntimeError(e.message)
00166 numpy.testing.assert_array_almost_equal(numpy.array(pos1), numpy.array(pos2), decimal=2)
00167 numpy.testing.assert_array_almost_equal(numpy.array(rot1), numpy.array(rot2), decimal=2)
00168 numpy.testing.assert_array_almost_equal(numpy.array(rpy1), numpy.array(rpy2), decimal=2)
00169
00170 def testGetterByFrame(self):
00171 def print_pose(msg, pose):
00172 print msg, (pose[3], pose[7], pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00173
00174 self.robot.goInitial()
00175
00176 posel1 = self.robot.getCurrentPose('LARM_JOINT5')
00177 try:
00178 posel2 = self.robot.getCurrentPose('LARM_JOINT5', 'WAIST')
00179 except RuntimeError as e:
00180 if re.match(r'frame_name \(.+\) is not supported', e.message):
00181 print(e.message + "...this is expected so pass the test")
00182 elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00183 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00184 print(e.message + "...this is expected so pass the test")
00185 return True
00186 else:
00187 raise RuntimeError(e.message)
00188 numpy.testing.assert_array_almost_equal(numpy.array(posel1), numpy.array(posel2), decimal=2)
00189
00190 print_pose("robot.getCurrentPose(LARM_JOINT5:DEFAULT)", posel1)
00191 print_pose("robot.getCurrentPose(LARM_JOINT5:WAIST)", posel2)
00192
00193 posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
00194 posl2 = self.robot.getCurrentPosition('LARM_JOINT5', 'WAIST')
00195 numpy.testing.assert_array_almost_equal(numpy.array(posl1), numpy.array(posl2), decimal=2)
00196
00197 print "robot.getCurrentPosition(LARM_JOINT5:DEFAULT)", posl1
00198 print "robot.getCurrentPosition(LARM_JOINT5:WAIST)", posl2
00199
00200 rotl1 = self.robot.getCurrentRotation('LARM_JOINT5')
00201 rotl2 = self.robot.getCurrentRotation('LARM_JOINT5', 'WAIST')
00202 numpy.testing.assert_array_almost_equal(numpy.array(rotl1), numpy.array(rotl2), decimal=2)
00203
00204 print "robot.getCurrentRotation(LARM_JOINT5:DEFAULT)", rotl1
00205 print "robot.getCurrentRotation(LARM_JOINT5:WAIST)", rotl2
00206
00207 rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
00208 try:
00209 rpyl2 = self.robot.getCurrentRPY('LARM_JOINT5', 'WAIST')
00210 except RuntimeError as e:
00211 if re.match(r'frame_name \(.+\) is not supported', e.message):
00212 print(e.message + "...this is expected so pass the test")
00213 elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00214 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00215 print(e.message + "...this is expected so pass the test")
00216 return True
00217 else:
00218 raise RuntimeError(e.message)
00219 numpy.testing.assert_array_almost_equal(numpy.array(rpyl1), numpy.array(rpyl2), decimal=2)
00220
00221 print "robot.getCurrentRPY(LARM_JOINT5:DEFAULT)", rpyl1
00222 print "robot.getCurrentRPY(LARM_JOINT5:WAIST)", rpyl2
00223
00224 ref_posel1 = self.robot.getReferencePose('LARM_JOINT5')
00225 ref_posel2 = self.robot.getReferencePose('LARM_JOINT5', 'WAIST')
00226 numpy.testing.assert_array_almost_equal(numpy.array(ref_posel1), numpy.array(ref_posel2), decimal=2)
00227
00228 print "robot.getReferencePose(LARM_JOINT5:DEFAULT)", ref_posel1
00229 print "robot.getReferencePose(LARM_JOINT5:WAIST)", ref_posel2
00230
00231 ref_posl1 = self.robot.getReferencePosition('LARM_JOINT5')
00232 ref_posl2 = self.robot.getReferencePosition('LARM_JOINT5', 'WAIST')
00233 numpy.testing.assert_array_almost_equal(numpy.array(ref_posl1), numpy.array(ref_posl2), decimal=2)
00234
00235 print "robot.getReferencePosition(LARM_JOINT5:DEFAULT)", ref_posl1
00236 print "robot.getReferencePosition(LARM_JOINT5:WAIST)", ref_posl2
00237
00238 ref_rotl1 = self.robot.getReferenceRotation('LARM_JOINT5')
00239 ref_rotl2 = self.robot.getReferenceRotation('LARM_JOINT5', 'WAIST')
00240 numpy.testing.assert_array_almost_equal(numpy.array(ref_rotl1), numpy.array(ref_rotl2), decimal=2)
00241
00242 print "robot.getReferenceRotation(LARM_JOINT5:DEFAULT)", ref_rotl1
00243 print "robot.getReferenceRotation(LARM_JOINT5:WAIST)", ref_rotl2
00244
00245 ref_rpyl1 = self.robot.getReferenceRPY('LARM_JOINT5')
00246 ref_rpyl2 = self.robot.getReferenceRPY('LARM_JOINT5', 'WAIST')
00247 numpy.testing.assert_array_almost_equal(numpy.array(ref_rpyl1), numpy.array(ref_rpyl2), decimal=2)
00248
00249 print "robot.getReferenceRPY(LARM_JOINT5:DEFAULT)", ref_rpyl1
00250 print "robot.getReferenceRPY(LARM_JOINT5:WAIST)", ref_rpyl2
00251
00252 def test_setTargetPoseRelative_rpy(self):
00253 '''
00254 Test if with setTargetPoseRelative with RPY values the arm pose becomes as intended.
00255 Contributed by Naoki Fuse (Daido Steel).
00256 '''
00257
00258 print "goInitial", self.robot.getCurrentRPY('RARM_JOINT5'), self.robot.getCurrentRPY('LARM_JOINT5')
00259 l_eef = 'LARM_JOINT5'
00260 r_eef = 'RARM_JOINT5'
00261
00262 init_l = quaternion_from_euler(-3.073, -1.57002, 3.073)
00263 init_r = quaternion_from_euler(3.073, -1.57002, -3.073)
00264 roll_l_post = quaternion_from_euler(-1.502, -1.5700, 3.073)
00265 roll_r_post = quaternion_from_euler(-1.639, -1.5700, -3.073)
00266 pitch_l_post = quaternion_from_euler(-0.000, -0.787, -4.924e-05)
00267 pitch_r_post = quaternion_from_euler(0.000, -0.787, 4.827e-05)
00268 yaw_l_post = quaternion_from_euler(-1.573, -4.205e-05, 1.571)
00269 yaw_r_post = quaternion_from_euler(-1.573, -0.000, 1.571)
00270
00271
00272 self.robot.goInitial(2)
00273 for i in range(0, 5):
00274 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=math.pi / 2, tm=0.5, wait=False)
00275 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=math.pi / 2, tm=0.5, wait=True)
00276 roll_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
00277 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])
00278 roll_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
00279 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])
00280 numpy.testing.assert_array_almost_equal(roll_l_post, roll_l_post_now, decimal=2)
00281 numpy.testing.assert_array_almost_equal(roll_r_post, roll_r_post_now, decimal=2)
00282 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=False)
00283 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=True)
00284 init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
00285 init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
00286 init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
00287 init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
00288 numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
00289 numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
00290
00291
00292 self.robot.goInitial(2)
00293 for i in range(0, 5):
00294 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=math.pi / 4, tm=0.5, wait=False)
00295 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=math.pi / 4, tm=0.5, wait=True)
00296 pitch_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
00297 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])
00298 pitch_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
00299 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])
00300 numpy.testing.assert_array_almost_equal(pitch_l_post, pitch_l_post_now, decimal=2)
00301 numpy.testing.assert_array_almost_equal(pitch_r_post, pitch_r_post_now, decimal=2)
00302 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=False)
00303 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=True)
00304 init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
00305 init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
00306 init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
00307 init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
00308 numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
00309 numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
00310
00311
00312 self.robot.goInitial(2)
00313 for i in range(0, 5):
00314 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=math.pi / 2, tm=0.5, wait=False)
00315 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=math.pi / 2, tm=0.5, wait=True)
00316 yaw_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
00317 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])
00318 yaw_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
00319 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])
00320 numpy.testing.assert_array_almost_equal(yaw_l_post, yaw_l_post_now, decimal=2)
00321 numpy.testing.assert_array_almost_equal(yaw_r_post, yaw_r_post_now, decimal=2)
00322 self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=-math.pi / 2, tm=0.5, wait=False)
00323 self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=-math.pi / 2, tm=0.5, wait=True)
00324 init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
00325 init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
00326 init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
00327 init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
00328 numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
00329 numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
00330
00331 def test_get_geometry_methods_noarg(self):
00332 '''
00333 @summary: What we call "geometry_methods" are supposed to raise
00334 RuntimeError in a normal condition.
00335
00336 geometry_methods are [
00337 'getCurrentPose', 'getCurrentPosition',
00338 'getCurrentRPY', 'getCurrentRPYRotation',
00339 'getReferencePose', 'getReferencePosition',
00340 'getReferenceRotation', 'getReferenceRPY']
00341 '''
00342 self.assertRaises(RuntimeError, lambda: self.robot.getCurrentPose())
00343 self.assertRaises(RuntimeError, lambda: self.robot.getReferencePose())
00344
00345 if __name__ == '__main__':
00346 import rostest
00347 rostest.rosrun(PKG, 'test_hronx_target', TestHiroTarget)