00001
00002
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
00018
00019
00020
00021 def test_rarm_setJointAngles_Wait (self):
00022 self.limbbody_init()
00023
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);
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
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);
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
00093
00094
00095
00096
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
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
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);
00132
00133
00134
00135
00136
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);
00139 self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00140
00141
00142
00143 self.robot.waitInterpolationOfGroup("rarm")
00144
00145 filename = self.filename_base + "-no-wait2"
00146 self.robot.saveLog(filename)
00147
00148
00149 q_filename = filename+"."+self.robot.rh.name()+"_q"
00150 self.write_all_joint_pdf(q_filename, "rarm_accel_check.pdf")
00151
00152
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)
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
00181
00182 filename = self.filename_base + "-minus"
00183 self.robot.saveLog(filename)
00184
00185 q_filename = filename+"."+self.robot.rh.name()+"_q"
00186 self.write_all_joint_pdf(q_filename, "rarm_minus_check.pdf")
00187
00188
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)
00194
00195
00196 if __name__ == '__main__':
00197 import rostest
00198 rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)