maxwell_head_fk_unittest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('maxwell_calibration')
00003 
00004 import sys
00005 import unittest
00006 import rospy
00007 import time
00008 
00009 from calibration_estimation.full_chain import FullChainRobotParams
00010 from calibration_estimation.urdf_params import UrdfParams
00011 from calibration_estimation.single_transform import SingleTransform
00012 from sensor_msgs.msg import JointState
00013 
00014 import yaml
00015 from pr2_calibration_launch.srv import FkTest
00016 from numpy import *
00017 import numpy
00018 
00019 class LoadData(unittest.TestCase):
00020     def setUp(self):
00021         config = yaml.load(open(rospy.get_param('config_file')))
00022         self.robot_params = UrdfParams(rospy.get_param('robot_description'), config)
00023 
00024         rospy.wait_for_service('fk', 3.0)
00025         self._fk_ref = rospy.ServiceProxy('fk', FkTest)
00026 
00027     def loadCommands(self, param_name):
00028         command_str = rospy.get_param(param_name)
00029         cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
00030         return cmds
00031 
00032     def getExpected(self, root, tip, cmd):
00033         resp = self._fk_ref(root, tip, cmd)
00034         T = matrix(zeros((4,4)), float)
00035         T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
00036         T[3,3] = 1.0;
00037         T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
00038         return T
00039 
00040 class TestMaxwellFk(LoadData):
00041 
00042     def run_test(self, full_chain, root, tip, cmds):
00043         for cmd in cmds:
00044             print "On Command: %s" % cmd
00045 
00046             expected_T = self.getExpected(root, tip, cmd)
00047             chain_state = JointState(position=cmd)
00048             actual_T = full_chain.calc_block.fk(chain_state)
00049 
00050             print "Expected_T:"
00051             print expected_T
00052             print "Actual T:"
00053             print actual_T
00054 
00055             self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 2)
00056 
00057     def test_head_tilt_link(self):
00058         full_chain = FullChainRobotParams('head_chain','head_tilt_link')
00059         full_chain.update_config(self.robot_params)
00060         cmds = self.loadCommands('head_commands')
00061         self.run_test(full_chain, 'torso_link', 'head_tilt_link', cmds)
00062 
00063     def test_kinect_head_def(self):
00064         full_chain = FullChainRobotParams('head_chain','head_camera_rgb_link')
00065         full_chain.update_config(self.robot_params)
00066         cmds = self.loadCommands('head_commands')
00067         self.run_test(full_chain, 'torso_link', 'head_camera_rgb_link', cmds)
00068 
00069     def test_kinect_head_optical(self):
00070         full_chain = FullChainRobotParams('head_chain','head_camera_rgb_optical_frame')
00071         full_chain.update_config(self.robot_params)
00072         cmds = self.loadCommands('head_commands')
00073         self.run_test(full_chain, 'torso_link', 'head_camera_rgb_optical_frame', cmds)
00074 
00075 if __name__ == '__main__':
00076     import rostest
00077     rospy.init_node("fk_test")
00078     rostest.unitrun('maxwell_calibration', 'test_maxwellFk', TestMaxwellFk)


maxwell_calibration
Author(s): Michael Ferguson
autogenerated on Wed Aug 26 2015 12:32:37