pr2_calibration_estimation: pr2_calibration_estimation: tilting_laser

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 time
00041 import numpy
00042 
00043 from pr2_calibration_estimation.tilting_laser import TiltingLaser
00044 from numpy import *
00045 
00046 class TestTiltingLaser(unittest.TestCase):
00047 
00048     def test_free(self):
00049         tl = TiltingLaser( {"before_joint":[ 0, 0, 10, 0,    0, pi/2],
00050                             "after_joint": [20, 0,  0, 0, pi/2,    0],
00051                             "gearing":1} )
00052         free_list = tl.calc_free( {"before_joint":[ 0, 0, 1, 0, 0, 0 ],
00053                                    "after_joint": [ 1, 0, 0, 0, 0, 0 ],
00054                                    "gearing":0} )
00055 
00056         self.assertEqual(free_list[0], False)
00057         self.assertEqual(free_list[1], False)
00058         self.assertEqual(free_list[2], True)
00059         self.assertEqual(free_list[6], True)
00060         self.assertEqual(free_list[11],False)
00061         self.assertEqual(free_list[12],False)
00062 
00063     def test_inflate(self):
00064         tl = TiltingLaser( {"before_joint":[ 0, 0, 10, 0,    0, pi/2],
00065                             "after_joint": [20, 0,  0, 0, pi/2,    0],
00066                             "gearing":1 } )
00067         expected_before = numpy.matrix( [[ 0,-1, 0,  0],
00068                                          [ 1, 0, 0,  0],
00069                                          [ 0, 0, 1, 10],
00070                                          [ 0, 0, 0,  1]], float )
00071 
00072         expected_after  = numpy.matrix( [[ 0, 0, 1, 20],
00073                                          [ 0, 1, 0,  0],
00074                                          [-1, 0, 0,  0],
00075                                          [ 0, 0, 0,  1]], float )
00076 
00077         print ""
00078         print "Before Joint"
00079         print tl._before_joint.transform
00080         print "After Joint"
00081         print tl._after_joint.transform
00082 
00083         self.assertAlmostEqual(numpy.linalg.norm(tl._before_joint.transform - expected_before), 0.0, 6)
00084         self.assertAlmostEqual(numpy.linalg.norm(tl._after_joint.transform  - expected_after),  0.0, 6)
00085 
00086     def test_deflate(self):
00087         tl = TiltingLaser()
00088         p = reshape ( matrix( [ 1,  2,  3,  4,  5,  6,
00089                                 7,  8,  9, 10, 11, 12,
00090                                 13 ] ), (-1,1))
00091         tl.inflate(p)
00092         result = tl.deflate()
00093         #import code; code.interact(local=locals())
00094         self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
00095 
00096     def test_params_to_config(self):
00097         tl = TiltingLaser()
00098         p = reshape ( matrix( [  1,  2,  3,  4,  5,  6,
00099                                  7,  8,  9, 10, 11, 12, 13 ] ), (-1,1))
00100         config = tl.params_to_config(p)
00101         self.assertAlmostEqual(config["before_joint"][0], 1, 6)
00102         self.assertAlmostEqual(config["after_joint"][2], 9, 6)
00103 
00104     def test_project_point_easy_1(self):
00105         tl = TiltingLaser( {"before_joint": [  0, 0, 10, 0, 0, 0],
00106                             "after_joint" : [ 20, 0,  0, 0, 0, 0],
00107                             "gearing":1 } )
00108         result = tl.project_point_to_3D([0, 0, 0])
00109         expected = numpy.matrix( [20, 0, 10, 1] ).T
00110         print ""
00111         print result
00112         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
00113 
00114     def test_project_point_easy_2(self):
00115         tl = TiltingLaser( {"before_joint": [  0, 0, 10, 0, 0, 0],
00116                             "after_joint" : [ 20, 0,  0, 0, 0, 0],
00117                             "gearing":1 } )
00118         result = tl.project_point_to_3D([0, pi/2, 1])
00119         expected = numpy.matrix( [20, 1, 10, 1] ).T
00120         print ""
00121         print result
00122         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
00123 
00124     def test_project_point_easy_3(self):
00125         tl = TiltingLaser( {"before_joint": [  0,  0, 10, 0, 0, 0],
00126                             "after_joint" : [ 20,  0,  0, 0, 0, 0],
00127                             "gearing":1 } )
00128         result = tl.project_point_to_3D([pi/2, 0, 0])
00129         expected = numpy.matrix( [0 , 0,-10, 1] ).T
00130         print ""
00131         print result
00132         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
00133 
00134 
00135     def test_project_point_easy_4(self):
00136         tl = TiltingLaser( {"before_joint": [  0,  0, 10, 0, 0, 0],
00137                             "after_joint" : [ 20,  0,  0, 0, 0, 0],
00138                             "gearing":1 } )
00139         result = tl.project_point_to_3D([pi/2, -pi/2, 15])
00140         expected = numpy.matrix( [0 ,-15, -10, 1] ).T
00141         print ""
00142         print result
00143         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
00144 
00145 
00146     def test_project(self):
00147         tl = TiltingLaser( {"before_joint": [  0,  0, 10, 0, 0, 0],
00148                             "after_joint" : [ 20,  0,  0, 0, 0, 0],
00149                             "gearing":1 } )
00150 
00151         result = tl.project_to_3D( [ [0,    0, 0],
00152                                      [0,    0, 1],
00153                                      [pi/2, 0, 0],
00154                                      [pi/2, pi/2,1] ] )
00155 
00156         expected = numpy.matrix( [ [ 20, 21,   0,   0 ],
00157                                    [  0,  0,   0,   1 ],
00158                                    [ 10, 10, -10, -10 ],
00159                                    [  1,  1,   1,   1 ] ] )
00160 
00161         print
00162         print result
00163 
00164         self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
00165 
00166     def test_get_length(self):
00167         tl = TiltingLaser()
00168         self.assertEqual(tl.get_length(), 13)
00169 
00170 if __name__ == '__main__':
00171     import rostest
00172     rostest.unitrun('pr2_calibration_estimation', 'test_TiltingLaser', TestTiltingLaser, coverage_packages=['pr2_calibration_estimation.tilting_laser'])
00173
tilting_laser_unittest.py
