00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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:
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:
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:
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