00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00036
00037 import sys
00038 import unittest
00039 import rospy
00040 import time
00041 import numpy
00042
00043 from pr2_calibration_estimation.dh_chain import chain_T
00044 from pr2_calibration_estimation.dh_chain import DhChain
00045 from sensor_msgs.msg import JointState
00046
00047 class LoadDhChain(unittest.TestCase):
00048 def setUp(self):
00049 print ""
00050 params = [ [ 0, 0, 1, 0 ],
00051 [ 0, 0, 1, 0 ],
00052 [ 0, 0, 1, 2 ] ]
00053
00054 self.dh_chain = DhChain({'dh':params, 'gearing':[1,1,1], 'cov':{'joint_angles':[1,1,1]}})
00055
00056 class TestDhChain(LoadDhChain):
00057 def test_init(self):
00058 pass
00059
00060
00061 def test_get_length(self):
00062 self.assertEqual(self.dh_chain.get_length(), 15)
00063
00064 def test_free(self):
00065 free_config = [ [ 0, 0, 1, 0 ],
00066 [ 0, 0, 1, 0 ],
00067 [ 0, 0, 1, 1 ] ]
00068
00069 free_list = self.dh_chain.calc_free({'dh':free_config, 'gearing':[0,0,0]})
00070 self.assertEqual(free_list[0], 0)
00071 self.assertEqual(free_list[1], 0)
00072 self.assertEqual(free_list[2], 1)
00073 self.assertEqual(free_list[11], 1)
00074
00075 def test_deflate(self):
00076 param_vec = self.dh_chain.deflate()
00077 self.assertEqual(param_vec[0,0], 0)
00078 self.assertEqual(param_vec[10,0], 1)
00079 self.assertEqual(param_vec[11,0], 2)
00080
00081 def test_inflate(self):
00082 param_vec = numpy.reshape(numpy.matrix(numpy.arange(12)),(3,4))
00083 self.dh_chain.inflate(param_vec)
00084 self.assertEqual(self.dh_chain._config[2,2], 10)
00085 self.assertEqual(self.dh_chain._config[2,3], 11)
00086
00087 def test_to_params(self):
00088 param_vec = self.dh_chain.deflate()
00089 param_vec[0,0] = 10
00090 config = self.dh_chain.params_to_config(param_vec)
00091 self.assertAlmostEqual(config['dh'][0][0], 10, 6)
00092 self.assertAlmostEqual(config['dh'][2][3], 2, 6)
00093
00094 def test_fk_easy1(self):
00095 chain_state = JointState()
00096 chain_state.position = [0, 0, 0]
00097 eef = self.dh_chain.fk(chain_state, 0)
00098 eef_expected = numpy.matrix( [[ 1, 0, 0, 1],
00099 [ 0, 1, 0, 0],
00100 [ 0, 0, 1, 0],
00101 [ 0, 0, 0, 1]] )
00102 self.assertAlmostEqual(numpy.linalg.norm(eef-eef_expected), 0.0, 6)
00103
00104 def test_fk_easy2(self):
00105 chain_state = JointState()
00106 chain_state.position = [numpy.pi/2, 0, 0]
00107 eef = self.dh_chain.fk(chain_state, 0)
00108 print eef
00109 eef_expected = numpy.matrix( [[ 0,-1, 0, 0],
00110 [ 1, 0, 0, 1],
00111 [ 0, 0, 1, 0],
00112 [ 0, 0, 0, 1]] )
00113 self.assertAlmostEqual(numpy.linalg.norm(eef-eef_expected), 0.0, 6)
00114
00115 def test_fk_easy3(self):
00116 chain_state = JointState()
00117 chain_state.position = [numpy.pi/2, numpy.pi/2, 0]
00118 eef = self.dh_chain.fk(chain_state, 1)
00119 print eef
00120 eef_expected = numpy.matrix( [[-1, 0, 0,-1],
00121 [ 0,-1, 0, 1],
00122 [ 0, 0, 1, 0],
00123 [ 0, 0, 0, 1]] )
00124 self.assertAlmostEqual(numpy.linalg.norm(eef-eef_expected), 0.0, 6)
00125
00126 def test_fk_easy4(self):
00127 chain_state = JointState()
00128 chain_state.position = [0, 0, 0]
00129 eef = self.dh_chain.fk(chain_state, -1)
00130 print eef
00131 eef_expected = numpy.matrix( [[ 1, 0, 0, 3],
00132 [ 0, 1, 0, 0],
00133 [ 0, 0, 1, 2],
00134 [ 0, 0, 0, 1]] )
00135 self.assertAlmostEqual(numpy.linalg.norm(eef-eef_expected), 0.0, 6)
00136
00137 class TestChainT(unittest.TestCase):
00138
00139 def test_easy_identity(self):
00140 dh_elems = numpy.matrix([0, 0, 0, 0])
00141 joints_elems = numpy.matrix([0])
00142
00143 result = chain_T( dh_elems, joints_elems)
00144 expected_result = numpy.matrix(numpy.eye(4))
00145 expected_result.shape = 4,4
00146
00147 self.assertAlmostEqual(numpy.linalg.norm(result-expected_result), 0.0, 4)
00148
00149 def test_easy_theta(self):
00150 dh_elems = numpy.matrix([numpy.pi, 0, 0, 0])
00151 joints_elems = numpy.matrix([0])
00152
00153 result = chain_T( dh_elems, joints_elems)
00154 expected_result = numpy.matrix( [[-1, 0, 0, 0],
00155 [ 0,-1, 0, 0],
00156 [ 0, 0, 1, 0],
00157 [ 0, 0, 0, 1]])
00158
00159
00160
00161 self.assertAlmostEqual(numpy.linalg.norm(result-expected_result), 0.0, 4)
00162
00163 def test_easy_joint(self):
00164 print "TEST EASY JOINT"
00165 dh_elems = numpy.matrix([0, 0, 0, 0], float)
00166 joints_elems = numpy.matrix([numpy.pi])
00167
00168 result = chain_T( dh_elems, joints_elems)
00169 expected_result = numpy.matrix( [[-1, 0, 0, 0],
00170 [ 0,-1, 0, 0],
00171 [ 0, 0, 1, 0],
00172 [ 0, 0, 0, 1]])
00173
00174 print result
00175 self.assertAlmostEqual(numpy.linalg.norm(result-expected_result), 0.0, 4)
00176 print "DONE WITH TEST"
00177
00178 def test_hard(self):
00179 sample_nums = range(1,6)
00180 dh_filenames = ["test/data/chain_T_data/dh_%02u.txt" % n for n in sample_nums]
00181 joints_filenames = ["test/data/chain_T_data/joints_%02u.txt" % n for n in sample_nums]
00182 output_filenames = ["test/data/chain_T_data/output_%02u.txt" % n for n in sample_nums]
00183
00184 for dh_filename, joints_filename, output_filename in zip(dh_filenames, joints_filenames, output_filenames):
00185 dh_elems = [float(x) for x in open(dh_filename).read().split()]
00186 joints_elems = [float(x) for x in open(joints_filename).read().split()]
00187 output_elems = [float(x) for x in open(output_filename).read().split()]
00188
00189 result = chain_T( numpy.array(dh_elems), numpy.array(joints_elems))
00190 expected_result = numpy.matrix(output_elems)
00191 expected_result.shape = 4,4
00192
00193 self.assertAlmostEqual(numpy.linalg.norm(result-expected_result), 0.0, 4, "Failed on %s" % dh_filename)
00194
00195 if __name__ == '__main__':
00196 import rostest
00197 rostest.unitrun('pr2_calibration_estimation', 'test_ChainT', TestChainT, coverage_packages=['pr2_calibration_estimation.dh_chain'])
00198 rostest.unitrun('pr2_calibration_estimation', 'test_DhChain', TestDhChain, coverage_packages=['pr2_calibration_estimation.dh_chain'])
