pr2_calibration_estimation: pr2_calibration_estimation: single_transform

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.single_transform import SingleTransform
00044 from numpy import *
00045 
00046 
00047 
00048 class TestSingleTransform(unittest.TestCase):
00049 
00050     def test_free(self):
00051 
00052         st = SingleTransform([0, 0, 0, 0, 0, 0])
00053         free_list = st.calc_free( [0, 0, 1, 1, 0, 0] )
00054 
00055         self.assertEqual(free_list[0], False)
00056         self.assertEqual(free_list[1], False)
00057         self.assertEqual(free_list[2], True)
00058         self.assertEqual(free_list[3], True)
00059         self.assertEqual(free_list[4], False)
00060         self.assertEqual(free_list[5], False)
00061 
00062     def test_inflate(self):
00063         st = SingleTransform([0, 0, 0, 0, 0, 0])
00064         st.inflate( reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ))
00065         expected = numpy.matrix( [[ 1, 0, 0, 1],
00066                                   [ 0, 1, 0, 0],
00067                                   [ 0, 0, 1, 0],
00068                                   [ 0, 0, 0, 1]], float )
00069 
00070         print ""
00071         print st.transform
00072 
00073         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00074 
00075     def test_deflate(self):
00076         st = SingleTransform([0, 0, 0, 0, 0, 0])
00077         p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
00078         st.inflate(p)
00079         result = st.deflate()
00080         self.assertAlmostEqual(numpy.linalg.norm(p-result), 0.0, 6)
00081 
00082     def test_params_to_config(self):
00083         st = SingleTransform()
00084         p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
00085         config = st.params_to_config(p)
00086         self.assertAlmostEqual( config[0], 1, 6)
00087         self.assertAlmostEqual( config[1], 0, 6)
00088 
00089 
00090     def test_easy_trans_1(self):
00091         st = SingleTransform([1, 2, 3, 0, 0, 0])
00092         expected = numpy.matrix( [[ 1, 0, 0, 1],
00093                                   [ 0, 1, 0, 2],
00094                                   [ 0, 0, 1, 3],
00095                                   [ 0, 0, 0, 1]], float )
00096 
00097         print ""
00098         print st.transform
00099 
00100         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00101 
00102     def test_easy_rot_1(self):
00103         st = SingleTransform([0, 0, 0, 0, 0, pi/2])
00104         expected = numpy.matrix( [[ 0,-1, 0, 0],
00105                                   [ 1, 0, 0, 0],
00106                                   [ 0, 0, 1, 0],
00107                                   [ 0, 0, 0, 1]], float )
00108         print ""
00109         print st.transform
00110 
00111         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00112 
00113     def test_easy_rot_2(self):
00114         st = SingleTransform([0, 0, 0, 0, pi/2, 0])
00115         expected = numpy.matrix( [[ 0, 0, 1, 0],
00116                                   [ 0, 1, 0, 0],
00117                                   [-1, 0, 0, 0],
00118                                   [ 0, 0, 0, 1]], float )
00119         print ""
00120         print st.transform
00121 
00122         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00123 
00124     def test_easy_rot_3(self):
00125         st = SingleTransform([0, 0, 0, pi/2, 0, 0])
00126         expected = numpy.matrix( [[ 1, 0, 0, 0],
00127                                   [ 0, 0,-1, 0],
00128                                   [ 0, 1, 0, 0],
00129                                   [ 0, 0, 0, 1]], float )
00130         print ""
00131         print st.transform
00132 
00133         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00134 
00135     def test_length(self):
00136         st = SingleTransform([0, 0, 0, 0, 0, 0])
00137         self.assertEqual(st.get_length(), 6)
00138 
00139     def test_hard(self):
00140         sample_nums = range(1,6)
00141         params_filenames    = ["test/data/single_transform_data/params_%02u.txt" % n for n in sample_nums]
00142         transform_filenames = ["test/data/single_transform_data/transform_%02u.txt" % n for n in sample_nums]
00143 
00144         for params_filename, transform_filename in zip(params_filenames, transform_filenames):
00145             f_params = open(params_filename)
00146             f_transforms = open(transform_filename)
00147             param_elems     = [float(x) for x in f_params.read().split()]
00148             transform_elems = [float(x) for x in f_transforms.read().split()]
00149 
00150             st = SingleTransform(param_elems)
00151             expected_result = numpy.matrix(transform_elems)
00152             expected_result.shape = 4,4
00153 
00154             self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected_result), 0.0, 4, "Failed on %s" % params_filename)
00155 
00156 if __name__ == '__main__':
00157     import rostest
00158     rostest.unitrun('pr2_calibration_estimation', 'test_SingleTransform', TestSingleTransform, coverage_packages=['pr2_calibration_estimation.single_transform'])
00159
single_transform_unittest.py
