00001
00002
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
00014 self.filename_base = tempfile.mkstemp()[1]
00015
00016 def test_fullbody_setJointAngles_Wait (self):
00017 self.fullbody_init()
00018
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);
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
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);
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)
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
00116 q_filename = filename+"."+self.robot.rh.name()+"_q"
00117 self.write_all_joint_pdf(q_filename, "full_minus_check.pdf")
00118
00119
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)
00125
00126 if __name__ == '__main__':
00127 import rostest
00128 rostest.rosrun(PKG, 'test_hronx_fullbody', TestHiroFullbody)