00001
00002
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
00020
00021
00022
00023 def test_rarm_setJointAngles_Wait (self):
00024 self.limbbody_init()
00025
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);
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
00071 self.assertTrue(True)
00072 return
00073
00074 self.limbbody_init()
00075
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);
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
00100
00101
00102
00103
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
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
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);
00139
00140
00141
00142
00143
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);
00146 self.robot.setJointAnglesOfGroup("rarm", [-0.6, 0, -120, 15.2, 9.4, 3.2], 3, wait=False);
00147
00148
00149
00150 self.robot.waitInterpolationOfGroup("rarm")
00151
00152 filename = self.filename_base + "-no-wait2"
00153 self.robot.saveLog(filename)
00154
00155
00156 q_filename = filename+"."+self.robot.rh.name()+"_q"
00157 self.write_all_joint_pdf(q_filename, "rarm_accel_check.pdf")
00158
00159
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)
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
00188
00189 filename = self.filename_base + "-minus"
00190 self.robot.saveLog(filename)
00191
00192 q_filename = filename+"."+self.robot.rh.name()+"_q"
00193 self.write_all_joint_pdf(q_filename, "rarm_minus_check.pdf")
00194
00195
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)
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
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
00223
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)