hironx_ros_bridge: test_hironx_fullbody.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 TestHiroFullbody(TestHiro):
00007 
00008     def fullbody_init (self):
00009         self.filenames = []
00010         self.robot.seq_svc.removeJointGroup("larm")
00011         self.robot.seq_svc.removeJointGroup("rarm")
00012         self.robot.seq_svc.removeJointGroup("head")
00013         #self.robot.seq_svc.removeJointGroup("torso") we use torso for sleep certain time, so not remove this
00014         self.filename_base = tempfile.mkstemp()[1]
00015 
00016     def test_fullbody_setJointAngles_Wait (self):
00017         self.fullbody_init()
00018         # move to initiali position
00019         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00020         self.robot.waitInterpolation()
00021 
00022         self.robot.clearLog()
00023         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00024         self.robot.waitInterpolation()
00025         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -100, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00026         self.robot.waitInterpolation()
00027         filename = self.filename_base + "-wait"
00028         self.robot.saveLog(filename)
00029         q_filename = filename+"."+self.robot.rh.name()+"_q"
00030         data = self.load_log_data(q_filename)
00031 
00032         print "check setJointAngles(wait=True)"
00033         self.check_log_data(data, 6, 10.0, -140.0, -100.0)
00034         return True
00035 
00036     def test_fullbody_setJointAngles_NoWait (self):
00037         self.fullbody_init()
00038         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00039         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -100, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00040         self.robot.waitInterpolation()
00041         for i in range(len(clear_time)):
00042             self.robot.clearLog()
00043             self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00044             self.robot.setJointAnglesOfGroup("torso", [0], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00045 
00046             self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -100, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00047             self.robot.waitInterpolation()
00048             filename = self.filename_base + "-no-wait-"+str(clear_time[i])
00049             self.robot.saveLog(filename)
00050             q_filename = filename+"."+self.robot.rh.name()+"_q"
00051             data = self.load_log_data(q_filename)
00052 
00053             print "check setJointAngles(wait=True)"
00054             self.check_log_data(data, 6, (10.0 - (5 - clear_time[i])), [-140+i*40/len(clear_time),20], -100.0)
00055 
00056     def test_fullbody_setJointAngles_Clear (self):
00057         self.fullbody_init()
00058         # check if clear stops interpolation
00059         clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
00060         for i in range(len(clear_time)):
00061             self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -100, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00062             self.robot.waitInterpolation()
00063             self.robot.clearLog()
00064             self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -100, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5-clear_time[i]);
00065             self.robot.waitInterpolation()
00066             self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 5);
00067             self.robot.setJointAnglesOfGroup("torso", [0], clear_time[i], wait=True);#  time.sleep(clear_time[i]);
00068 
00069             self.robot.clear()
00070             self.robot.waitInterpolation()
00071             filename = self.filename_base + "-clear-"+str(clear_time[i])
00072             self.robot.saveLog(filename)
00073             q_filename = filename+"."+self.robot.rh.name()+"_q"
00074             data = self.load_log_data(q_filename)
00075 
00076             print "check setJointAngles(Clear)"
00077             self.check_log_data(data, 6, 5, [-140+(i+1)*40/len(clear_time),20], -100.0, acc_thre=0.2)
00078 
00079     def test_fullbody_setJointAngles_minus(self):
00080         self.fullbody_init()
00081         self.robot.el_svc.setServoErrorLimit("all", 0.01) # default is 0.18
00082         
00083         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 3);
00084         self.robot.waitInterpolation()
00085         self.robot.clearLog()
00086         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 1);
00087         self.robot.waitInterpolation()
00088 
00089         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], -1);
00090         self.robot.waitInterpolation()
00091 
00092         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 3);
00093         self.robot.waitInterpolation()
00094 
00095         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 0);
00096         self.robot.waitInterpolation()
00097 
00098         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 1);
00099         self.robot.waitInterpolation()
00100 
00101         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 0.1);
00102         self.robot.waitInterpolation()
00103 
00104         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -140, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 1.0);
00105         self.robot.waitInterpolation()
00106 
00107         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 0.05);
00108         self.robot.waitInterpolation()
00109 
00110         self.robot.setJointAngles([0, 0, 0, 0,   -0.6, 0, -120, 15.2, 9.4, 3.2,  0,0,0,0,    -0.6, 0, -100,-15.2, 9.4,-3.2,   0, 0, 0, 0], 1.0);
00111         self.robot.waitInterpolation()
00112 
00113         filename = self.filename_base + "-minus"
00114         self.robot.saveLog(filename)
00115         # write pdf file
00116         q_filename = filename+"."+self.robot.rh.name()+"_q"
00117         self.write_all_joint_pdf(q_filename, "full_minus_check.pdf")
00118 
00119         # assertion
00120         data = self.load_log_data(q_filename)
00121         print "check setJointAngles(minus)"
00122         self.check_log_data(data, 6, 7.19, -140, -120.0, acc_thre = 1.0)
00123 
00124         self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18
00125 
00126 if __name__ == '__main__':
00127     import rostest
00128     rostest.rosrun(PKG, 'test_hronx_fullbody', TestHiroFullbody)