test_hironx.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 PKG = 'hironx_ros_bridge'
00005 # rosbuild needs load_manifest
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         #modelfile = rospy.get_param("hironx/collada_model_filepath")
00034         #rtm.nshost = 'hiro024'
00035         #robotname = "RobotHardware0"
00036 
00037         cls.robot = hironx.HIRONX()
00038         #cls.robot.init(robotname=robotname, url=modelfile)
00039         cls.robot.init()
00040 
00041     @classmethod
00042     def tearDownClass(cls):
00043         #self.write_output_to_pdf("test-hironx.pdf") # don't know how to call this mehtod
00044         True
00045 
00046     ###
00047     def set_joint_angles_no_wait_test (self):
00048         # send initial pose
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         # make sure setJointAnglesOfGroup with wait
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         # make sure setJointAnglesOfGorup with wita=False returns immediately
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         # make sure that waitInterpolationOfGroup wait until target reaches the goal
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     # load from log data, [[p0,p1,....p25],[p0,p1,....p25],...,[p0,p1,....p25]]
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         # print "%f %f" % (current_time - start_time, current_value*180/math.pi)
00094         f.close()
00095         self.filenames.append(name)
00096         return data
00097 
00098     # load from log data, [[p0,p1,....p25],[p0,p1,....p25],...,[p0,p1,....p25]] # degree
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         # print "%f %f" % (current_time - start_time, current_value*180/math.pi)
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): # expected, time, min, max of index
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         # min_data = [min_data, min_thre]
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         # check acceleration
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 # for debug
00244 # $ python -m unittest test_hironx.TestHiro.test_impedance_controller
00245 #
00246 if __name__ == '__main__':
00247     import rostest
00248     rostest.rosrun(PKG, 'test_hronx', TestHiro) 
00249 


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39