00001
00002
00003 PKG = 'hrpsys_tutorials'
00004 import roslib; roslib.load_manifest(PKG)
00005 import rospkg
00006
00007 import numpy
00008 import unittest
00009
00010 import sys; sys.path.insert (0, roslib.packages.get_pkg_dir('hrpsys')+'/scripts');
00011 from hrpsys import HrpsysConfigurator
00012
00013 class TestHRP4C(unittest.TestCase):
00014
00015 @classmethod
00016 def setUpClass(self):
00017 self.robot = HrpsysConfigurator()
00018 self.robot.init("HRP4C(Robot)0", rospkg.RosPack().get_path("hrpsys")+"/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl")
00019
00020 def test_walk(self):
00021 self.robot.loadPattern(rospkg.RosPack().get_path("hrpsys")+"/share/hrpsys/samples/HRP4C/data/walk2m", 1.0)
00022 self.robot.waitInterpolation()
00023
00024
00025 print self.robot.getReferencePosition("WAIST")
00026 print self.robot.getReferenceRotation("WAIST")
00027 self.assertTrue(numpy.linalg.norm(self.robot.getReferencePosition("WAIST") - numpy.array([0,0,0.78415])) < 1.0e-6)
00028 self.assertTrue(numpy.array_equal(self.robot.getReferenceRotation("WAIST"),numpy.identity(3)))
00029
00030 def test_ik(self):
00031 self.robot.seq_svc.addJointGroup("LARM", ["L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P", "L_WRIST_Y", "L_WRIST_R"])
00032 lav = [60,0,0,-90,0,0]
00033 for av in [[60, 0, 0, -80, 0, 0],[30, 30, 0,-80, 0, 0],
00034 [40,40,20, -20, 0, 0],[30, 30,20,-80,20,20]]:
00035 self.robot.setJointAnglesOfGroup("LARM", av, 5)
00036 self.robot.waitInterpolationOfGroup("LARM")
00037 pos1 = self.robot.getReferencePosition("L_WRIST_R")
00038 rpy1 = self.robot.getReferenceRPY("L_WRIST_R")
00039 self.robot.setJointAnglesOfGroup("LARM", lav, 5)
00040 self.robot.waitInterpolationOfGroup("LARM")
00041 self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 10))
00042 self.robot.waitInterpolationOfGroup("LARM")
00043 pos2 = self.robot.getReferencePosition("L_WRIST_R")
00044 rpy2 = self.robot.getReferenceRPY("L_WRIST_R")
00045 print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
00046 print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))
00047 self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<5.0e-3)
00048 self.assertTrue(numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))<5.0e-3)
00049 lav = av
00050
00051
00052
00053 if __name__ == '__main__':
00054 import rostest
00055 rostest.rosrun(PKG, 'test_hrp4c', TestHRP4C)
00056
00057
00058
00059
