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 class TestHiroIK(unittest.TestCase):
00028 
00029     @classmethod
00030     def setUpClass(self):
00031         #modelfile = rospy.get_param("hironx/collada_model_filepath")
00032         #rtm.nshost = 'hiro014'
00033         #robotname = "RobotHardware0"
00034 
00035         self.robot = hironx.HIRONX()
00036         #self.robot.init(robotname=robotname, url=modelfile)
00037         self.robot.init()
00038 
00039     def angle_vector_generator(self):
00040         for i0 in range(-80,80,80):
00041             for i1 in range(-130,50,90):
00042                 for i2 in range(-150,10,80):
00043                     for i3 in range(-160,100,130):
00044                         for i4 in range(-80,80,80):
00045                             for i5 in range(-160,160,160):
00046                                 yield [i0, i1, i2, i3, i4, i5]
00047 
00048 # 3/9/2014 Isaac comments out the entire test_ik_joint_angle that can cause left arm to hit its shoulder.
00049 #    def test_ik_joint_angle(self):
00050 #        lav = self.angle_vector_generator().next()
00051 #        for av in self.angle_vector_generator():
00052 #            print "av", av
00053 #            self.robot.setJointAnglesOfGroup("LARM", av, 2)
00054 #            self.robot.waitInterpolationOfGroup("LARM")
00055 #            pos1 = self.robot.getReferencePosition("LARM_JOINT5")
00056 #            rpy1 = self.robot.getReferenceRPY("LARM_JOINT5")
00057 #            if numpy.linalg.norm(numpy.array(lav) - numpy.array(av)) > 10:
00058 #                lav = av
00059 #            self.robot.setJointAnglesOfGroup("LARM", lav, 2)
00060 #            self.robot.waitInterpolationOfGroup("LARM")
00061 #            self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 5))
00062 #            self.robot.waitInterpolationOfGroup("LARM")
00063 #            pos2 = self.robot.getReferencePosition("LARM_JOINT5")
00064 #            rpy2 = self.robot.getReferenceRPY("LARM_JOINT5")
00065 #            print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
00066 #            print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))
00067 #            self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<1.0e-4) # 0.1 mm
00068 #            self.assertTrue(numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))<1.0e-3) # 0.001 rad = 0.057296 deg
00069 #            lav = av
00070 
00071     def target_point_generator(self,xmin,xmax,ymin,ymax,zmin,zmax,step):
00072         for x in range(xmin,xmax,step):
00073             for y in range(ymin,ymax,step):
00074                 for z in range (zmin,zmax,step):
00075                     yield [x/1000.0, y/1000.0, z/1000.0]
00076 
00077     def test_ik_left(self):
00078         self.pos_ik_test("LARM", 220,400, -5,320, 5,300, 100) # zmax=390
00079 
00080     def test_ik_right(self):
00081         self.pos_ik_test("RARM", 220,400, -320,5,  5,300, 100) # zmax=390
00082 
00083     def pos_ik_test(self, arm, xmin,xmax,ymin,ymax,zmin,zmax,step):
00084         arm_target = arm+"_JOINT5"
00085         rot1 = [[0,0,-1],[0,1,0],[1,0,0]]
00086         for pos1 in self.target_point_generator(xmin,xmax,ymin,ymax,zmin,zmax,step):
00087             self.robot.goInitial(tm=2)
00088             print self.robot.getReferenceRPY(arm_target)
00089             self.assertTrue(self.robot.setTargetPose(arm, pos1, euler_from_matrix(rot1), 1))
00090             self.robot.waitInterpolationOfGroup(arm)
00091             pos2 = self.robot.getReferencePosition(arm_target)
00092             rot2 = self.robot.getReferenceRotation(arm_target)
00093             print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
00094             print "rpy", rot1, rot2, numpy.linalg.norm(numpy.array(rot1)-numpy.array(rot2))
00095             self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<1.0e-4) # 0.1 mm
00096             self.assertTrue(numpy.linalg.norm(numpy.array(rot1)-numpy.array(rot2))<1.0e-3) # 0.001 rad = 0.057296 deg
00097 
00098     def test_set_target_pose(self):
00099         self.robot.setJointAnglesOfGroup("TORSO",[45],3)
00100         self.robot.waitInterpolationOfGroup("TORSO")
00101         ret = self.robot.setTargetPose("larm", [0.3255627368715471, 0.1823638733778268, 0.07462449717662004+0.2], [-3.0732189053889805, -1.5690225912054285, 3.0730289207320203], 5, "CHEST_JOINT0")
00102         self.assertTrue(ret)
00103 
00104 
00105 # for debug
00106 # $ python -m unittest test_hironx_ik.TestHiroIK.test_set_target_pose
00107 #
00108 if __name__ == '__main__':
00109     import rostest
00110     rostest.rosrun(PKG, 'test_hronx_ik', TestHiroIK) 


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Mon Oct 6 2014 07:19:42