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 import rtm
00015 from hrpsys.hrpsys_config import euler_from_matrix
00016
00017 import numpy
00018 import unittest
00019 import time
00020 import tempfile
00021
00022 import random
00023 import numpy as np
00024
00025 from rtm import connectPorts, disconnectPorts
00026
00027 from distutils.version import StrictVersion
00028
00029 class TestHiroIK(unittest.TestCase):
00030
00031 @classmethod
00032 def setUpClass(self):
00033
00034
00035
00036
00037 self.robot = hironx.HIRONX()
00038
00039 self.robot.init()
00040
00041 def angle_vector_generator(self):
00042 for i0 in range(-80,80,80):
00043 for i1 in range(-130,50,90):
00044 for i2 in range(-150,10,80):
00045 for i3 in range(-160,100,130):
00046 for i4 in range(-80,80,80):
00047 for i5 in range(-160,160,160):
00048 yield [i0, i1, i2, i3, i4, i5]
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 def target_point_generator(self,xmin,xmax,ymin,ymax,zmin,zmax,step):
00074 for x in range(xmin,xmax,step):
00075 for y in range(ymin,ymax,step):
00076 for z in range (zmin,zmax,step):
00077 yield [x/1000.0, y/1000.0, z/1000.0]
00078
00079 def test_ik_left(self):
00080 self.pos_ik_test("LARM", 220,400, -5,320, 5,300, 100)
00081
00082 def test_ik_right(self):
00083 self.pos_ik_test("RARM", 220,400, -320,5, 5,300, 100)
00084
00085 def pos_ik_test(self, arm, xmin,xmax,ymin,ymax,zmin,zmax,step):
00086 arm_target = arm+"_JOINT5"
00087 rot1 = [[0,0,-1],[0,1,0],[1,0,0]]
00088 for pos1 in self.target_point_generator(xmin,xmax,ymin,ymax,zmin,zmax,step):
00089 self.robot.goInitial(tm=2)
00090 print self.robot.getReferenceRPY(arm_target)
00091 self.assertTrue(self.robot.setTargetPose(arm, pos1, euler_from_matrix(rot1), 1))
00092 self.robot.waitInterpolationOfGroup(arm)
00093 pos2 = self.robot.getReferencePosition(arm_target)
00094 rot2 = self.robot.getReferenceRotation(arm_target)
00095 print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
00096 print "rpy", rot1, rot2, numpy.linalg.norm(numpy.array(rot1)-numpy.array(rot2))
00097 self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<1.0e-4)
00098 self.assertTrue(numpy.linalg.norm(numpy.array(rot1)-numpy.array(rot2))<1.0e-3)
00099
00100 def test_set_target_pose(self):
00101 self.robot.setJointAnglesOfGroup("TORSO",[45],3)
00102 self.robot.waitInterpolationOfGroup("TORSO")
00103 ret = self.robot.setTargetPose("larm", [0.3255627368715471, 0.1823638733778268, 0.07462449717662004+0.2], [-3.0732189053889805, -1.5690225912054285, 3.0730289207320203], 5, "CHEST_JOINT0")
00104 self.assertTrue(ret)
00105
00106 def test_set_target_pose_clear(self):
00107 if StrictVersion(self.robot.hrpsys_version) < StrictVersion('315.5.0'):
00108 return
00109 self.robot.goInitial(tm=3)
00110
00111 self.robot.connectLoggerPort(self.robot.el, 'beepCommand')
00112 ret = self.robot.setTargetPose("larm", [0.3255627368715471, 0.1823638733778268, 0.07462449717662004+0.2], [-3.0732189053889805, -1.5690225912054285, 3.0730289207320203], 5, "CHEST_JOINT0")
00113 self.robot.waitInterpolationOfGroup("larm")
00114 self.assertTrue(ret)
00115
00116 ret = self.robot.setTargetPose("larm", [0.3255627368715471, 0.1823638733778268, 0.07462449717662004-0.1], [-3.0732189053889805, -1.5690225912054285, 3.0730289207320203], 5, "CHEST_JOINT0")
00117 import time
00118 time.sleep(1)
00119 self.robot.clearOfGroup('larm')
00120 self.assertTrue(ret)
00121
00122 self.robot.clearLog()
00123 self.robot.setMaxLogLength(2000)
00124 ret = self.robot.setTargetPose("larm", [0.3255627368715471, 0.1823638733778268, 0.07462449717662004+0.2], [-3.0732189053889805, -1.5690225912054285, 3.0730289207320203], 5, "CHEST_JOINT0")
00125 self.robot.waitInterpolationOfGroup("larm")
00126 self.robot.saveLog(fname='/tmp/test_hironx_ik')
00127 print("check /tmp/test_hironx_ik.el_beepCommand")
00128 fp = open('/tmp/test_hironx_ik.el_beepCommand', "r")
00129 for l in fp:
00130 print l
00131 assert(False)
00132
00133
00134 def test_set_target_pose_relative_319(self):
00135 self.robot.goInitial(tm=3)
00136 ret = self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dz=0.01, tm=0.5)
00137 self.assertTrue(ret)
00138 ret = self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dz=0, tm=0.5)
00139 self.assertTrue(ret)
00140
00141
00142
00143
00144 if __name__ == '__main__':
00145 import rostest
00146 rostest.rosrun(PKG, 'test_hronx_ik', TestHiroIK)