35 from math
import fmod, pi, fabs
38 """ Normalizes the angle to be 0 to 2*pi
39 It takes and returns radians. """
40 return angle % (2.0*pi)
43 """ Normalizes the angle to be -pi to +pi
44 It takes and returns radians."""
51 """ Given 2 angles, this returns the shortest angular
52 difference. The inputs and ouputs are of course radians.
54 The result would always be -pi <= result <= pi. Adding the result
55 to "from" will always get you an equivelent angle to "to".
60 """ returns the angle in [-2*pi, 2*pi] going the other way along the unit circle.
61 \param angle The angle to which you want to turn in the range [-2*pi, 2*pi]
62 E.g. two_pi_complement(-pi/4) returns 7_pi/4
63 two_pi_complement(pi/4) returns -7*pi/4
66 if angle > 2*pi
or angle < -2.0*pi:
67 angle = fmod(angle, 2.0*pi)
76 """ This function is only intended for internal use and not intended for external use.
77 If you do use it, read the documentation very carefully.
79 Returns the min and max amount (in radians) that can be moved
80 from "from" angle to "left_limit" and "right_limit".
82 \param from - "from" angle - must lie in [-pi, pi)
83 \param left_limit - left limit of valid interval for angular position
84 - must lie in [-pi, pi], left and right limits are specified on
85 the unit circle w.r.t to a reference pointing inwards
86 \param right_limit - right limit of valid interval for angular position
87 - must lie in [-pi, pi], left and right limits are specified on
88 the unit circle w.r.t to a reference pointing inwards
89 \return (valid, min, max) - angle in radians that can be moved from "from" position before hitting the joint stop
90 valid is False if "from" angle does not lie in the interval [left_limit,right_limit]
99 return True, delta[0], max(delta[1], delta[3])
102 return True, min(delta[0], delta[2]), delta[1]
105 delta_min_2pi = delta[2]
106 if delta[2] < delta_min:
108 delta_min_2pi = delta[0]
111 delta_max_2pi = delta[3]
112 if delta[3] > delta_max:
114 delta_max_2pi = delta[1]
117 if (delta_min <= delta_max_2pi)
or (delta_max >= delta_min_2pi):
118 if left_limit == -pi
and right_limit == pi:
119 return (
True, delta_max_2pi, delta_min_2pi)
121 return (
False, delta_max_2pi, delta_min_2pi)
122 return True, delta_min, delta_max
125 """ Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by left_limit and right_limit.
126 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.
127 But [0.25,-0.25] is a 2*pi-0.5 wide interval that contains pi (but not 0).
128 The value of shortest_angle is the angular difference between "from" and "to" that lies within the defined valid interval.
130 E.g. shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25) returns 2*pi-1.0
131 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]
133 \param left_limit - left limit of valid interval for angular position
134 - must lie in [-pi, pi], left and right limits are specified on
135 the unit circle w.r.t to a reference pointing inwards
136 \param right_limit - right limit of valid interval for angular position
137 - must lie in [-pi, pi], left and right limits are specified on
138 the unit circle w.r.t to a reference pointing inwards
139 \returns valid_flag, shortest_angle
150 if delta >= min_delta
and delta <= max_delta:
152 elif delta_mod_2pi >= min_delta
and delta_mod_2pi <= max_delta:
153 return True, delta_mod_2pi
156 if fabs(min_delta_to) < fabs(max_delta_to):
157 shortest_angle = max(delta, delta_mod_2pi)
158 elif fabs(min_delta_to) > fabs(max_delta_to):
159 shortest_angle = min(delta,delta_mod_2pi)
161 if fabs(delta) < fabs(delta_mod_2pi):
162 shortest_angle = delta
164 shortest_angle = delta_mod_2pi
165 return False, shortest_angle
169 if fabs(min_delta) < fabs(max_delta):
170 shortest_angle = min(delta,delta_mod_2pi)
171 elif fabs(min_delta) > fabs(max_delta):
172 shortest_angle = max(delta,delta_mod_2pi)
174 if fabs(delta) < fabs(delta_mod_2pi):
175 shortest_angle = delta
177 shortest_angle = delta_mod_2pi
178 return False, shortest_angle
181 """ Returns the delta from `from_angle` to `to_angle`, making sure it does not violate limits specified by `left_limit` and `right_limit`.
182 This function is similar to `shortest_angular_distance_with_limits()`, with the main difference that it accepts limits outside the `[-M_PI, M_PI]` range.
183 Even if this is quite uncommon, one could indeed consider revolute joints with large rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`.
185 In this case, a strict requirement is to have `left_limit` smaller than `right_limit`.
186 Note also that `from_angle` must lie inside the valid range, while `to_angle` does not need to.
187 In fact, this function will evaluate the shortest (valid) angle `shortest_angle` so that `from_angle+shortest_angle` equals `to_angle` up to an integer multiple of `2*M_PI`.
188 As an example, a call to `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI)` will return `true`, with `shortest_angle=0.5*M_PI`.
189 This is because `from_angle` and `from_angle+shortest_angle` are both inside the limits, and `fmod(to_angle+shortest_angle, 2*M_PI)` equals `fmod(to_angle, 2*M_PI)`.
190 On the other hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI)` will return false, since `from_angle` is not in the valid range.
191 Finally, note that the call `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI)` will also return `true`.
192 However, `shortest_angle` in this case will be `-1.5*M_PI`.
194 \return valid_flag, shortest_angle - valid_flag will be true if `left_limit < right_limit` and if "from_angle" and "from_angle+shortest_angle" positions are within the valid interval, false otherwise.
195 \param left_limit - left limit of valid interval, must be smaller than right_limit.
196 \param right_limit - right limit of valid interval, must be greater than left_limit.
203 if fabs(delta) > fabs(delta_2pi):
204 delta, delta_2pi = delta_2pi, delta
206 if left_limit > right_limit:
224 to2 = from_angle + delta
225 if left_limit <= to2
and to2 <= right_limit:
227 valid_flag = left_limit <= from_angle
and from_angle <= right_limit
228 return valid_flag, delta
231 to2 = from_angle + delta_2pi
232 if left_limit <= to2
and to2 <= right_limit:
234 valid_flag = left_limit <= from_angle
and from_angle <= right_limit
235 return valid_flag, delta_2pi