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('calibration_estimation')
00036
00037 import sys
00038 import unittest
00039 import rospy
00040 import numpy
00041 import yaml
00042
00043 from calibration_estimation.sensors.chain_sensor import ChainBundler, ChainSensor
00044
00045
00046
00047 from calibration_msgs.msg import *
00048 from sensor_msgs.msg import JointState, CameraInfo
00049
00050 from calibration_estimation.single_transform import SingleTransform
00051 from calibration_estimation.joint_chain import JointChain
00052 from calibration_estimation.camera import RectifiedCamera
00053 from calibration_estimation.tilting_laser import TiltingLaser
00054 from calibration_estimation.full_chain import FullChainCalcBlock
00055 from calibration_estimation.checkerboard import Checkerboard
00056 from calibration_estimation.urdf_params import UrdfParams
00057
00058 from numpy import *
00059
00060 def loadSystem():
00061 urdf = '''
00062 <robot>
00063 <link name="base_link"/>
00064 <joint name="j0" type="fixed">
00065 <origin xyz="0 0 0" rpy="0 0 0"/>
00066 <parent link="base_link"/>
00067 <child link="j0_link"/>
00068 </joint>
00069 <link name="j0_link"/>
00070 <joint name="j1" type="revolute">
00071 <axis xyz="0 0 1"/>
00072 <origin xyz="1 0 0" rpy="0 0 0"/>
00073 <parent link="j0_link"/>
00074 <child link="j1_link"/>
00075 </joint>
00076 <link name="j1_link"/>
00077 <joint name="j2" type="revolute">
00078 <axis xyz="0 0 1"/>
00079 <origin xyz="1 0 0" rpy="0 0 0"/>
00080 <parent link="j1_link"/>
00081 <child link="j2_link"/>
00082 </joint>
00083 <link name="j2_link"/>
00084 <joint name="j3" type="fixed">
00085 <origin xyz="0 0 0" rpy="0 0 0"/>
00086 <parent link="j2_link"/>
00087 <child link="j3_link"/>
00088 </joint>
00089 <link name="j3_link"/>
00090 </robot>
00091 '''
00092 config = yaml.load('''
00093 sensors:
00094 chains:
00095 chainA:
00096 sensor_id: chainA
00097 root: j0_link
00098 tip: j1_link
00099 cov:
00100 joint_angles: [1]
00101 gearing: [1.0]
00102 chainB:
00103 sensor_id: chainB
00104 root: j0_link
00105 tip: j2_link
00106 cov:
00107 joint_angles: [1, 1]
00108 gearing: [1.0, 1.0]
00109 rectified_cams: {}
00110 tilting_lasers: {}
00111 transforms:
00112 chainA_cb: [0, 0, 0, 0, 0, 0]
00113 chainB_cb: [0, 0, 0, 0, 0, 0]
00114 checkerboards:
00115 boardA:
00116 corners_x: 2
00117 corners_y: 2
00118 spacing_x: 1
00119 spacing_y: 1
00120 ''')
00121
00122 return config["sensors"]["chains"], UrdfParams(urdf, config)
00123
00124
00125 class TestChainBundler(unittest.TestCase):
00126 def test_basic_match(self):
00127 config, robot_params = loadSystem()
00128
00129 bundler = ChainBundler(config.values())
00130
00131 M_robot = RobotMeasurement( target_id = "targetA",
00132 chain_id = "chainA",
00133 M_cam = [],
00134 M_chain = [ ChainMeasurement(chain_id="chainA") ])
00135
00136 blocks = bundler.build_blocks(M_robot)
00137
00138 self.assertEqual( len(blocks), 1)
00139 block = blocks[0]
00140 self.assertEqual( block._M_chain.chain_id, "chainA")
00141 self.assertEqual( block._target_id, "targetA")
00142
00143 def test_basic_no_match(self):
00144 config, robot_params = loadSystem()
00145
00146 bundler = ChainBundler(config.values())
00147
00148 M_robot = RobotMeasurement( target_id = "targetA",
00149 chain_id = "chainA",
00150 M_chain = [ ChainMeasurement(chain_id="chainB") ])
00151
00152 blocks = bundler.build_blocks(M_robot)
00153
00154 self.assertEqual( len(blocks), 0)
00155
00156 class TestChainSensor(unittest.TestCase):
00157 def test_cov(self):
00158 config, robot_params = loadSystem()
00159 block = ChainSensor(config["chainA"],
00160 ChainMeasurement(chain_id="chainA",
00161 chain_state=JointState(position=[0]) ),
00162 "boardA")
00163 block.update_config(robot_params)
00164 cov = block.compute_cov(None)
00165 print cov
00166
00167 self.assertAlmostEqual(cov[0,0], 0.0, 6)
00168 self.assertAlmostEqual(cov[1,0], 0.0, 6)
00169 self.assertAlmostEqual(cov[1,1], 1.0, 6)
00170 self.assertAlmostEqual(cov[4,4], 4.0, 6)
00171
00172 def test_update1(self):
00173 config, robot_params = loadSystem()
00174 block = ChainSensor(config["chainA"],
00175 ChainMeasurement(chain_id="chainA",
00176 chain_state=JointState(position=[0]) ),
00177 "boardA")
00178 block.update_config(robot_params)
00179
00180 target = matrix([[1, 2, 1, 2],
00181 [0, 0, 1, 1],
00182 [0, 0, 0, 0],
00183 [1, 1, 1, 1]])
00184
00185 h = block.compute_expected(target)
00186 z = block.get_measurement()
00187 r = block.compute_residual(target)
00188
00189 self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
00190
00191 print "z=\n",z
00192 print "target=\n",target
00193
00194 self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
00195 self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
00196
00197 def test_update2(self):
00198 config, robot_params = loadSystem()
00199 block = ChainSensor(config["chainA"],
00200 ChainMeasurement(chain_id="chainA",
00201 chain_state=JointState(position=[numpy.pi / 2.0]) ),
00202 "boardA")
00203 block.update_config(robot_params)
00204
00205 target = matrix([[0, 0,-1,-1],
00206 [1, 2, 1, 2],
00207 [0, 0, 0, 0],
00208 [1, 1, 1, 1]])
00209
00210 h = block.compute_expected(target)
00211 z = block.get_measurement()
00212 r = block.compute_residual(target)
00213
00214 self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
00215
00216 print "z=\n",z
00217 print "target=\n",target
00218
00219 self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
00220 self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
00221
00222 def test_update3(self):
00223 config, robot_params = loadSystem()
00224 block = ChainSensor(config["chainB"],
00225 ChainMeasurement(chain_id="chainB",
00226 chain_state=JointState(position=[0.0, 0.0]) ),
00227 "boardA")
00228 block.update_config(robot_params)
00229
00230 target = matrix([[2, 3, 2, 3],
00231 [0, 0, 1, 1],
00232 [0, 0, 0, 0],
00233 [1, 1, 1, 1]])
00234
00235 h = block.compute_expected(target)
00236 z = block.get_measurement()
00237 r = block.compute_residual(target)
00238
00239 self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
00240
00241 print "z=\n",z
00242 print "target=\n",target
00243
00244 self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
00245 self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
00246
00247 def test_sparsity(self):
00248 config, robot_params = loadSystem()
00249 block = ChainSensor(config["chainA"],
00250 ChainMeasurement(chain_id="chainA",
00251 chain_state=JointState(position=[numpy.pi / 2.0]) ),
00252 "boardA")
00253 block.update_config(robot_params)
00254 sparsity = block.build_sparsity_dict()
00255 self.assertEqual(sparsity['transforms']['j0'], [1,1,1,1,1,1])
00256 self.assertEqual(sparsity['transforms']['j1'], [1,1,1,1,1,1])
00257 self.assertEqual(sparsity['chains']['chainA'], {'gearing':[1]})
00258
00259 if __name__ == '__main__':
00260 import rostest
00261 rostest.unitrun('calibration_estimation', 'test_ChainBundler', TestChainBundler)
00262 rostest.unitrun('calibration_estimation', 'test_ChainSensor', TestChainSensor)