hironx_ros_bridge: test_hironx

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 TestHiroLimb(TestHiro):
00007 
00008     def limbbody_init (self):
00009         self.filenames = []
00010         self.robot.setSelfGroups()
00011         self.filename_base = tempfile.mkstemp()[1]
00012 
00013     def test_goInitial(self):
00014         self.limbbody_init()
00015         self.robot.goInitial()
00016 
00017 #    def test_goOffPose(self):
00018 #        self.limbbody_init()
00019 #        self.robot.goOffPose()
00020 
00021     def test_rarm_setJointAngles_Wait (self):
00022         self.limbbody_init()
00023         # move to initiali position
00024         self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
00025         self.robot.waitInterpolationOfGroup("rarm")
00026         self.robot.clearLog()
00027 
00028         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00029         self.robot.waitInterpolationOfGroup("rarm")
00030         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 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 
00035         filename = self.filename_base + "-wait"
00036         self.robot.saveLog(filename)
00037         q_filename = filename+"."+self.robot.rh.name()+"_q"
00038         data = self.load_log_data(q_filename)
00039 
00040         print "check setJointAnglesOfGroup(wait=True)"
00041         self.check_log_data(data, 6, 15.0, -140.0, -100.0)
00042         return True
00043 
00044     def test_rarm_setJointAngles_NoWait (self):
00045         self.limbbody_init()
00046         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00047         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00048         self.robot.waitInterpolationOfGroup("rarm")
00049         for i in range(len(clear_time)):
00050             self.robot.clearLog()
00051 
00052             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00053             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00054             self.robot.setJointAnglesOfGroup("rarm",[-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00055             self.robot.waitInterpolationOfGroup("rarm")
00056 
00057             filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00058             self.robot.saveLog(filename)
00059             q_filename = filename+"."+self.robot.rh.name()+"_q"
00060             data = self.load_log_data(q_filename)
00061 
00062             print "check setJointAnglesOfGroup(wait=True) "+str(clear_time[i])
00063             self.check_log_data(data, 6, (10.0 - (5 - clear_time[i])), [(-140+i*40/len(clear_time)),20], -100.0)
00064 
00065 
00066     def test_rarm_setJointAngles_Clear (self):
00067         self.limbbody_init()
00068         # check if clear stops interpolation
00069         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00070         for i in range(len(clear_time)):
00071             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 5, wait=False);
00072             self.robot.waitInterpolationOfGroup("rarm")
00073             self.robot.clearLog()
00074 
00075             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 5, wait=False);
00076             self.robot.waitInterpolationOfGroup("rarm")
00077             self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 5, wait=False);
00078             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00079             self.robot.clearOfGroup("rarm")
00080             self.robot.waitInterpolationOfGroup("rarm")
00081 
00082             filename = self.filename_base + "-clear-"+str(clear_time[i])
00083             self.robot.saveLog(filename)
00084             q_filename = filename+"."+self.robot.rh.name()+"_q"
00085             data = self.load_log_data(q_filename)
00086 
00087             print "check setJointAnglesOfGroup(clear) "+str(clear_time[i])
00088             self.check_log_data(data, 6, (5 + clear_time[i]), [(-140+i*40/len(clear_time)),20], -100.0)
00089 
00090     def test_rarm_setJointAnglesOfGroup_Override_Acceleration (self):
00091         self.limbbody_init()
00092         #disconnectPorts(self.robot.rh.port("q"), self.robot.log.port("q"))
00093         #connectPorts(self.robot.el.port("q"), self.robot.log.port("q"))
00094 
00095         #self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157,  45, 0, 0], 3, wait=False);
00096         #self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
00097         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00098         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00099         self.robot.waitInterpolationOfGroup("rarm")
00100         self.robot.clearLog()
00101         #self.robot.log_svc.maxLength(200*30)
00102 
00103         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 3, wait=False);
00104         # time.sleep(1.5);
00105         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00106         # self.robot.waitInterpolationOfGroup("rarm")
00107         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00108         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=False);
00109         # time.sleep(1.5);
00110         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00111         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 3, wait=False);
00112         # self.robot.waitInterpolationOfGroup("rarm")
00113         # self.robot.waitInterpolationOfGroup("larm")
00114         # self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.1, wait=False);
00115         # self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 0.1, wait=False);
00116         # self.robot.waitInterpolationOfGroup("rarm")
00117         # self.robot.waitInterpolationOfGroup("larm")
00118         # self.robot.setJointAnglesOfGroup("rarm", [ 25, -139, -157,  45, 0, 0], 3, wait=False);
00119         # self.robot.setJointAnglesOfGroup("larm", [-25, -139, -157, -45, 0, 0], 3, wait=False);
00120         # time.sleep(2.5);
00121         # for i in range(1,10):
00122         #     time.sleep(i/30.0)
00123         #     self.robot.setJointAnglesOfGroup("larm",
00124         #                                      [ 0.6, 0, -120,-15.2, 9.4,-3.2]+numpy.random.normal(0,1,6),
00125         #                                      3, wait=False);
00126 
00127         for i in range(3):
00128             self.robot.setJointAnglesOfGroup("rarm",
00129                                              [-0.6, 0, -100, 15.2, 9.4, 3.2]+numpy.random.normal(0,1,6),
00130                                              3, wait=False);
00131             self.robot.setJointAnglesOfGroup("larm", [-0.6, 0, -100, 15.2, 9.4, 3.2], 1.5, wait=True);#  time.sleep(clear_time[i]);
00132 
00133         #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
00134         #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
00135         #self.robot.waitInterpolationOfGroup("rarm")
00136         #self.robot.waitInterpolationOfGroup("larm")
00137         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -140, 15.2, 9.4, 3.2], 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         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00140         #time.sleep(1.5)
00141         #self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=False);
00142         #self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 0, wait=False);
00143         self.robot.waitInterpolationOfGroup("rarm")
00144 
00145         filename = self.filename_base + "-no-wait2"
00146         self.robot.saveLog(filename)
00147 
00148         # write pdf file
00149         q_filename = filename+"."+self.robot.rh.name()+"_q"
00150         self.write_all_joint_pdf(q_filename, "rarm_accel_check.pdf")
00151 
00152         # assertion
00153         data = self.load_log_data(q_filename)
00154         print "check setJointAnglesOfGroup(Override_acceleratoin)"
00155         self.check_log_data(data, 6, 9, -135, -100.0)
00156 
00157 
00158     def test_rarm_setJointAnglesOfGroup_minus(self):
00159         self.limbbody_init()
00160         self.robot.el_svc.setServoErrorLimit("all", 0.01) # default is 0.18
00161 
00162         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00163         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 3, wait=False);
00164         self.robot.waitInterpolationOfGroup("rarm")
00165         self.robot.clearLog()
00166         self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 1, wait=False);
00167         self.robot.setJointAnglesOfGroup("larm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1, wait=False);
00168         self.robot.waitInterpolationOfGroup("rarm")
00169 
00170         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], -1, wait=True);
00171 
00172         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], -1, wait=True);
00173         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 3, wait=True);
00174         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0, wait=True);
00175         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
00176         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 0.1, wait=True);
00177         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
00178         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 0.05, wait=True);
00179         self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -120,-15.2, 9.4,-3.2], 1.0, wait=True);
00180         #self.robot.setJointAnglesOfGroup("rarm", [ 0.6, 0, -140,-15.2, 9.4,-3.2], 1.0, wait=True);
00181 
00182         filename = self.filename_base + "-minus"
00183         self.robot.saveLog(filename)
00184         # write pdf file
00185         q_filename = filename+"."+self.robot.rh.name()+"_q"
00186         self.write_all_joint_pdf(q_filename, "rarm_minus_check.pdf")
00187 
00188         # assertion
00189         data = self.load_log_data(q_filename)
00190         print "check setJointAnglesOfGroup(minus)"
00191         self.check_log_data(data, 6, 7.19, -140, -120.0, acc_thre = 1.5, tm_thre = 0.3)
00192 
00193         self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18
00194 
00195 
00196 if __name__ == '__main__':
00197     import rostest
00198     rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)