angles: utest.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #*********************************************************************
00003 # Software License Agreement (BSD License)
00004 #
00005 #  Copyright (c) 2015, Bossa Nova Robotics
00006 #  All rights reserved.
00007 #
00008 #  Redistribution and use in source and binary forms, with or without
00009 #  modification, are permitted provided that the following conditions
00010 #  are met:
00011 #
00012 #   * Redistributions of source code must retain the above copyright
00013 #     notice, this list of conditions and the following disclaimer.
00014 #   * Redistributions in binary form must reproduce the above
00015 #     copyright notice, this list of conditions and the following
00016 #     disclaimer in the documentation and/or other materials provided
00017 #     with the distribution.
00018 #   * Neither the name of the Bossa Nova Robotics nor the names of its
00019 #     contributors may be used to endorse or promote products derived
00020 #     from this software without specific prior written permission.
00021 #
00022 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES
00029 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #  POSSIBILITY OF SUCH DAMAGE.
00034 #********************************************************************/
00035 from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits
00036 from angles import _find_min_max_delta
00037 import sys
00038 import unittest
00039 from math import pi, fabs
00040 
00041 ## A sample python unit test
00042 class TestAngles(unittest.TestCase):
00043     def test_shortestDistanceWithLimits(self):
00044         result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25)
00045         self.assertFalse(result)
00046 
00047         result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25)
00048         self.assertFalse(result)
00049 
00050         result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25)
00051         self.assertTrue(result)
00052         self.assertAlmostEqual(shortest_angle, -2*pi+1.0)
00053 
00054         result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25)
00055         self.assertTrue(result)
00056         self.assertAlmostEqual(shortest_angle, 0)
00057 
00058         result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25)
00059         self.assertFalse(result)
00060         self.assertAlmostEqual(shortest_angle, -0.5)
00061 
00062         result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25)
00063         self.assertFalse(result)
00064         self.assertAlmostEqual(shortest_angle, 0.5)
00065 
00066         result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25)
00067         self.assertFalse(result)
00068         self.assertAlmostEqual(shortest_angle, -2*pi+0.4)
00069 
00070         result, shortest_angle = shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25)
00071         self.assertFalse(result)
00072         self.assertAlmostEqual(shortest_angle,2*pi-0.4)
00073 
00074         result, shortest_angle = shortest_angular_distance_with_limits(0.2,0,0.25,-0.25)
00075         self.assertFalse(result)
00076         self.assertAlmostEqual(shortest_angle,2*pi-0.2)
00077 
00078         result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25)
00079         self.assertFalse(result)
00080         self.assertAlmostEqual(shortest_angle,-2*pi+0.2)
00081 
00082         result, shortest_angle = shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25)
00083         self.assertTrue(result)
00084         self.assertAlmostEqual(shortest_angle,-0.25)
00085 
00086         result, shortest_angle = shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25)
00087         self.assertTrue(result)
00088         self.assertAlmostEqual(shortest_angle,-2*pi+0.75)
00089 
00090         result, shortest_angle = shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25)
00091         self.assertTrue(result)
00092         self.assertAlmostEqual(shortest_angle,-2*pi+0.5+0.2500001)
00093 
00094         result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25)
00095         self.assertFalse(result)
00096 
00097         result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25)
00098         self.assertFalse(result)
00099 
00100         result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3)
00101         self.assertFalse(result)
00102 
00103         result, shortest_angle = shortest_angular_distance_with_limits(-0.6, pi*3.0/4.0,-0.25,0.3)
00104         self.assertFalse(result)
00105 
00106         result, shortest_angle = shortest_angular_distance_with_limits(-pi, pi,-pi,pi)
00107         self.assertTrue(result)
00108         self.assertAlmostEqual(shortest_angle,0.0)
00109 
00110     def test_normalize_angle_positive(self):
00111         self.assertAlmostEqual(0, normalize_angle_positive(0))
00112         self.assertAlmostEqual(pi, normalize_angle_positive(pi))
00113         self.assertAlmostEqual(0, normalize_angle_positive(2*pi))
00114         self.assertAlmostEqual(pi, normalize_angle_positive(3*pi))
00115         self.assertAlmostEqual(0, normalize_angle_positive(4*pi))
00116 
00117         self.assertAlmostEqual(0, normalize_angle_positive(-0))
00118         self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
00119         self.assertAlmostEqual(0, normalize_angle_positive(-2*pi))
00120         self.assertAlmostEqual(pi, normalize_angle_positive(-3*pi))
00121         self.assertAlmostEqual(0, normalize_angle_positive(-4*pi))
00122 
00123         self.assertAlmostEqual(0, normalize_angle_positive(-0))
00124         self.assertAlmostEqual(3*pi/2, normalize_angle_positive(-pi/2))
00125         self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
00126         self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
00127         self.assertAlmostEqual(0, normalize_angle_positive(-4*pi/2))
00128 
00129         self.assertAlmostEqual(0, normalize_angle_positive(0))
00130         self.assertAlmostEqual(pi/2, normalize_angle_positive(pi/2))
00131         self.assertAlmostEqual(pi/2, normalize_angle_positive(5*pi/2))
00132         self.assertAlmostEqual(pi/2, normalize_angle_positive(9*pi/2))
00133         self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
00134 
00135     def test_normalize_angle(self):
00136         self.assertAlmostEqual(0, normalize_angle(0))
00137         self.assertAlmostEqual(pi, normalize_angle(pi))
00138         self.assertAlmostEqual(0, normalize_angle(2*pi))
00139         self.assertAlmostEqual(pi, normalize_angle(3*pi))
00140         self.assertAlmostEqual(0, normalize_angle(4*pi))
00141 
00142         self.assertAlmostEqual(0, normalize_angle(-0))
00143         self.assertAlmostEqual(pi, normalize_angle(-pi))
00144         self.assertAlmostEqual(0, normalize_angle(-2*pi))
00145         self.assertAlmostEqual(pi, normalize_angle(-3*pi))
00146         self.assertAlmostEqual(0, normalize_angle(-4*pi))
00147 
00148         self.assertAlmostEqual(0, normalize_angle(-0))
00149         self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2))
00150         self.assertAlmostEqual(pi, normalize_angle(-pi))
00151         self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
00152         self.assertAlmostEqual(0, normalize_angle(-4*pi/2))
00153 
00154         self.assertAlmostEqual(0, normalize_angle(0))
00155         self.assertAlmostEqual(pi/2, normalize_angle(pi/2))
00156         self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2))
00157         self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2))
00158         self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
00159  
00160     def test_shortest_angular_distance(self):
00161          self.assertAlmostEqual(pi/2, shortest_angular_distance(0, pi/2))
00162          self.assertAlmostEqual(-pi/2, shortest_angular_distance(0, -pi/2))
00163          self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi/2, 0))
00164          self.assertAlmostEqual(pi/2, shortest_angular_distance(-pi/2, 0))
00165 
00166          self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi, pi/2))
00167          self.assertAlmostEqual(pi/2, shortest_angular_distance(pi, -pi/2))
00168          self.assertAlmostEqual(pi/2, shortest_angular_distance(pi/2, pi))
00169          self.assertAlmostEqual(-pi/2, shortest_angular_distance(-pi/2, pi))
00170 
00171          self.assertAlmostEqual(-pi/2, shortest_angular_distance(5*pi, pi/2))
00172          self.assertAlmostEqual(pi/2, shortest_angular_distance(7*pi, -pi/2))
00173          self.assertAlmostEqual(pi/2, shortest_angular_distance(9*pi/2, pi))
00174          self.assertAlmostEqual(pi/2, shortest_angular_distance(-3*pi/2, pi))
00175 
00176          # Backside wrapping
00177          self.assertAlmostEqual(-pi/2, shortest_angular_distance(-3*pi/4, 3*pi/4))
00178          self.assertAlmostEqual(pi/2, shortest_angular_distance(3*pi/4, -3*pi/4))
00179 
00180     def test_two_pi_complement(self):
00181          epsilon = 1e-9
00182          self.assertAlmostEqual(two_pi_complement(0), 2*pi)
00183          self.assertAlmostEqual(two_pi_complement(2*pi), 0)
00184          self.assertAlmostEqual(two_pi_complement(-2*pi), 0)
00185          self.assertAlmostEqual(two_pi_complement(2*pi-epsilon), -epsilon)
00186          self.assertAlmostEqual(two_pi_complement(-2*pi+epsilon), epsilon)
00187          self.assertAlmostEqual(two_pi_complement(pi/2), -3*pi/2)
00188          self.assertAlmostEqual(two_pi_complement(pi), -pi)
00189          self.assertAlmostEqual(two_pi_complement(-pi), pi)
00190          self.assertAlmostEqual(two_pi_complement(-pi/2), 3*pi/2)
00191 
00192          self.assertAlmostEqual(two_pi_complement(3*pi), -pi)
00193          self.assertAlmostEqual(two_pi_complement(-3.0*pi), pi)
00194          self.assertAlmostEqual(two_pi_complement(-5.0*pi/2.0), 3*pi/2)
00195 
00196     def test_find_min_max_delta(self):
00197          epsilon = 1e-9
00198          # Straight forward full range
00199          flag, min_delta, max_delta = _find_min_max_delta( 0, -pi, pi)
00200          self.assertTrue(flag)
00201          self.assertAlmostEqual(min_delta, -pi)
00202          self.assertAlmostEqual(max_delta, pi)
00203 
00204          # pi/2 Full Range
00205          flag, min_delta, max_delta = _find_min_max_delta( pi/2, -pi, pi)
00206          self.assertTrue(flag)
00207          self.assertAlmostEqual(min_delta, -3*pi/2)
00208          self.assertAlmostEqual(max_delta, pi/2)
00209 
00210          # -pi/2 Full range
00211          flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi, pi)
00212          self.assertTrue(flag)
00213          self.assertAlmostEqual(min_delta, -pi/2)
00214          self.assertAlmostEqual(max_delta, 3*pi/2)
00215 
00216          # Straight forward partial range
00217          flag, min_delta, max_delta = _find_min_max_delta( 0, -pi/2, pi/2)
00218          self.assertTrue(flag)
00219          self.assertAlmostEqual(min_delta, -pi/2)
00220          self.assertAlmostEqual(max_delta, pi/2)
00221 
00222          # pi/4 Partial Range
00223          flag, min_delta, max_delta = _find_min_max_delta( pi/4, -pi/2, pi/2)
00224          self.assertTrue(flag)
00225          self.assertAlmostEqual(min_delta, -3*pi/4)
00226          self.assertAlmostEqual(max_delta, pi/4)
00227 
00228          # -pi/4 Partial Range
00229          flag, min_delta, max_delta = _find_min_max_delta( -pi/4, -pi/2, pi/2)
00230          self.assertTrue(flag)
00231          self.assertAlmostEqual(min_delta, -pi/4)
00232          self.assertAlmostEqual(max_delta, 3*pi/4)
00233 
00234          # bump stop negative full range
00235          flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
00236          self.assertTrue(flag)
00237          self.assertTrue((fabs(min_delta) <= epsilon and fabs(max_delta - 2*pi) <= epsilon) or (fabs(min_delta+2*pi) <= epsilon and fabs(max_delta) <= epsilon))
00238          self.assertAlmostEqual(min_delta, 0.0)
00239          self.assertAlmostEqual(max_delta, 2*pi)
00240 
00241          flag, min_delta, max_delta = _find_min_max_delta(-0.25,0.25,-0.25)
00242          self.assertTrue(flag)
00243          self.assertAlmostEqual(min_delta, -2*pi+0.5)
00244          self.assertAlmostEqual(max_delta, 0.0)
00245 
00246          # bump stop positive full range
00247          flag, min_delta, max_delta = _find_min_max_delta( pi-epsilon, -pi, pi)
00248          self.assertTrue(flag)
00249          #self.assertTrue((fabs(min_delta) <= epsilon and fabs(max_delta - 2*pi) <= epsilon) or (fabs(min_delta+2*pi) <= epsilon and fabs(max_delta) <= epsilon))
00250          self.assertAlmostEqual(min_delta, -2*pi+epsilon)
00251          self.assertAlmostEqual(max_delta, epsilon)
00252 
00253          # bump stop negative partial range
00254          flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
00255          self.assertTrue(flag)
00256          self.assertAlmostEqual(min_delta, 0)
00257          self.assertAlmostEqual(max_delta, 2*pi)
00258 
00259          # bump stop positive partial range
00260          flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi/2, pi/2)
00261          self.assertTrue(flag)
00262          self.assertAlmostEqual(min_delta, 0.0)
00263          self.assertAlmostEqual(max_delta, pi)
00264 
00265          #Test out of range negative
00266          flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi/2, pi/2)
00267          self.assertFalse(flag)
00268          #Test out of range postive
00269          flag, min_delta, max_delta = _find_min_max_delta( pi, -pi/2, pi/2)
00270          self.assertFalse(flag)
00271 
00272          # pi/4 Partial Range
00273          flag, min_delta, max_delta = _find_min_max_delta( 3*pi/4, pi/2, -pi/2)
00274          self.assertTrue(flag)
00275          self.assertAlmostEqual(min_delta, -pi/4)
00276          self.assertAlmostEqual(max_delta, 3*pi/4)
00277 
00278 if __name__ == '__main__':
00279     import rosunit
00280     rosunit.unitrun('angles', 'test_python_angles', TestAngles)