utest.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #*********************************************************************
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2015, Bossa Nova Robotics
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of the Bossa Nova Robotics nor the names of its
19 # contributors may be used to endorse or promote products derived
20 # from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 #********************************************************************/
35 from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits, shortest_angular_distance_with_large_limits
36 from angles import _find_min_max_delta
37 import sys
38 import unittest
39 from math import pi, fabs
40 
41 
42 class TestAngles(unittest.TestCase):
43  def test_shortestDistanceWithLimits(self):
44  result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25)
45  self.assertFalse(result)
46 
47  result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25)
48  self.assertFalse(result)
49 
50  result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25)
51  self.assertTrue(result)
52  self.assertAlmostEqual(shortest_angle, -2*pi+1.0)
53 
54  result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25)
55  self.assertTrue(result)
56  self.assertAlmostEqual(shortest_angle, 0)
57 
58  result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25)
59  self.assertFalse(result)
60  self.assertAlmostEqual(shortest_angle, -0.5)
61 
62  result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25)
63  self.assertFalse(result)
64  self.assertAlmostEqual(shortest_angle, 0.5)
65 
66  result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25)
67  self.assertFalse(result)
68  self.assertAlmostEqual(shortest_angle, -2*pi+0.4)
69 
70  result, shortest_angle = shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25)
71  self.assertFalse(result)
72  self.assertAlmostEqual(shortest_angle,2*pi-0.4)
73 
74  result, shortest_angle = shortest_angular_distance_with_limits(0.2,0,0.25,-0.25)
75  self.assertFalse(result)
76  self.assertAlmostEqual(shortest_angle,2*pi-0.2)
77 
78  result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25)
79  self.assertFalse(result)
80  self.assertAlmostEqual(shortest_angle,-2*pi+0.2)
81 
82  result, shortest_angle = shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25)
83  self.assertTrue(result)
84  self.assertAlmostEqual(shortest_angle,-0.25)
85 
86  result, shortest_angle = shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25)
87  self.assertTrue(result)
88  self.assertAlmostEqual(shortest_angle,-2*pi+0.75)
89 
90  result, shortest_angle = shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25)
91  self.assertTrue(result)
92  self.assertAlmostEqual(shortest_angle,-2*pi+0.5+0.2500001)
93 
94  result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25)
95  self.assertFalse(result)
96 
97  result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25)
98  self.assertFalse(result)
99 
100  result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3)
101  self.assertFalse(result)
102 
103  result, shortest_angle = shortest_angular_distance_with_limits(-0.6, pi*3.0/4.0,-0.25,0.3)
104  self.assertFalse(result)
105 
106  result, shortest_angle = shortest_angular_distance_with_limits(-pi, pi,-pi,pi)
107  self.assertTrue(result)
108  self.assertAlmostEqual(shortest_angle,0.0)
109 
110  def test_shortestDistanceWithLargeLimits(self):
111  # 'delta' is valid
112  result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 2*pi)
113  self.assertTrue(result)
114  self.assertAlmostEqual(shortest_angle, 0.5*pi)
115 
116  # 'delta' is not valid, but 'delta_2pi' is
117  result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 0.1*pi)
118  self.assertTrue(result)
119  self.assertAlmostEqual(shortest_angle, -1.5*pi)
120 
121  # neither 'delta' nor 'delta_2pi' are valid
122  result, shortest_angle = shortest_angular_distance_with_large_limits(2*pi, pi, 2*pi-0.1, 2*pi+0.1)
123  self.assertFalse(result)
124 
125  # start position outside limits
126  result, shortest_angle = shortest_angular_distance_with_large_limits(10.5*pi, 0, -2*pi, 2*pi)
127  self.assertFalse(result)
128 
129  # invalid limits (lower > upper)
130  result, shortest_angle = shortest_angular_distance_with_large_limits(0, 0.1, 2*pi, -2*pi)
131  self.assertFalse(result)
132 
133  # specific test case
134  result, shortest_angle = shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*pi, 20*pi)
135  self.assertTrue(result)
136  self.assertAlmostEqual(shortest_angle, 0.000493)
137 
138  def test_normalize_angle_positive(self):
139  self.assertAlmostEqual(0, normalize_angle_positive(0))
140  self.assertAlmostEqual(pi, normalize_angle_positive(pi))
141  self.assertAlmostEqual(0, normalize_angle_positive(2*pi))
142  self.assertAlmostEqual(pi, normalize_angle_positive(3*pi))
143  self.assertAlmostEqual(0, normalize_angle_positive(4*pi))
144 
145  self.assertAlmostEqual(0, normalize_angle_positive(-0))
146  self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
147  self.assertAlmostEqual(0, normalize_angle_positive(-2*pi))
148  self.assertAlmostEqual(pi, normalize_angle_positive(-3*pi))
149  self.assertAlmostEqual(0, normalize_angle_positive(-4*pi))
150 
151  self.assertAlmostEqual(0, normalize_angle_positive(-0))
152  self.assertAlmostEqual(3*pi/2, normalize_angle_positive(-pi/2))
153  self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
154  self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
155  self.assertAlmostEqual(0, normalize_angle_positive(-4*pi/2))
156 
157  self.assertAlmostEqual(0, normalize_angle_positive(0))
158  self.assertAlmostEqual(pi/2, normalize_angle_positive(pi/2))
159  self.assertAlmostEqual(pi/2, normalize_angle_positive(5*pi/2))
160  self.assertAlmostEqual(pi/2, normalize_angle_positive(9*pi/2))
161  self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
162 
163  def test_normalize_angle(self):
164  self.assertAlmostEqual(0, normalize_angle(0))
165  self.assertAlmostEqual(pi, normalize_angle(pi))
166  self.assertAlmostEqual(0, normalize_angle(2*pi))
167  self.assertAlmostEqual(pi, normalize_angle(3*pi))
168  self.assertAlmostEqual(0, normalize_angle(4*pi))
169 
170  self.assertAlmostEqual(0, normalize_angle(-0))
171  self.assertAlmostEqual(pi, normalize_angle(-pi))
172  self.assertAlmostEqual(0, normalize_angle(-2*pi))
173  self.assertAlmostEqual(pi, normalize_angle(-3*pi))
174  self.assertAlmostEqual(0, normalize_angle(-4*pi))
175 
176  self.assertAlmostEqual(0, normalize_angle(-0))
177  self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2))
178  self.assertAlmostEqual(pi, normalize_angle(-pi))
179  self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
180  self.assertAlmostEqual(0, normalize_angle(-4*pi/2))
181 
182  self.assertAlmostEqual(0, normalize_angle(0))
183  self.assertAlmostEqual(pi/2, normalize_angle(pi/2))
184  self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2))
185  self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2))
186  self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
187 
188  def test_shortest_angular_distance(self):
189  self.assertAlmostEqual(pi/2, shortest_angular_distance(0, pi/2))
190  self.assertAlmostEqual(-pi/2, shortest_angular_distance(0, -pi/2))
191  self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi/2, 0))
192  self.assertAlmostEqual(pi/2, shortest_angular_distance(-pi/2, 0))
193 
194  self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi, pi/2))
195  self.assertAlmostEqual(pi/2, shortest_angular_distance(pi, -pi/2))
196  self.assertAlmostEqual(pi/2, shortest_angular_distance(pi/2, pi))
197  self.assertAlmostEqual(-pi/2, shortest_angular_distance(-pi/2, pi))
198 
199  self.assertAlmostEqual(-pi/2, shortest_angular_distance(5*pi, pi/2))
200  self.assertAlmostEqual(pi/2, shortest_angular_distance(7*pi, -pi/2))
201  self.assertAlmostEqual(pi/2, shortest_angular_distance(9*pi/2, pi))
202  self.assertAlmostEqual(pi/2, shortest_angular_distance(-3*pi/2, pi))
203 
204  # Backside wrapping
205  self.assertAlmostEqual(-pi/2, shortest_angular_distance(-3*pi/4, 3*pi/4))
206  self.assertAlmostEqual(pi/2, shortest_angular_distance(3*pi/4, -3*pi/4))
207 
208  def test_two_pi_complement(self):
209  epsilon = 1e-9
210  self.assertAlmostEqual(two_pi_complement(0), 2*pi)
211  self.assertAlmostEqual(two_pi_complement(2*pi), 0)
212  self.assertAlmostEqual(two_pi_complement(-2*pi), 0)
213  self.assertAlmostEqual(two_pi_complement(2*pi-epsilon), -epsilon)
214  self.assertAlmostEqual(two_pi_complement(-2*pi+epsilon), epsilon)
215  self.assertAlmostEqual(two_pi_complement(pi/2), -3*pi/2)
216  self.assertAlmostEqual(two_pi_complement(pi), -pi)
217  self.assertAlmostEqual(two_pi_complement(-pi), pi)
218  self.assertAlmostEqual(two_pi_complement(-pi/2), 3*pi/2)
219 
220  self.assertAlmostEqual(two_pi_complement(3*pi), -pi)
221  self.assertAlmostEqual(two_pi_complement(-3.0*pi), pi)
222  self.assertAlmostEqual(two_pi_complement(-5.0*pi/2.0), 3*pi/2)
223 
224  def test_find_min_max_delta(self):
225  epsilon = 1e-9
226  # Straight forward full range
227  flag, min_delta, max_delta = _find_min_max_delta( 0, -pi, pi)
228  self.assertTrue(flag)
229  self.assertAlmostEqual(min_delta, -pi)
230  self.assertAlmostEqual(max_delta, pi)
231 
232  # pi/2 Full Range
233  flag, min_delta, max_delta = _find_min_max_delta( pi/2, -pi, pi)
234  self.assertTrue(flag)
235  self.assertAlmostEqual(min_delta, -3*pi/2)
236  self.assertAlmostEqual(max_delta, pi/2)
237 
238  # -pi/2 Full range
239  flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi, pi)
240  self.assertTrue(flag)
241  self.assertAlmostEqual(min_delta, -pi/2)
242  self.assertAlmostEqual(max_delta, 3*pi/2)
243 
244  # Straight forward partial range
245  flag, min_delta, max_delta = _find_min_max_delta( 0, -pi/2, pi/2)
246  self.assertTrue(flag)
247  self.assertAlmostEqual(min_delta, -pi/2)
248  self.assertAlmostEqual(max_delta, pi/2)
249 
250  # pi/4 Partial Range
251  flag, min_delta, max_delta = _find_min_max_delta( pi/4, -pi/2, pi/2)
252  self.assertTrue(flag)
253  self.assertAlmostEqual(min_delta, -3*pi/4)
254  self.assertAlmostEqual(max_delta, pi/4)
255 
256  # -pi/4 Partial Range
257  flag, min_delta, max_delta = _find_min_max_delta( -pi/4, -pi/2, pi/2)
258  self.assertTrue(flag)
259  self.assertAlmostEqual(min_delta, -pi/4)
260  self.assertAlmostEqual(max_delta, 3*pi/4)
261 
262  # bump stop negative full range
263  flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
264  self.assertTrue(flag)
265  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))
266  self.assertAlmostEqual(min_delta, 0.0)
267  self.assertAlmostEqual(max_delta, 2*pi)
268 
269  flag, min_delta, max_delta = _find_min_max_delta(-0.25,0.25,-0.25)
270  self.assertTrue(flag)
271  self.assertAlmostEqual(min_delta, -2*pi+0.5)
272  self.assertAlmostEqual(max_delta, 0.0)
273 
274  # bump stop positive full range
275  flag, min_delta, max_delta = _find_min_max_delta( pi-epsilon, -pi, pi)
276  self.assertTrue(flag)
277  #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))
278  self.assertAlmostEqual(min_delta, -2*pi+epsilon)
279  self.assertAlmostEqual(max_delta, epsilon)
280 
281  # bump stop negative partial range
282  flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
283  self.assertTrue(flag)
284  self.assertAlmostEqual(min_delta, 0)
285  self.assertAlmostEqual(max_delta, 2*pi)
286 
287  # bump stop positive partial range
288  flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi/2, pi/2)
289  self.assertTrue(flag)
290  self.assertAlmostEqual(min_delta, 0.0)
291  self.assertAlmostEqual(max_delta, pi)
292 
293  #Test out of range negative
294  flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi/2, pi/2)
295  self.assertFalse(flag)
296  #Test out of range postive
297  flag, min_delta, max_delta = _find_min_max_delta( pi, -pi/2, pi/2)
298  self.assertFalse(flag)
299 
300  # pi/4 Partial Range
301  flag, min_delta, max_delta = _find_min_max_delta( 3*pi/4, pi/2, -pi/2)
302  self.assertTrue(flag)
303  self.assertAlmostEqual(min_delta, -pi/4)
304  self.assertAlmostEqual(max_delta, 3*pi/4)
305 
306 if __name__ == '__main__':
307  import rosunit
308  rosunit.unitrun('angles', 'test_python_angles', TestAngles)
utest.TestAngles.test_normalize_angle
def test_normalize_angle(self)
Definition: utest.py:163
angles::shortest_angular_distance
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
Definition: angles.h:135
angles::normalize_angle
static double normalize_angle(double angle)
normalize
Definition: angles.h:115
angles._find_min_max_delta
def _find_min_max_delta(from_angle, left_limit, right_limit)
Definition: __init__.py:75
angles::shortest_angular_distance_with_limits
static bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by...
Definition: angles.h:321
angles::normalize_angle_positive
static double normalize_angle_positive(double angle)
normalize_angle_positive
Definition: angles.h:100
utest.TestAngles
A sample python unit test.
Definition: utest.py:42
utest.TestAngles.test_shortest_angular_distance
def test_shortest_angular_distance(self)
Definition: utest.py:188
utest.TestAngles.test_find_min_max_delta
def test_find_min_max_delta(self)
Definition: utest.py:224
utest.TestAngles.test_two_pi_complement
def test_two_pi_complement(self)
Definition: utest.py:208
angles::shortest_angular_distance_with_large_limits
static bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from from_angle to to_angle, making sure it does not violate limits specified by le...
Definition: angles.h:254
angles::two_pi_complement
static double two_pi_complement(double angle)
returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circle.
Definition: angles.h:149
utest.TestAngles.test_shortestDistanceWithLargeLimits
def test_shortestDistanceWithLargeLimits(self)
Definition: utest.py:110
utest.TestAngles.test_normalize_angle_positive
def test_normalize_angle_positive(self)
Definition: utest.py:138
utest.TestAngles.test_shortestDistanceWithLimits
def test_shortestDistanceWithLimits(self)
Definition: utest.py:43

angles
Author(s): John Hsu , Tully Foote
autogenerated on Tue Mar 1 2022 23:45:04