00001
00002
00003
00004 PKG = 'hironx_ros_bridge'
00005
00006 try:
00007 import roslib
00008 import hironx_ros_bridge
00009 except:
00010 import roslib; roslib.load_manifest(PKG)
00011 import hironx_ros_bridge
00012
00013 from hironx_ros_bridge import hironx_client as hironx
00014 from hrpsys.hrpsys_config import euler_from_matrix
00015 from hrpsys import rtm
00016
00017 import os
00018 import unittest
00019 import time
00020 import tempfile
00021
00022 import math
00023 import random
00024 import numpy
00025 import re
00026
00027 from rtm import connectPorts, disconnectPorts
00028
00029 class TestHiro(unittest.TestCase):
00030
00031 @classmethod
00032 def setUpClass(cls):
00033
00034
00035
00036
00037 cls.robot = hironx.HIRONX()
00038
00039 cls.robot.init()
00040
00041 @classmethod
00042 def tearDownClass(cls):
00043
00044 True
00045
00046
00047 def set_joint_angles_no_wait_test (self):
00048
00049 av0 = [-0.6, 0, -100, 15.2, 9.4, 3.2]
00050 av1 = [-0.6, -90, -100, 15.2, 9.4, 3.2]
00051 self.robot.setJointAnglesOfGroup( "rarm" , av0, 5)
00052 pose0 = robot.getReferencePose("RARM_JOINT5")
00053 print pose0
00054
00055
00056 tm0 = time.time()
00057 self.robot.setJointAnglesOfGroup( "rarm" , av1, 5)
00058 self.robot.setJointAnglesOfGroup( "rarm" , av0, 5)
00059 tm1 = time.time()
00060 pose1 = self.robot.getReferencePose("RARM_JOINT5")
00061 print "setJointAnglesOfGroup : reach goal? ", numpy.linalg.norm(numpy.array(pose1)-numpy.array(pose0)) < 5.0e-3
00062 print " : spend 10 sec?", abs((tm1 - tm0) - 10.0) < 0.1, " " , tm1 - tm0
00063
00064
00065 self.robot.setJointAnglesOfGroup( "rarm" , av1, 5)
00066 tm0 = time.time()
00067 self.robot.setJointAnglesOfGroup( "rarm" , av0, 5, wait=False)
00068 tm1 = time.time()
00069 pose1 = self.robot.getReferencePose("RARM_JOINT5")
00070 print "setJointAnglesOfGroup(wait=False)", numpy.linalg.norm(numpy.array(pose1)-numpy.array(pose0)) > 5.0e-3
00071 print " : spend 10 sec?", abs((tm1 - tm0) - 0.0) < 0.1, " " , tm1 - tm0
00072
00073
00074 self.robot.waitInterpolationOfGroup("rarm")
00075 tm1 = time.time()
00076 pose1 = self.robot.getReferencePose("RARM_JOINT5")
00077 print "waitInterpolationOfGroup", numpy.linalg.norm(numpy.array(pose1)-numpy.array(pose0)) < 5.0e-3
00078 print " : spend 10 sec?", abs((tm1 - tm0) - 5.0) < 0.1, " " , tm1 - tm0
00079
00080
00081 def check_q_data(self,name):
00082 import math
00083 data = []
00084 name = name+".q"
00085 f = open(name)
00086 start_time = None
00087 for line in f:
00088 current_time = float(line.split(' ')[0])
00089 current_value = float(line.split(' ')[7])
00090 if not start_time:
00091 start_time = current_time
00092 data.append([current_time, current_value*180/math.pi])
00093
00094 f.close()
00095 self.filenames.append(name)
00096 return data
00097
00098
00099 def load_log_data(self,name):
00100 import math
00101 data = []
00102 f = open(name)
00103 start_time = None
00104 for line in f:
00105 data.append([float(i)*180/math.pi for i in line.split(' ')[1:-1]])
00106
00107 f.close()
00108 self.filenames.append(name)
00109 return data
00110
00111 def check_log_data(self, data, idx, tm_data, min_data, max_data, acc_thre=0.06, tm_thre=0.1):
00112 _tm_data = len(data)/200.0
00113 _min_data = min([d[idx] for d in data])
00114 _max_data = max([d[idx] for d in data])
00115 _tm_thre = tm_thre
00116
00117 if isinstance(min_data, (int, float)):
00118 min_data = [min_data, 5]
00119 if isinstance(max_data, (int, float)):
00120 max_data = [max_data, 5]
00121
00122 print "time (= ", _tm_data, ") == ", tm_data, " -> ", abs(_tm_data - tm_data) < tm_data*_tm_thre
00123 print " min (= ", _min_data, ") == ", min_data, " -> ", abs(_min_data - min_data[0]) < min_data[1]
00124 print " max (= ", _max_data, ") == ", max_data, " -> ", abs(_max_data - max_data[0]) < max_data[1]
00125 self.assertTrue(abs(_tm_data - tm_data) < tm_data*_tm_thre)
00126 self.assertTrue(abs(_min_data - min_data[0]) < min_data[1])
00127 self.assertTrue(abs(_max_data - max_data[0]) < max_data[1])
00128
00129
00130 flag = True
00131 for i in range(1, len(data)-1):
00132 for j in range(len(data[i])):
00133 p0 = data[i-1][j]
00134 p1 = data[i+0][j]
00135 p2 = data[i+1][j]
00136 v0 = p1 - p0
00137 v1 = p2 - p1
00138 if abs(v1 - v0) > acc_thre:
00139 flag = False
00140 print("Acceleration vaiorated! : n = %d, idx %d, p0 %f, p1 %f, p2 %f, v1 %f, v2 %f, acc %f (%f)" % (i, j, p0, p1, p2, v0, v1, v1-v0, (v1-v0)/180.0*math.pi))
00141 self.assertTrue(flag)
00142
00143 def write_d_dd_data(self, name):
00144 name_d = os.path.splitext(name)[0]+".dq"
00145 name_dd = os.path.splitext(name)[0]+".ddq"
00146 f = open(name)
00147 f_d = open(name_d, 'w')
00148 f_dd = open(name_dd, 'w')
00149 line_0 = None
00150 line_1 = None
00151 line_2 = None
00152 for line_0 in f:
00153 if line_2 and line_1 :
00154 tm = float(line_1.split(' ')[0])
00155 f_d.write(str(tm))
00156 f_dd.write(str(tm))
00157 for i in range(1,len(line_0.split(' '))-1):
00158 p0 = float(line_0.split(' ')[i])
00159 p1 = float(line_1.split(' ')[i])
00160 p2 = float(line_2.split(' ')[i])
00161 v0 = p1 - p0
00162 v1 = p2 - p1
00163 f_d.write(" %.10f"%v0)
00164 f_dd.write(" %.10f"%(v1 - v0))
00165 f_d.write('\n')
00166 f_dd.write('\n')
00167 line_2 = line_1
00168 line_1 = line_0
00169 f.close()
00170 f_d.close()
00171 f_dd.close()
00172 print "[%s] write %s"%(__file__, name_d)
00173 print "[%s] write %s"%(__file__, name_dd)
00174
00175 def write_all_joint_pdf(self, name, pdf_name):
00176 print "[%s] write pdf %s from log data %s"%(__file__, pdf_name, name)
00177 self.write_d_dd_data(name)
00178 _pdf_names = []
00179 for _name in [name, os.path.splitext(name)[0]+".dq", os.path.splitext(name)[0]+".ddq"]:
00180 _pdf_names.append(_name+"_"+pdf_name)
00181 cmd = "gnuplot -p -e \"set terminal pdf; set output '"+_pdf_names[-1]+"'; plot "
00182 f = open(_name)
00183 l = f.readline().split(' ')[:-1]
00184 f.close()
00185 for i in range(1,len(l)):
00186 cmd += "'"+_name+"' using 1:"+str(i+1)+" title 'joint "+str(i)+"' with lines"
00187 if i != len(l)-1:
00188 cmd += ","
00189 cmd += "\""
00190 os.system(cmd)
00191 cmd_str = 'pdfunite '+' '.join(_pdf_names) + ' ' + pdf_name
00192 cmd_str = cmd_str.replace('(', '\(')
00193 cmd_str = cmd_str.replace(')', '\)')
00194 os.system(cmd_str)
00195 return
00196
00197 def check_acceleration(self, name):
00198 name1 = name+".q"
00199 name2 = name+".acc"
00200 f1 = open(name1)
00201 f2 = open(name2, 'w')
00202 line_0 = None
00203 line_1 = None
00204 line_2 = None
00205 for line_0 in f1:
00206 if line_2 and line_1 :
00207 tm = float(line_1.split(' ')[0])
00208 f2.write(str(tm))
00209 for i in range(1,len(line_0.split(' '))-1):
00210 p0 = float(line_0.split(' ')[i])
00211 p1 = float(line_1.split(' ')[i])
00212 p2 = float(line_2.split(' ')[i])
00213 v0 = p1 - p0
00214 v1 = p2 - p1
00215 if abs(v1 - v0) > 0.0001:
00216 print("%s Acceleration vaiorated! : p0 %f, p1 %f, p2 %f, v1 %f, v2 %f, acc %f" % (name, p0, p1, p2, v0, v1, v1-v0))
00217 self.assertTrue(abs(v1 - v0) < 0.0001)
00218 f2.write(' ' + str(v1 - v0))
00219 f2.write('\n')
00220 line_2 = line_1
00221 line_1 = line_0
00222 f1.close()
00223 f2.close()
00224
00225
00226 def assertTrue(self,a):
00227 assert(a)
00228
00229 def write_output_to_pdf (self,name):
00230 print ";; write log data to "+name
00231 global filenames
00232 import os
00233 cmd = "gnuplot -p -e \"set terminal pdf; set output '"+name+"'; plot "
00234 for name in self.filenames:
00235 cmd += "'"+name+"' using 0:8 title '"+name+"' with lines"
00236 if name != self.filenames[-1]:
00237 cmd += ","
00238 cmd += "\""
00239 os.system(cmd)
00240 return cmd
00241
00242
00243
00244
00245
00246 if __name__ == '__main__':
00247 import rostest
00248 rostest.rosrun(PKG, 'test_hronx', TestHiro)
00249