I am currently debugging issues for a robot with very large joint limits (>2pi for both left and right limits). In particular, when the current implementation of shortest_angular_distance_with_large_limits
returns false
.
I traced some of these cases down to:
(a) user input: when from
is outside of left_limit
--> This is already highlighted in the doxygen as a requirement for the function to work as expected.
(b) machine precision when comparing the clockwise/counter-clockwise options with <=
.
I'd like to get your insights into how to best address:
For (a), would it be an option to clamp from
to left/right limits as a first step inside the function if outside, or should this be on the user to enforce? This would resolve the current stated limitation for the function to work as expected as highlighted in the function documentation, but would change current existing behaviour. Some users, e.g. the ROS-Control boilerplate controllers, do not currently take this restriction into account and may be unaware of the incorrect function if from
is outside the limits. (I will prepare a fix for the JointGroupPositionController
).
For (b), this case happens e.g. when we are exactly on the boundary of the limits (clamped outside as a solution to (a)), and with the additional floating point operations i.e. adding the delta/delta_2pi, the conditions are now no longer met with <=
(cf.
|
// start by trying with the shortest angle (delta). |
|
double to2 = from + delta; |
|
if(left_limit <= to2 && to2 <= right_limit) { |
|
// we can move in this direction: return success if the "from" angle is inside limits |
|
shortest_angle = delta; |
|
return left_limit <= from && from <= right_limit; |
|
} |
|
|
|
// delta is not ok, try to move in the other direction (using its complement) |
|
to2 = from + delta_2pi; |
|
if(left_limit <= to2 && to2 <= right_limit) { |
|
// we can move in this direction: return success if the "from" angle is inside limits |
|
shortest_angle = delta_2pi; |
|
return left_limit <= from && from <= right_limit; |
|
} |
). One work-around I am currently using is to subtract/add an
eps
to the limits when clamping before passing the
from
/
to
as arguments.. We could use the machine precision epsilon and figure out the number of operations/expected deviation or select a small-enough-to-be-not-noticeable-in-practice value (1e-6, 1e-9). Would that be workable?
Alternatively, and perhaps the "wontfix" solution could be to advise to clamp the from
and to
arguments to something smaller by eps than the limits. This would of course resolve (b) entirely in user-code.
Here are a few example cases:
from __future__ import print_function
import angles
examples = [
#[5.95171, 0.331613, 0.331613, 5.95157, 0.66309], # issue: from value is outside right_limit --> (a), expected behaviour
#[6.80644, 2, 0.523599, 5.75959, -4.80644], # issue: from value is outside right_limit --> (a), expected behaviour
#[9.09866, 0.523599, 0.523599, 5.75959, -2.29187], # issue: from value is outside right_limit --> (a), expected behaviour
[5.75959, 0.523599, 0.523599, 5.75959, -2.29187], # issue: from is _at_ right_limit, to is _at_ left_limit --> (b), machine precision issue
]
for example in examples:
print(example[:-1], "=", angles.shortest_angular_distance_with_large_limits(example[0], example[1], example[2], example[3]))