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
00042 from hrpsys import rtm
00043 from nextage_ros_bridge import nextage_client
00044
00045 _ARMGROUP_TESTED = 'larm'
00046 _LINK_TESTED = 'LARM_JOINT5'
00047 _GOINITIAL_TIME_MIDSPEED = 3
00048 _NUM_CARTESIAN_ITERATION = 300
00049 _PKG = 'nextage_ros_bridge'
00050
00051
00052 class TestNextageopen(unittest.TestCase):
00053 '''
00054 Test NextageClient with rostest.
00055 '''
00056
00057 @classmethod
00058 def setUpClass(self):
00059
00060 modelfile = '/opt/jsk/etc/NEXTAGE/model/main.wrl'
00061 rtm.nshost = 'nxc100'
00062 robotname = "RobotHardware0"
00063
00064 self._robot = nextage_client.NextageClient()
00065 self._robot.init(robotname=robotname, url=modelfile)
00066
00067 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00068
00069
00070 def _set_relative(self, dx=0, dy=0, dz=0):
00071
00072 self._robot.seq_svc.setMaxIKError(0.00001, 0.01)
00073 posi_prev = self._robot.getCurrentPosition(_LINK_TESTED)
00074 for i in range(_NUM_CARTESIAN_ITERATION):
00075 self._robot.setTargetPoseRelative(
00076 _ARMGROUP_TESTED, _LINK_TESTED, dx, dy, dz, tm=0.15)
00077
00078 posi_post = self._robot.getCurrentPosition(_LINK_TESTED)
00079
00080 diff_result = posi_post
00081 for i in range(len(posi_prev)):
00082 diff_result[i] = posi_prev[i] - posi_post[i]
00083 print('Position diff={}'.format(diff_result))
00084 return True
00085
00086 def test_set_targetpose_relative_dx(self):
00087 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00088 assert(self._set_relative(dx=0.0001))
00089
00090 def test_set_targetpose_relative_dy(self):
00091 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00092 assert(self._set_relative(dy=0.0001))
00093
00094 def test_set_targetpose_relative_dz(self):
00095 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00096 assert(self._set_relative(dz=0.0001))
00097
00098 if __name__ == '__main__':
00099 import rostest
00100 rostest.rosrun(_PKG, 'test_nxopen', TestNextageopen)