hironx_ros_bridge: test_hironx_target.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 from test_hironx import *
00005 
00006 class TestHiroTarget(TestHiro):
00007 
00008     def testSetTargetPoseBothArm(self):
00009         tm = 10
00010         self.robot.goInitial()
00011         posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
00012         posl2 = self.robot.getCurrentPosition('LARM_JOINT5')
00013         posr1 = self.robot.getCurrentPosition('RARM_JOINT5')
00014         posr2 = self.robot.getCurrentPosition('RARM_JOINT5')
00015         rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
00016         rpyr1 = self.robot.getCurrentRPY('RARM_JOINT5')
00017         posr1[0] += 0.05
00018         posr2[2] += 0.08
00019         posl1[0] -= 0.09
00020         posl2[2] -= 0.07
00021         # Til here initializing.
00022 
00023         if not self.robot.setTargetPose('larm', posl1, rpyl1, tm):
00024             assert(False)
00025         if not self.robot.setTargetPose('rarm', posr1, rpyr1, tm):
00026             assert(False)
00027         print('Before waitInterpolationOfGroup(larm) begins.')
00028         self.robot.waitInterpolationOfGroup('larm')
00029         print('waitInterpolationOfGroup(larm) returned.')
00030         self.robot.waitInterpolationOfGroup('rarm') # just to make sure
00031         print('waitInterpolationOfGroup(rarm) returned.')
00032         if not self.robot.setTargetPose('larm', posl2, rpyl1, tm):
00033             assert(False)
00034         if not self.robot.setTargetPose('rarm', posr2, rpyr1, tm):
00035             assert(False)
00036 
00037         # Making sure if reached here. If any error occurred. If not reached
00038         # assert false should be returned earlier.
00039         assert(True)  
00040 
00041     def testGetReferencePose(self):
00042         def print_pose(msg, pose):
00043             print msg, (pose[3],pose[7],pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00044         self.robot.goInitial()
00045         posel1 = self.robot.getReferencePose('LARM_JOINT5')
00046         poser1 = self.robot.getReferencePose('RARM_JOINT5')
00047         try:
00048             posel2 = self.robot.getReferencePose('LARM_JOINT5:WAIST')
00049             poser2 = self.robot.getReferencePose('RARM_JOINT5:WAIST')
00050         except RuntimeError as e:
00051             if re.match(r'frame_name \(.+\) is not supported', e.message):
00052                 print(e.message + "...this is expected so pass the test")
00053                 return True
00054             elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00055                 print("target version is " + self.robot.fk.ref.get_component_profile().version)
00056                 print(e.message + "...this is expected so pass the test")
00057                 return True
00058             else:
00059                 raise RuntimeError(e.message)
00060         print_pose("robot.getReferencePose('LARM_JOINT5')", posel1);
00061         print_pose("robot.getReferencePose('RARM_JOINT5')", poser1);
00062         print_pose("robot.getReferencePose('LARM_JOINT5:WAIST')", posel2);
00063         print_pose("robot.getReferencePose('RARM_JOINT5:WAIST')", poser2);
00064         numpy.testing.assert_array_almost_equal(numpy.array(posel1),numpy.array(posel2), decimal=3)
00065         numpy.testing.assert_array_almost_equal(numpy.array(poser1),numpy.array(poser2), decimal=3)
00066 
00067         posel1 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
00068         poser1 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
00069         print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel1);
00070         print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser1);
00071         self.robot.setJointAnglesOfGroup('torso', [45], 1)
00072         self.robot.waitInterpolationOfGroup('torso')
00073         posel2 = self.robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')
00074         poser2 = self.robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')
00075         print_pose("robot.getReferencePose('LARM_JOINT5:CHEST_JOINT0')", posel2);
00076         print_pose("robot.getReferencePose('RARM_JOINT5:CHEST_JOINT0')", poser2);
00077         numpy.testing.assert_array_almost_equal(numpy.array(posel1),numpy.array(posel2), decimal=3)
00078         numpy.testing.assert_array_almost_equal(numpy.array(poser1),numpy.array(poser2), decimal=3)
00079 
00080         self.robot.setJointAnglesOfGroup('torso', [0], 1)
00081         self.robot.waitInterpolationOfGroup('torso')
00082         pos1 = self.robot.getReferencePosition('LARM_JOINT5')
00083         rot1 = self.robot.getReferenceRotation('LARM_JOINT5')
00084         rpy1 = self.robot.getReferenceRPY('LARM_JOINT5')
00085 
00086         self.robot.setJointAnglesOfGroup('torso', [0], 1)
00087         self.robot.waitInterpolationOfGroup('torso')
00088         pos2 = self.robot.getReferencePosition('LARM_JOINT5', 'CHEST_JOINT0')
00089         rot2 = self.robot.getReferenceRotation('LARM_JOINT5', 'CHEST_JOINT0')
00090         rpy2 = self.robot.getReferenceRPY('LARM_JOINT5', 'CHEST_JOINT0')
00091         numpy.testing.assert_array_almost_equal(numpy.array(pos1),numpy.array(pos2), decimal=3)
00092         numpy.testing.assert_array_almost_equal(numpy.array(rot1),numpy.array(rot2), decimal=3)
00093         numpy.testing.assert_array_almost_equal(numpy.array(rpy1),numpy.array(rpy2), decimal=3)
00094 
00095 
00096     def testGetCurrentPose(self):
00097         def print_pose(msg, pose):
00098             print msg, (pose[3],pose[7],pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00099         self.robot.goInitial()
00100         posel1 = self.robot.getCurrentPose('LARM_JOINT5')
00101         poser1 = self.robot.getCurrentPose('RARM_JOINT5')
00102         try:
00103             posel2 = self.robot.getCurrentPose('LARM_JOINT5:WAIST')
00104             poser2 = self.robot.getCurrentPose('RARM_JOINT5:WAIST')
00105         except RuntimeError as e:
00106             if re.match(r'frame_name \(.+\) is not supported', e.message):
00107                 print(e.message + "...this is expected so pass the test")
00108                 return True
00109             elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00110                 print("target version is " + self.robot.fk.ref.get_component_profile().version)
00111                 print(e.message + "...this is expected so pass the test")
00112                 return True
00113             else:
00114                 raise RuntimeError(e.message)
00115         print_pose("robot.getCurrentPose('LARM_JOINT5')", posel1);
00116         print_pose("robot.getCurrentPose('RARM_JOINT5')", poser1);
00117         print_pose("robot.getCurrentPose('LARM_JOINT5:WAIST')", posel2);
00118         print_pose("robot.getCurrentPose('RARM_JOINT5:WAIST')", poser2);
00119         numpy.testing.assert_array_almost_equal(numpy.array(posel1),numpy.array(posel2), decimal=3)
00120         numpy.testing.assert_array_almost_equal(numpy.array(poser1),numpy.array(poser2), decimal=3)
00121 
00122         posel1 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
00123         poser1 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
00124         print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel1);
00125         print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser1);
00126         self.robot.setJointAnglesOfGroup('torso', [45], 1)
00127         self.robot.waitInterpolationOfGroup('torso')
00128         posel2 = self.robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')
00129         poser2 = self.robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')
00130         print_pose("robot.getCurrentPose('LARM_JOINT5:CHEST_JOINT0')", posel2);
00131         print_pose("robot.getCurrentPose('RARM_JOINT5:CHEST_JOINT0')", poser2);
00132         numpy.testing.assert_array_almost_equal(numpy.array(posel1),numpy.array(posel2), decimal=3)
00133         numpy.testing.assert_array_almost_equal(numpy.array(poser1),numpy.array(poser2), decimal=3)
00134 
00135         self.robot.setJointAnglesOfGroup('torso', [0], 1)
00136         self.robot.waitInterpolationOfGroup('torso')
00137         pos1 = self.robot.getCurrentPosition('LARM_JOINT5')
00138         rot1 = self.robot.getCurrentRotation('LARM_JOINT5')
00139         rpy1 = self.robot.getCurrentRPY('LARM_JOINT5')
00140 
00141         self.robot.setJointAnglesOfGroup('torso', [0], 1)
00142         self.robot.waitInterpolationOfGroup('torso')
00143         pos2 = self.robot.getCurrentPosition('LARM_JOINT5', 'CHEST_JOINT0')
00144         rot2 = self.robot.getCurrentRotation('LARM_JOINT5', 'CHEST_JOINT0')
00145         try:
00146             rpy2 = self.robot.getCurrentRPY('LARM_JOINT5', 'CHEST_JOINT0')
00147         except RuntimeError as e:
00148             if re.match(r'frame_name \(.+\) is not supported', e.message):
00149                 print(e.message + "...this is expected so pass the test")
00150             elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00151                 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00152                 print(e.message + "...this is expected so pass the test")
00153                 return True
00154             else:
00155                 raise RuntimeError(e.message)
00156         numpy.testing.assert_array_almost_equal(numpy.array(pos1),numpy.array(pos2), decimal=3)
00157         numpy.testing.assert_array_almost_equal(numpy.array(rot1),numpy.array(rot2), decimal=3)
00158         numpy.testing.assert_array_almost_equal(numpy.array(rpy1),numpy.array(rpy2), decimal=3)
00159     
00160     def testGetterByFrame(self):
00161         def print_pose(msg, pose):
00162             print msg, (pose[3],pose[7],pose[11]), euler_from_matrix([pose[0:3], pose[4:7], pose[8:11]], 'sxyz')
00163 
00164         self.robot.goInitial()
00165         
00166         posel1 = self.robot.getCurrentPose('LARM_JOINT5')
00167         try:
00168             posel2 = self.robot.getCurrentPose('LARM_JOINT5', 'WAIST')
00169         except RuntimeError as e:
00170             if re.match(r'frame_name \(.+\) is not supported', e.message):
00171                 print(e.message + "...this is expected so pass the test")
00172             elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00173                 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00174                 print(e.message + "...this is expected so pass the test")
00175                 return True
00176             else:
00177                 raise RuntimeError(e.message)
00178         numpy.testing.assert_array_almost_equal(numpy.array(posel1),numpy.array(posel2), decimal=3)
00179         
00180         print_pose("robot.getCurrentPose(LARM_JOINT5:DEFAULT)", posel1)
00181         print_pose("robot.getCurrentPose(LARM_JOINT5:WAIST)", posel2)
00182 
00183         posl1 = self.robot.getCurrentPosition('LARM_JOINT5')
00184         posl2 = self.robot.getCurrentPosition('LARM_JOINT5', 'WAIST')
00185         numpy.testing.assert_array_almost_equal(numpy.array(posl1),numpy.array(posl2), decimal=3)
00186         
00187         print "robot.getCurrentPosition(LARM_JOINT5:DEFAULT)", posl1
00188         print "robot.getCurrentPosition(LARM_JOINT5:WAIST)", posl2
00189 
00190         rotl1 = self.robot.getCurrentRotation('LARM_JOINT5')
00191         rotl2 = self.robot.getCurrentRotation('LARM_JOINT5', 'WAIST')
00192         numpy.testing.assert_array_almost_equal(numpy.array(rotl1),numpy.array(rotl2), decimal=3)
00193         
00194         print "robot.getCurrentRotation(LARM_JOINT5:DEFAULT)", rotl1
00195         print "robot.getCurrentRotation(LARM_JOINT5:WAIST)", rotl2
00196 
00197         rpyl1 = self.robot.getCurrentRPY('LARM_JOINT5')
00198         try:
00199             rpyl2 = self.robot.getCurrentRPY('LARM_JOINT5', 'WAIST')
00200         except RuntimeError as e:
00201             if re.match(r'frame_name \(.+\) is not supported', e.message):
00202                 print(e.message + "...this is expected so pass the test")
00203             elif self.robot.fk.ref.get_component_profile().version <= '315.2.4':
00204                 print("target hrpsys version is " + self.robot.fk.ref.get_component_profile().version)
00205                 print(e.message + "...this is expected so pass the test")
00206                 return True
00207             else:
00208                 raise RuntimeError(e.message)
00209         numpy.testing.assert_array_almost_equal(numpy.array(rpyl1),numpy.array(rpyl2), decimal=3)
00210         
00211         print "robot.getCurrentRPY(LARM_JOINT5:DEFAULT)", rpyl1
00212         print "robot.getCurrentRPY(LARM_JOINT5:WAIST)", rpyl2
00213 
00214         ref_posel1 = self.robot.getReferencePose('LARM_JOINT5')
00215         ref_posel2 = self.robot.getReferencePose('LARM_JOINT5', 'WAIST')
00216         numpy.testing.assert_array_almost_equal(numpy.array(ref_posel1),numpy.array(ref_posel2), decimal=3)
00217         
00218         print "robot.getReferencePose(LARM_JOINT5:DEFAULT)", ref_posel1
00219         print "robot.getReferencePose(LARM_JOINT5:WAIST)", ref_posel2
00220 
00221         ref_posl1 = self.robot.getReferencePosition('LARM_JOINT5')
00222         ref_posl2 = self.robot.getReferencePosition('LARM_JOINT5', 'WAIST')
00223         numpy.testing.assert_array_almost_equal(numpy.array(ref_posl1),numpy.array(ref_posl2), decimal=3)
00224         
00225         print "robot.getReferencePosition(LARM_JOINT5:DEFAULT)", ref_posl1
00226         print "robot.getReferencePosition(LARM_JOINT5:WAIST)", ref_posl2
00227 
00228         ref_rotl1 = self.robot.getReferenceRotation('LARM_JOINT5')
00229         ref_rotl2 = self.robot.getReferenceRotation('LARM_JOINT5', 'WAIST')
00230         numpy.testing.assert_array_almost_equal(numpy.array(ref_rotl1),numpy.array(ref_rotl2), decimal=3)
00231         
00232         print "robot.getReferenceRotation(LARM_JOINT5:DEFAULT)", ref_rotl1
00233         print "robot.getReferenceRotation(LARM_JOINT5:WAIST)", ref_rotl2
00234 
00235         ref_rpyl1 = self.robot.getReferenceRPY('LARM_JOINT5')
00236         ref_rpyl2 = self.robot.getReferenceRPY('LARM_JOINT5', 'WAIST')
00237         numpy.testing.assert_array_almost_equal(numpy.array(ref_rpyl1),numpy.array(ref_rpyl2), decimal=3)
00238         
00239         print "robot.getReferenceRPY(LARM_JOINT5:DEFAULT)", ref_rpyl1
00240         print "robot.getReferenceRPY(LARM_JOINT5:WAIST)", ref_rpyl2
00241 
00242 if __name__ == '__main__':
00243     import rostest
00244     rostest.rosrun(PKG, 'test_hronx_target', TestHiroTarget)