chain_sensor_unittest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
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 #from pr2_calibration_estimation.blocks.camera_chain import CameraChainCalcBlock
00045 #from pr2_calibration_estimation.blocks.camera_chain import CameraChainRobotParamsBlock
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([[0.5, 1.5, 0.5, 1.5],
00181                          [0.5, 0.5, -0.5, -0.5],
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.5, 0.5,1.5,1.5],
00206                          [-0.5, 0.5, -0.5, 0.5],
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([[1.5, 2.5, 1.5, 2.5],
00231                          [0.5, 0.5, -0.5, -0.5],
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)


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:33