hironx_ros_bridge: test_hironx

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 from distutils.version import StrictVersion
00005 
00006 from test_hironx import *
00007 
00008 class TestHiroLimb(TestHiro):
00009 
00010     def limbbody_init (self):
00011         self.filenames = []
00012         self.robot.setSelfGroups()
00013         self.filename_base = tempfile.mkstemp()[1]
00014 
00015     def test_goInitial(self):
00016         self.limbbody_init()
00017         self.robot.goInitial()
00018 
00019 #    def test_goOffPose(self):
00020 #        self.limbbody_init()
00021 #        self.robot.goOffPose()
00022 
00023     def test_rarm_setJointAngles_Wait (self):
00024         self.limbbody_init()
00025         # move to initiali position
00026         self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
00027         self.robot.waitInterpolationOfGroup("rarm")
00028         self.robot.clearLog()
00029 
00030         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00031         self.robot.waitInterpolationOfGroup("rarm")
00032         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00033         self.robot.waitInterpolationOfGroup("rarm")
00034         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00035         self.robot.waitInterpolationOfGroup("rarm")
00036 
00037         filename = self.filename_base + "-wait"
00038         self.robot.saveLog(filename)
00039         q_filename = filename+"."+self.robot.rh.name()+"_q"
00040         data = self.load_log_data(q_filename)
00041 
00042         print "check setJointAnglesOfGroup(wait=True)"
00043         self.check_log_data(data, 6, 15.0, -140.0, -100.0)
00044         return True
00045 
00046     def test_rarm_setJointAngles_NoWait (self):
00047         self.limbbody_init()
00048         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00049         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00050         self.robot.waitInterpolationOfGroup("rarm")
00051         for i in range(len(clear_time)):
00052             self.robot.clearLog()
00053 
00054             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00055             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00056             self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00057             self.robot.waitInterpolationOfGroup("rarm")
00058 
00059             filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00060             self.robot.saveLog(filename)
00061             q_filename = filename+"."+self.robot.rh.name()+"_q"
00062             data = self.load_log_data(q_filename)
00063 
00064             print "check setJointAnglesOfGroup(wait=True) "+str(clear_time[i])
00065             self.check_log_data(data, 6, (10.0 - (5 - clear_time[i])), [(-140+i*40/len(clear_time)),20], -100.0)
00066 
00067 
00068     def test_rarm_setJointAngles_Clear (self):
00069         if StrictVersion(self.robot.hrpsys_version) < StrictVersion('315.5.0'):
00070             # clearOfGroup is not available until '315.5.0'
00071             self.assertTrue(True)
00072             return
00073 
00074         self.limbbody_init()
00075         # check if clear stops interpolation
00076         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00077         for i in range(len(clear_time)):
00078             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
00079             self.robot.waitInterpolationOfGroup("rarm")
00080             self.robot.clearLog()
00081 
00082             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00083             self.robot.waitInterpolationOfGroup("rarm")
00084             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00085             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00086             self.robot.clearOfGroup("rarm")
00087             self.robot.waitInterpolationOfGroup("rarm")
00088 
00089             filename = self.filename_base + "-clear-"+str(clear_time[i])
00090             self.robot.saveLog(filename)
00091             q_filename = filename+"."+self.robot.rh.name()+"_q"
00092             data = self.load_log_data(q_filename)
00093 
00094             print "check setJointAnglesOfGroup(clear) "+str(clear_time[i])
00095             self.check_log_data(data, 6, (5 + clear_time[i]), [(-140+i*40/len(clear_time)),20], -100.0)
00096 
00097     def test_rarm_setJointAnglesOfGroup_Override_Acceleration (self):
00098         self.limbbody_init()
00099         #disconnectPorts(self.robot.rh.port("q"), self.robot.log.port("q"))
00100         #connectPorts(self.robot.el.port("q"), self.robot.log.port("q"))
00101 
00102         #self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157,  45, 0, 0], 3, wait=False);
00103         #self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
00104         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00105         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00106         self.robot.waitInterpolationOfGroup("rarm")
00107         self.robot.clearLog()
00108         #self.robot.log_svc.maxLength(200*30)
00109 
00110         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 3, wait=False);
00111         # time.sleep(1.5);
00112         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00113         # self.robot.waitInterpolationOfGroup("rarm")
00114         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00115         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=False);
00116         # time.sleep(1.5);
00117         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00118         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00119         # self.robot.waitInterpolationOfGroup("rarm")
00120         # self.robot.waitInterpolationOfGroup("larm")
00121         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.1, wait=False);
00122         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 0.1, wait=False);
00123         # self.robot.waitInterpolationOfGroup("rarm")
00124         # self.robot.waitInterpolationOfGroup("larm")
00125         # self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157,  45, 0, 0], 3, wait=False);
00126         # self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
00127         # time.sleep(2.5);
00128         # for i in range(1,10):
00129         #     time.sleep(i/30.0)
00130         #     self.robot.setJointAnglesOfGroup("larm",
00131         #                                      [ 0.6, 0, -120,-15.2, 9.4,-3.2]+numpy.random.normal(0,1,6),
00132         #                                      3, wait=False);
00133 
00134         for i in range(3):
00135             self.robot.setJointAnglesOfGroup("rarm",
00136                                              [-0.6, 0, -100, 15.2, 9.4, 3.2]+numpy.random.normal(0,1,6),
00137                                              3, wait=False);
00138             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 1.5, wait=True);#  time.sleep(clear_time[i]);
00139 
00140         #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
00141         #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
00142         #self.robot.waitInterpolationOfGroup("rarm")
00143         #self.robot.waitInterpolationOfGroup("larm")
00144         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 3, wait=False);
00145         self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 1.5, wait=True);#  time.sleep(clear_time[i]);
00146         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00147         #time.sleep(1.5)
00148         #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=False);
00149         #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 0, wait=False);
00150         self.robot.waitInterpolationOfGroup("rarm")
00151 
00152         filename = self.filename_base + "-no-wait2"
00153         self.robot.saveLog(filename)
00154 
00155         # write pdf file
00156         q_filename = filename+"."+self.robot.rh.name()+"_q"
00157         self.write_all_joint_pdf(q_filename, "rarm_accel_check.pdf")
00158 
00159         # assertion
00160         data = self.load_log_data(q_filename)
00161         print "check setJointAnglesOfGroup(Override_acceleratoin)"
00162         self.check_log_data(data, 6, 9, -135, -100.0)
00163 
00164 
00165     def test_rarm_setJointAnglesOfGroup_minus(self):
00166         self.limbbody_init()
00167         self.robot.el_svc.setServoErrorLimit("all", 0.01) # default is 0.18
00168 
00169         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00170         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00171         self.robot.waitInterpolationOfGroup("rarm")
00172         self.robot.clearLog()
00173         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
00174         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
00175         self.robot.waitInterpolationOfGroup("rarm")
00176 
00177         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], -1, wait=True);
00178 
00179         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], -1, wait=True);
00180         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=True);
00181         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=True);
00182         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
00183         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 0.1, wait=True);
00184         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
00185         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.05, wait=True);
00186         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
00187         #self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
00188 
00189         filename = self.filename_base + "-minus"
00190         self.robot.saveLog(filename)
00191         # write pdf file
00192         q_filename = filename+"."+self.robot.rh.name()+"_q"
00193         self.write_all_joint_pdf(q_filename, "rarm_minus_check.pdf")
00194 
00195         # assertion
00196         data = self.load_log_data(q_filename)
00197         print "check setJointAnglesOfGroup(minus)"
00198         self.check_log_data(data, 6, 7.19, -140, -120.0, acc_thre = 1.5, tm_thre = 0.3)
00199 
00200         self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18
00201 
00202     def test_movejoints_neck_waist(self):
00203         '''
00204         Move neck and waist joints to the positional limit defined in
00205         the user's manual (with an arbitrary safety coefficient)
00206         using hrpsys commands.
00207 
00208         This test is originally made to catch issues like
00209         https://github.com/start-jsk/rtmros_hironx/issues/411
00210         '''
00211         safety_coeffiecient = 0.8
00212         durtion_operation = 10
00213         # Values claimed by manufacturer
00214         max_neck_pitch = 0.63
00215         min_neck_pitch = -0.4
00216         max_neck_yaw = 1.22
00217         min_neck_yaw = -1.22
00218         max_waist_yaw = 2.84
00219         min_waist_yaw = -2.84
00220 
00221         self.limbbody_init()
00222         # Here multiplied by safety coefficient to avoid the robot tries
00223         # the maximum angles, in case this test is used with real robots.
00224         self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=max_neck_pitch*safety_coeffiecient, tm=durtion_operation))
00225         self.robot.goInitial()
00226         self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=min_neck_pitch*safety_coeffiecient, tm=durtion_operation))
00227         self.robot.goInitial()
00228         self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dw=max_neck_yaw*safety_coeffiecient, tm=durtion_operation))
00229         self.robot.goInitial()
00230         self.assertTrue(self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dw=min_neck_yaw*safety_coeffiecient, tm=durtion_operation))
00231         self.robot.goInitial()
00232         self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=max_waist_yaw*safety_coeffiecient, tm=durtion_operation))
00233         self.robot.goInitial()
00234         self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=min_waist_yaw*safety_coeffiecient, tm=durtion_operation))
00235 
00236 if __name__ == '__main__':
00237     import rostest
00238     rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)