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 numpy
00041 import yaml
00042
00043 from pr2_calibration_estimation.sensors.chain_sensor import ChainBundler, ChainSensor
00044
00045
00046
00047
00048
00049 from calibration_msgs.msg import *
00050 from sensor_msgs.msg import JointState, CameraInfo
00051
00052 from pr2_calibration_estimation.single_transform import SingleTransform
00053 from pr2_calibration_estimation.dh_chain import DhChain
00054 from pr2_calibration_estimation.camera import RectifiedCamera
00055 from pr2_calibration_estimation.tilting_laser import TiltingLaser
00056 from pr2_calibration_estimation.full_chain import FullChainCalcBlock
00057 from pr2_calibration_estimation.checkerboard import Checkerboard
00058
00059 from numpy import *
00060
00061 def loadConfigList():
00062
00063 config_yaml = '''
00064 - chain_id: chainA
00065 before_chain: [transformA]
00066 dh_link_num: 1
00067 after_chain: [transformB]
00068 - chain_id: chainB
00069 before_chain: [transformC]
00070 after_chain: [transformD]
00071 dh_link_num: 6
00072 '''
00073 config_dict = yaml.load(config_yaml)
00074
00075 return config_dict
00076
00077 class TestChainBundler(unittest.TestCase):
00078 def test_basic_match(self):
00079 config_list = loadConfigList()
00080
00081 bundler = ChainBundler(config_list)
00082
00083 M_robot = RobotMeasurement( target_id = "targetA",
00084 chain_id = "chainA",
00085 M_cam = [],
00086 M_chain = [ ChainMeasurement(chain_id="chainA") ])
00087
00088 blocks = bundler.build_blocks(M_robot)
00089
00090 self.assertEqual( len(blocks), 1)
00091 block = blocks[0]
00092 self.assertEqual( block._M_chain.chain_id, "chainA")
00093 self.assertEqual( block._target_id, "targetA")
00094
00095 def test_basic_no_match(self):
00096 config_list = loadConfigList()
00097
00098 bundler = ChainBundler(config_list)
00099
00100 M_robot = RobotMeasurement( target_id = "targetA",
00101 chain_id = "chainA",
00102 M_chain = [ ChainMeasurement(chain_id="chainB") ])
00103
00104 blocks = bundler.build_blocks(M_robot)
00105
00106 self.assertEqual( len(blocks), 0)
00107
00108 from pr2_calibration_estimation.robot_params import RobotParams
00109
00110 class TestChainSensor(unittest.TestCase):
00111 def load(self):
00112 config = yaml.load('''
00113 chain_id: chainA
00114 before_chain: [transformA]
00115 dh_link_num: 1
00116 after_chain: [transformB]
00117 ''')
00118
00119 robot_params = RobotParams()
00120 robot_params.configure( yaml.load('''
00121 dh_chains:
00122 chainA:
00123 dh:
00124 - [ 0, 0, 1, 0 ]
00125 gearing: [1]
00126 cov:
00127 joint_angles: [1]
00128 tilting_lasers: {}
00129 rectified_cams: {}
00130 transforms:
00131 transformA: [0, 0, 0, 0, 0, 0]
00132 transformB: [0, 0, 0, 0, 0, 0]
00133 checkerboards:
00134 boardA:
00135 corners_x: 2
00136 corners_y: 2
00137 spacing_x: 1
00138 spacing_y: 1
00139 ''' ) )
00140 return config, robot_params
00141
00142 def test_cov(self):
00143 config, robot_params = self.load()
00144 block = ChainSensor(config,
00145 ChainMeasurement(chain_id="chainA",
00146 chain_state=JointState(position=[0]) ),
00147 "boardA")
00148 block.update_config(robot_params)
00149 cov = block.compute_cov(None)
00150
00151 self.assertAlmostEqual(cov[0,0], 0.0, 6)
00152 self.assertAlmostEqual(cov[1,0], 0.0, 6)
00153 self.assertAlmostEqual(cov[1,1], 1.0, 6)
00154 self.assertAlmostEqual(cov[4,4], 4.0, 6)
00155
00156 def test_update1(self):
00157 config, robot_params = self.load()
00158 block = ChainSensor(config,
00159 ChainMeasurement(chain_id="chainA",
00160 chain_state=JointState(position=[0]) ),
00161 "boardA")
00162 block.update_config(robot_params)
00163
00164 target = matrix([[1, 2, 1, 2],
00165 [0, 0, 1, 1],
00166 [0, 0, 0, 0],
00167 [1, 1, 1, 1]])
00168
00169 h = block.compute_expected(target)
00170 z = block.get_measurement()
00171 r = block.compute_residual(target)
00172
00173 self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
00174
00175 print "z=\n",z
00176 print "target=\n",target
00177
00178 self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
00179 self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
00180
00181 def test_update2(self):
00182 config, robot_params = self.load()
00183 block = ChainSensor(config,
00184 ChainMeasurement(chain_id="chainA",
00185 chain_state=JointState(position=[numpy.pi / 2.0]) ),
00186 "boardA")
00187 block.update_config(robot_params)
00188
00189 target = matrix([[0, 0,-1,-1],
00190 [1, 2, 1, 2],
00191 [0, 0, 0, 0],
00192 [1, 1, 1, 1]])
00193
00194 h = block.compute_expected(target)
00195 z = block.get_measurement()
00196 r = block.compute_residual(target)
00197
00198 self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
00199
00200 print "z=\n",z
00201 print "target=\n",target
00202
00203 self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
00204 self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
00205
00206 def test_sparsity(self):
00207 config, robot_params = self.load()
00208 block = ChainSensor(config,
00209 ChainMeasurement(chain_id="chainA",
00210 chain_state=JointState(position=[numpy.pi / 2.0]) ),
00211 "boardA")
00212 block.update_config(robot_params)
00213 sparsity = block.build_sparsity_dict()
00214 self.assertEqual(sparsity['transforms']['transformA'], [1,1,1,1,1,1])
00215 self.assertEqual(sparsity['transforms']['transformB'], [1,1,1,1,1,1])
00216 self.assertEqual(sparsity['dh_chains']['chainA'], {'dh':[[1,1,1,1]], 'gearing':[1]})
00217
00218 if __name__ == '__main__':
00219 import rostest
00220 rostest.unitrun('pr2_calibration_estimation', 'test_ChainBundler', TestChainBundler)
00221
00222 rostest.unitrun('pr2_calibration_estimation', 'test_ChainSensor', TestChainSensor)
