__init__.py
Go to the documentation of this file.
00001 #*********************************************************************
00002 # Software License Agreement (BSD License)
00003 #
00004 #  Copyright (c) 2015, Bossa Nova Robotics
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 the Bossa Nova Robotics 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 from math import fmod, pi, fabs
00036 
00037 def normalize_angle_positive(angle):
00038     """ Normalizes the angle to be 0 to 2*pi
00039         It takes and returns radians. """
00040     return fmod(fmod(angle, 2.0*pi) + 2.0*pi, 2.0*pi)
00041 
00042 def normalize_angle(angle):
00043     """ Normalizes the angle to be -pi to +pi
00044         It takes and returns radians."""
00045     a = normalize_angle_positive(angle)
00046     if a > pi:
00047         a -= 2.0 *pi
00048     return a
00049 
00050 def shortest_angular_distance(from_angle, to_angle):
00051     """ Given 2 angles, this returns the shortest angular
00052         difference.  The inputs and ouputs are of course radians.
00053  
00054         The result would always be -pi <= result <= pi. Adding the result
00055         to "from" will always get you an equivelent angle to "to".
00056     """
00057     return normalize_angle(to_angle-from_angle)
00058 
00059 def two_pi_complement(angle):
00060     """ returns the angle in [-2*pi, 2*pi]  going the other way along the unit circle.
00061         \param angle The angle to which you want to turn in the range [-2*pi, 2*pi]
00062             E.g. two_pi_complement(-pi/4) returns 7_pi/4
00063                  two_pi_complement(pi/4) returns -7*pi/4
00064     """
00065     #check input conditions
00066     if angle > 2*pi or angle < -2.0*pi:
00067         angle = fmod(angle, 2.0*pi)
00068     if angle < 0:
00069         return 2*pi+angle
00070     elif angle > 0:
00071         return -2*pi+angle
00072 
00073     return 2*pi
00074 
00075 def _find_min_max_delta(from_angle, left_limit, right_limit):
00076     """ This function is only intended for internal use and not intended for external use. 
00077         If you do use it, read the documentation very carefully. 
00078         
00079         Returns the min and max amount (in radians) that can be moved 
00080         from "from" angle to "left_limit" and "right_limit".
00081         
00082         \param from - "from" angle - must lie in [-pi, pi)
00083         \param left_limit - left limit of valid interval for angular position 
00084             - must lie in [-pi, pi], left and right limits are specified on 
00085               the unit circle w.r.t to a reference pointing inwards
00086         \param right_limit - right limit of valid interval for angular position 
00087             - must lie in [-pi, pi], left and right limits are specified on 
00088               the unit circle w.r.t to a reference pointing inwards      
00089         \return (valid, min, max) - angle in radians that can be moved from "from" position before hitting the joint stop
00090                  valid is False  if "from" angle does not lie in the interval [left_limit,right_limit]
00091         """
00092     delta = [0]*4
00093     delta[0] = shortest_angular_distance(from_angle,left_limit)
00094     delta[1] = shortest_angular_distance(from_angle,right_limit)
00095     delta[2] = two_pi_complement(delta[0])
00096     delta[3] = two_pi_complement(delta[1])
00097 
00098     if delta[0] == 0:
00099         return True, delta[0], max(delta[1], delta[3])
00100     
00101     if delta[1] == 0:
00102         return True, min(delta[0], delta[2]), delta[1]
00103 
00104     delta_min = delta[0]
00105     delta_min_2pi = delta[2]
00106     if delta[2] < delta_min:
00107         delta_min = delta[2]
00108         delta_min_2pi = delta[0]
00109   
00110     delta_max = delta[1]
00111     delta_max_2pi = delta[3]
00112     if delta[3] > delta_max:
00113         delta_max = delta[3]
00114         delta_max_2pi = delta[1]
00115 
00116     # printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi)
00117     if (delta_min <= delta_max_2pi) or (delta_max >= delta_min_2pi):
00118         if left_limit == -pi and right_limit == pi:
00119             return (True, delta_max_2pi, delta_min_2pi)
00120         else:
00121             return (False, delta_max_2pi, delta_min_2pi)
00122     return True, delta_min, delta_max
00123 
00124 def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, right_limit):
00125     """ Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by left_limit and right_limit.
00126         The valid interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25] is a 0.5 radians wide interval that contains 0.
00127         But [0.25,-0.25] is a 2*pi-0.5 wide interval that contains pi (but not 0).
00128         The value of shortest_angle is the angular difference between "from" and "to" that lies within the defined valid interval.
00129         
00130         E.g. shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25) returns 2*pi-1.0
00131              shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25) returns None since -0.5 and 0.5 do not lie in the interval [-0.25,0.25]
00132              
00133         \param left_limit - left limit of valid interval for angular position 
00134             - must lie in [-pi, pi], left and right limits are specified on 
00135               the unit circle w.r.t to a reference pointing inwards
00136         \param right_limit - right limit of valid interval for angular position 
00137             - must lie in [-pi, pi], left and right limits are specified on 
00138               the unit circle w.r.t to a reference pointing inwards   
00139         \returns valid_flag, shortest_angle 
00140     """
00141     min_delta = -2*pi
00142     max_delta = 2*pi
00143     min_delta_to = -2*pi
00144     max_delta_to = 2*pi
00145     flag, min_delta, max_delta = _find_min_max_delta(from_angle, left_limit, right_limit)
00146     delta = shortest_angular_distance(from_angle,to_angle)
00147     delta_mod_2pi  = two_pi_complement(delta)
00148 
00149     if flag: #from position is within the limits
00150         if delta >= min_delta and delta <= max_delta:
00151             return True, delta
00152         elif delta_mod_2pi >= min_delta and delta_mod_2pi <= max_delta:
00153           return True, delta_mod_2pi
00154         else: #to position is outside the limits
00155             flag, min_delta_to, max_delta_to = _find_min_max_delta(to_angle,left_limit,right_limit)
00156             if fabs(min_delta_to) < fabs(max_delta_to):
00157                 shortest_angle = max(delta, delta_mod_2pi)
00158             elif fabs(min_delta_to) > fabs(max_delta_to):
00159                 shortest_angle = min(delta,delta_mod_2pi)
00160             else:
00161                 if fabs(delta) < fabs(delta_mod_2pi):
00162                     shortest_angle = delta
00163                 else:
00164                     shortest_angle = delta_mod_2pi
00165             return False, shortest_angle
00166     else: # from position is outside the limits
00167         flag, min_delta_to, max_delta_to = _find_min_max_delta(to_angle,left_limit,right_limit)
00168 
00169         if fabs(min_delta) < fabs(max_delta):
00170             shortest_angle = min(delta,delta_mod_2pi)
00171         elif fabs(min_delta) > fabs(max_delta):
00172           shortest_angle = max(delta,delta_mod_2pi)
00173         else:
00174             if fabs(delta) < fabs(delta_mod_2pi):
00175                 shortest_angle = delta
00176             else:
00177                 shortest_angle = delta_mod_2pi
00178         return False, shortest_angle
00179 


angles
Author(s): John Hsu
autogenerated on Sun Apr 16 2017 02:17:29