pr2_calibration_estimation: robot_params

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('pr2_calibration_estimation')
00036 
00037 import sys
00038 import unittest
00039 import rospy
00040 import numpy
00041 import yaml
00042 from pr2_calibration_estimation.robot_params import RobotParams
00043 from numpy import *
00044 
00045 def loadConfigDict():
00046 
00047     config_yaml = '''
00048 dh_chains:
00049   chain1:
00050     dh:
00051     - [0, 1, 2, 3]
00052     - [4, 5, 6, 7]
00053     gearing: [71,72]
00054     cov:
00055       joint_angles: [73,74]
00056   chain2:
00057     dh:
00058     - [10, 11, 12, 13]
00059     - [14, 15, 16, 17]
00060     - [18, 19, 20, 21]
00061     gearing: [22,23,24]
00062     cov:
00063       joint_angles: [25,26,27]
00064 
00065 tilting_lasers:
00066   laserA:
00067     before_joint: [0, 0, 0, 0, 0, 0]
00068     after_joint:  [0, 0, 0, 0, 0, 0]
00069     gearing: 1
00070 
00071 transforms:
00072   transformA: [0, 0, 0, 0, 0, 0]
00073 
00074 rectified_cams:
00075   camA:
00076     baseline_shift: 0.0
00077     f_shift: 0.0
00078     cx_shift: 0.0
00079     cy_shift: 0.0
00080     cov:
00081       u: 1
00082       v: 2
00083 
00084 checkerboards:
00085   boardA:
00086     corners_x: 2
00087     corners_y: 3
00088     spacing_x: 10
00089     spacing_y: 20
00090 '''
00091     config_dict = yaml.load(config_yaml)
00092 
00093     #import code; code.interact(local=locals())
00094     return config_dict
00095 
00096 def loadParamVec():
00097     params = [ # DH Chains
00098                  # Chain 2
00099                    0, 0, 0, 0,
00100                    0,10, 0, 0,
00101                    0, 0, 0, 0,
00102                    0, 0, 0,
00103                  # Chain 1
00104                  -10, 0, 0, 0,
00105                    0, 0, 0, 0,
00106                    0, 0,
00107                # Tilting Lasers
00108                  # Laser A
00109                    0,20, 0, 0, 0, 0,
00110                    0, 0,30, 0, 0, 0,
00111                    1,
00112                # Transforms
00113                  # transformA
00114                   40, 0, 0, 0, 0, 0,
00115                # Rectified Cams
00116                  # Cam A
00117                    4, 0, 0, 0,
00118                # Checkerboards
00119                  # Board A
00120                    30, 40]
00121     param_vec = matrix( params, float).T
00122     return param_vec
00123 
00124 def loadFreeDict():
00125     free_yaml = '''
00126 dh_chains:
00127   chain1:
00128     dh:
00129     - [0, 0, 0, 1]
00130     - [0, 0, 0, 0]
00131     gearing: [0,0]
00132   chain2:
00133     dh:
00134     - [ 1,  0,  0,  0]
00135     - [ 0,  0,  0,  0]
00136     - [ 0,  0,  0,  0]
00137     gearing: [0, 0, 0]
00138 
00139 tilting_lasers:
00140   laserA:
00141     before_joint: [0, 0, 1, 0, 0, 0]
00142     after_joint:  [0, 0, 0, 0, 0, 0]
00143     gearing: 0
00144 
00145 transforms:
00146   transformA: [0, 0, 1, 0, 0, 0]
00147 
00148 rectified_cams:
00149   camA:
00150     baseline_shift: 0
00151     f_shift: 0
00152     cx_shift: 0
00153     cy_shift: 0
00154 
00155 checkerboards:
00156   boardA:
00157     spacing_x: 1
00158     spacing_y: 0
00159 '''
00160     free_dict = yaml.load(free_yaml)
00161 
00162     #import code; code.interact(local=locals())
00163     return free_dict
00164 
00165 
00166 class TestRobotParams(unittest.TestCase):
00167     def test_configure(self):
00168         robot_params = RobotParams()
00169 
00170         robot_params.configure(loadConfigDict())
00171 
00172         print "laser start: %u" % robot_params.tilting_lasers["laserA"].start
00173         print "laser end:   %u" % robot_params.tilting_lasers["laserA"].end
00174 
00175         print "transform start: %u" % robot_params.transforms["transformA"].start
00176         print "transform end:   %u" % robot_params.transforms["transformA"].end
00177 
00178         print "cam start: %u" % robot_params.rectified_cams["camA"].start
00179         print "cam end:   %u" % robot_params.rectified_cams["camA"].end
00180 
00181         # ****** DH Chains ******
00182         self.assertEqual(robot_params.dh_chains["chain1"].start, 15)
00183         self.assertEqual(robot_params.dh_chains["chain1"].end,   25)
00184 
00185         self.assertEqual(robot_params.dh_chains["chain2"].start,  0)
00186         self.assertEqual(robot_params.dh_chains["chain2"].end,   15)
00187 
00188         # ****** Tilting Lasers ******
00189         self.assertEqual(robot_params.tilting_lasers["laserA"].start, 25)
00190         self.assertEqual(robot_params.tilting_lasers["laserA"].end,   38)
00191 
00192         # ****** Transforms ******
00193         self.assertEqual(robot_params.transforms["transformA"].start, 38)
00194         self.assertEqual(robot_params.transforms["transformA"].end,   44)
00195 
00196         # ****** Rectified Cams ******
00197         self.assertEqual(robot_params.rectified_cams["camA"].start, 44)
00198         self.assertEqual(robot_params.rectified_cams["camA"].end,   48)
00199 
00200         # ****** Checkerboards ******
00201         self.assertEqual(robot_params.checkerboards["boardA"].start, 48)
00202         self.assertEqual(robot_params.checkerboards["boardA"].end,   50)
00203 
00204     def test_inflate(self):
00205         robot_params = RobotParams()
00206         robot_params.configure(loadConfigDict())
00207         robot_params.inflate(loadParamVec())
00208 
00209         self.assertEqual(robot_params.dh_chains["chain1"]._config[0,0], -10)
00210         self.assertEqual(robot_params.dh_chains["chain2"]._config[1,1],  10)
00211         self.assertEqual(robot_params.tilting_lasers["laserA"]._before_joint.transform[1,3], 20)
00212         self.assertEqual(robot_params.tilting_lasers["laserA"]._after_joint.transform[2,3],  30)
00213         self.assertEqual(robot_params.transforms["transformA"].transform[0,3],  40)
00214         self.assertEqual(robot_params.rectified_cams["camA"]._config['baseline_shift'],  4)
00215         self.assertEqual(robot_params.checkerboards["boardA"]._spacing_x,  30)
00216 
00217     def test_params_to_config(self):
00218         robot_params = RobotParams()
00219         robot_params.configure(loadConfigDict())
00220         config = robot_params.params_to_config(loadParamVec())
00221         self.assertAlmostEqual(config["dh_chains"]["chain2"]['dh'][1][1], 10, 6)
00222         self.assertAlmostEqual(config["rectified_cams"]["camA"]["baseline_shift"], 4, 6)
00223         self.assertAlmostEqual(config["tilting_lasers"]["laserA"]["before_joint"][1], 20, 6)
00224         self.assertAlmostEqual(config["checkerboards"]["boardA"]["spacing_y"], 40)
00225 
00226 
00227     def test_free(self):
00228         robot_params = RobotParams()
00229         robot_params.configure(loadConfigDict())
00230         free_list = robot_params.calc_free(loadFreeDict())
00231         print free_list
00232         self.assertEqual(free_list[0], True)
00233         self.assertEqual(free_list[1], False)
00234         self.assertEqual(free_list[18], True)
00235         self.assertEqual(free_list[19], False)
00236         self.assertEqual(free_list[27], True)
00237 
00238     def test_deflate(self):
00239         robot_params = RobotParams()
00240         robot_params.configure(loadConfigDict())
00241 
00242         p = loadParamVec()
00243         robot_params.inflate(p)
00244 
00245         result = robot_params.deflate()
00246         self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
00247 
00248 if __name__ == '__main__':
00249     import rostest
00250     rostest.unitrun('pr2_calibration_estimation', 'test_RobotParams', TestRobotParams, coverage_packages=['pr2_calibration_estimation.robot_params'])
robot_params_unittest.py
