test_hironx_target.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
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         # Til here initializing.
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)  # just to make sure
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         # Making sure if reached here. If any error occurred. If not reached
00049         # assert false should be returned earlier.
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)  # goInit (not factory setting) pose
00263         init_r = quaternion_from_euler(3.073, -1.57002, -3.073)  # goInit (not factory setting) pose
00264         roll_l_post = quaternion_from_euler(-1.502, -1.5700, 3.073)  # dr=math.pi / 2 from init pose
00265         roll_r_post = quaternion_from_euler(-1.639, -1.5700, -3.073)  # dr=math.pi / 2 from init pose
00266         pitch_l_post = quaternion_from_euler(-0.000, -0.787, -4.924e-05)  # dp=math.pi / 4 from init pose
00267         pitch_r_post = quaternion_from_euler(0.000, -0.787, 4.827e-05)  # dp=math.pi / 4 from init pose
00268         yaw_l_post = quaternion_from_euler(-1.573, -4.205e-05, 1.571)  # dw=math.pi / 2 from init pose
00269         yaw_r_post = quaternion_from_euler(-1.573, -0.000, 1.571)  # dw=math.pi / 2 from init pose
00270 
00271         # roll motion
00272         self.robot.goInitial(2)
00273         for i in range(0, 5):  # Repeat the same movement 5 times
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         # pitch motion
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         # yaw motion
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)

hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37