maxwell_arm_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 sensor_msgs.msg import JointState
00012 
00013 import yaml
00014 from pr2_calibration_launch.srv import FkTest
00015 from numpy import *
00016 import numpy
00017 
00018 class LoadData(unittest.TestCase):
00019     def setUp(self):
00020         config = yaml.load(open(rospy.get_param('config_file')))
00021         self.robot_params = UrdfParams(rospy.get_param('robot_description'),config)
00022 
00023         rospy.wait_for_service('fk', 3.0)
00024         self._fk_ref = rospy.ServiceProxy('fk', FkTest)
00025 
00026     def loadCommands(self, param_name):
00027         command_str = rospy.get_param(param_name)
00028         cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
00029         return cmds
00030 
00031     def getExpected(self, root, tip, cmd):
00032         resp = self._fk_ref(root, tip, cmd)
00033         T = matrix(zeros((4,4)), float)
00034         T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
00035         T[3,3] = 1.0;
00036         T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
00037         return T
00038 
00039 class TestMaxwellFk(LoadData):
00040 
00041     def run_test(self, full_chain, root, tip, cmds):
00042         for cmd in cmds:
00043             print "On Command: %s" % cmd
00044 
00045             expected_T = self.getExpected(root, tip, cmd)
00046             chain_state = JointState(position=cmd)
00047             actual_T = full_chain.calc_block.fk(chain_state)
00048 
00049             print "Expected_T:"
00050             print expected_T
00051             print "Actual T:"
00052             print actual_T
00053 
00054             self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 2)
00055 
00056     def test_arm_fk(self):
00057         print "\n\n\n"
00058         full_chain = FullChainRobotParams('arm_chain', 'arm_wrist_roll_link')
00059         full_chain.update_config(self.robot_params)
00060 
00061         cmds = self.loadCommands('arm_commands')
00062 
00063         self.run_test(full_chain, 'torso_link', 'arm_wrist_roll_link', cmds)
00064 
00065 if __name__ == '__main__':
00066     import rostest
00067     rospy.init_node("fk_test")
00068     rostest.unitrun('maxwell_calibration', 'test_MaxwellFk', TestMaxwellFk)


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