test_hironx_ik.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 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         #modelfile = rospy.get_param("hironx/collada_model_filepath")
00034         #rtm.nshost = 'hiro014'
00035         #robotname = "RobotHardware0"
00036 
00037         self.robot = hironx.HIRONX()
00038         #self.robot.init(robotname=robotname, url=modelfile)
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 # 3/9/2014 Isaac comments out the entire test_ik_joint_angle that can cause left arm to hit its shoulder.
00051 #    def test_ik_joint_angle(self):
00052 #        lav = self.angle_vector_generator().next()
00053 #        for av in self.angle_vector_generator():
00054 #            print "av", av
00055 #            self.robot.setJointAnglesOfGroup("LARM", av, 2)
00056 #            self.robot.waitInterpolationOfGroup("LARM")
00057 #            pos1 = self.robot.getReferencePosition("LARM_JOINT5")
00058 #            rpy1 = self.robot.getReferenceRPY("LARM_JOINT5")
00059 #            if numpy.linalg.norm(numpy.array(lav) - numpy.array(av)) > 10:
00060 #                lav = av
00061 #            self.robot.setJointAnglesOfGroup("LARM", lav, 2)
00062 #            self.robot.waitInterpolationOfGroup("LARM")
00063 #            self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 5))
00064 #            self.robot.waitInterpolationOfGroup("LARM")
00065 #            pos2 = self.robot.getReferencePosition("LARM_JOINT5")
00066 #            rpy2 = self.robot.getReferenceRPY("LARM_JOINT5")
00067 #            print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
00068 #            print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))
00069 #            self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<1.0e-4) # 0.1 mm
00070 #            self.assertTrue(numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))<1.0e-3) # 0.001 rad = 0.057296 deg
00071 #            lav = av
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) # zmax=390
00081 
00082     def test_ik_right(self):
00083         self.pos_ik_test("RARM", 220,400, -320,5,  5,300, 100) # zmax=390
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) # 0.1 mm
00098             self.assertTrue(numpy.linalg.norm(numpy.array(rot1)-numpy.array(rot2))<1.0e-3) # 0.001 rad = 0.057296 deg
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         # https://github.com/tork-a/rtmros_nextage/issues/332
00111         self.robot.connectLoggerPort(self.robot.el, 'beepCommand') # Just for checking
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): # https://github.com/start-jsk/rtmros_hironx/issues/319
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 # for debug
00142 # $ python -m unittest test_hironx_ik.TestHiroIK.test_set_target_pose
00143 #
00144 if __name__ == '__main__':
00145     import rostest
00146     rostest.rosrun(PKG, 'test_hronx_ik', TestHiroIK) 


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37