hironx_ros_bridge: test_hironx_controller.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 TestHiroController(TestHiro):
00007 
00008     # https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_impedance_controller.py.in
00009     def test_impedance_controller(self): # https://github.com/start-jsk/rtmros_hironx/issues/337
00010         if not self.robot.ic or self.robot.ic.ref.get_component_profile().version < '315.3.0':
00011             self.assertTrue(True)
00012             return True
00013         # although this is not stable rtc, we'll test them
00014         self.robot.goInitial(tm=1)
00015         ret = self.robot.startImpedance('rarm') # this returns ret, this is bug
00016         #self.assertTrue(ret)
00017         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00018                                               10,0,0,0,0,0,], 2.0);
00019         #self.assertTrue(ret)
00020         self.robot.seq_svc.waitInterpolation();
00021         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00022                                               0,0,0,0,0,0,], 2.0);
00023         #self.assertTrue(ret)
00024         self.robot.seq_svc.waitInterpolation();
00025         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00026                                               0,10,0,0,0,0,], 2.0);
00027         #self.assertTrue(ret)
00028         self.robot.seq_svc.waitInterpolation();
00029         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00030                                               0,0,0,0,0,0,], 2.0);
00031         #self.assertTrue(ret)
00032         self.robot.seq_svc.waitInterpolation();
00033         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00034                                               0,0,10,0,0,0,], 2.0);
00035         #self.assertTrue(ret)
00036         self.robot.seq_svc.waitInterpolation();
00037         #self.assertTrue(ret)
00038         ret = self.robot.stopImpedance('rarm')
00039         #self.assertTrue(ret)
00040         self.assertTrue(True) # this is dummy, current simulate hiro does not have force sensor so it retunrs None
00041 
00042 
00043 if __name__ == '__main__':
00044     import rostest
00045     rostest.rosrun(PKG, 'test_hronx_controller', TestHiroController)