Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 import unittest
00041 import numpy
00042
00043 from hironx_ros_bridge import hironx_client as hironx
00044 from hrpsys import rtm
00045
00046 _ARMGROUP_TESTED = 'larm'
00047 _LINK_TESTED = 'LARM_JOINT5'
00048 _GOINITIAL_TIME_MIDSPEED = 3
00049 _NUM_CARTESIAN_ITERATION = 300
00050 _PKG = 'nextage_ros_bridge'
00051
00052
00053 class TestNextageopen(unittest.TestCase):
00054 '''
00055 Test NextageClient with rostest.
00056 '''
00057
00058 @classmethod
00059 def setUpClass(self):
00060
00061
00062
00063
00064
00065 self._robot = hironx.HIRONX()
00066
00067 self._robot.init()
00068
00069 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00070
00071
00072 def _set_relative(self, dx=0, dy=0, dz=0):
00073
00074 self._robot.seq_svc.setMaxIKError(0.00001, 0.01)
00075 posi_prev = self._robot.getCurrentPosition(_LINK_TESTED)
00076 for i in range(_NUM_CARTESIAN_ITERATION):
00077 self._robot.setTargetPoseRelative(_ARMGROUP_TESTED,
00078 _LINK_TESTED, dx, dy, dz, tm=0.15)
00079
00080 posi_post = self._robot.getCurrentPosition(_LINK_TESTED)
00081
00082 diff_result = posi_prev
00083 for i in range(len(posi_prev)):
00084 diff_result[i] = posi_post[i] - posi_prev[i]
00085 diff_expected = _NUM_CARTESIAN_ITERATION*numpy.array([dx,dy,dz])
00086 diff_result = numpy.array(diff_result)
00087 print('Position diff={}, expected={}'.format(diff_result, diff_expected))
00088 numpy.testing.assert_array_almost_equal(diff_result, diff_expected, decimal=3)
00089 return True
00090
00091 def test_set_targetpose_relative_dx(self):
00092 assert(self._set_relative(dx=0.0001))
00093 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00094
00095 def test_set_targetpose_relative_dy(self):
00096 assert(self._set_relative(dy=0.0001))
00097 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00098
00099 def test_set_targetpose_relative_dz(self):
00100 assert(self._set_relative(dz=0.0001))
00101 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00102
00103
00104 if __name__ == '__main__':
00105 import rostest
00106 rostest.rosrun(_PKG, 'test_nxopen', TestNextageopen)