[effort_controllers] JointGroupPositionController: Check output of angles::shortest_angular_distance_with_large_limits

[wxmerkt]

On top of #504

The current implementation does not check the output of angles::shortest_angular_distance_with_large_limits which indicates if the returned distance is an unspecified (and potentially wrong) case.

Firstly, this commit checks the return statement and adds a detailed warning. If false is returned, e.g. the direction of the error might be wrong and the PID would command further against the joint limit.

Secondly, the implementation in angles::shortest_angular_distance_with_large_limits is only valid if the from argument is within the range given by the upper and lower joint limit. This is highlighted in the doxygen of the function. As thus, the current_position is now enforced/clamped to the joint limits.

There are further cases which are not yet treated in this PR: When the current or command positions are directly on the joint limit due to floating point precision. I've opened an issue to get insights into where to best handle this upstream: ros/angles#25. A quick-fix would be to modify the limits in enforceJointLimits by e.g. 1e-6.

[matthew-reynolds]

This change has no dependence on your last 2 PRs, could you please trim the branch down to just the relevant commit to make it easier to review and merge.

[matthew-reynolds]

Ping @francofusco as the one who knows most about this.

I'll need to give this a proper review tomorrow but my initial impression is that clipping/saturating the current position seems like the wrong approach.

[wxmerkt]

I've separated out the commit.

Very interested to hear about alternatives to enforcing the joint limits - the important factor is right now it's broken by using undefined, known to be incorrect results for the error when the return value is false. I've seen this lead to joint run-offs when they are outside the URDF-defined position limits. We need to check the return result for sure, and then happy to consider other options/logic to handle those cases.

[francofusco]

Hi @wxmerkt!

The current implementation does not check the output of angles::shortest_angular_distance_with_large_limits which indicates if the returned distance is an unspecified (and potentially wrong) case.

You are totally right, when I updated the code I simply changed calls to angles::shortest_angular_distance_with_limits into their large_limits counterpart... My bad!

Firstly, this commit checks the return statement and adds a detailed warning.

Looks good 👍

Secondly, the implementation in angles::shortest_angular_distance_with_large_limits is only valid if the from argument is within the range given by the upper and lower joint limit. This is highlighted in the doxygen of the function. As thus, the current_position is now enforced/clamped to the joint limits.

This choice does not fully convince me. If I am not mistaken, current_position should contain the actual position of the motor. As such, it is not something that you can "freely manipulate". I think that it would be important to decide how to handle the case in which, for any reason, one joint goes out of its limits. I mainly see two solutions:

1. Stop the robot, meaning that it will hold the current position until hard-reset
2. Keep controlling the motor as if nothing happened

The second option is almost "for free". In fact, even when false is returned, the error variable should still be set to the shortest angular distance. It think this second solution should be slightly more "resilient": say that your upper limit is 90°, you command the robot to be in 89° and there is a slight overshoot, so that the shaft rotates up to 91°. In the current implementation, the error would be set to -2°, with false being returned because the current position is larger than the limit. But the -2° are evaluated anyway, so the motor would indeed be controlled! At least, this is what I intended the code to do and what I experienced in my tests. However, you said:

The important factor is right now it's broken by using undefined, [...] I've seen this lead to joint run-offs when they are outside the URDF-defined position limits.

If I understand correctly, you mean that the error was left undefined after the function call, leading to some motor misbehaving. If this is the case, do you have more information related to the issue?

---

To sum up: your ROS_WARN should definitely be included, while I'd tackle the second issue in a different way. I suggest to go for "solution nr.2", but we can think of better ways to deal with it 😉

Regarding the issue with the desired position being exactly the joint limit: as you said this is perhaps better addressed in ros-angles, I'll answer you there 😃

[francofusco]

Another important point: when I introduced the large_limits functions, I updated three controllers (those that were using shortest_angular_distance_with_limits):

• effort_controllers/JointGroupPositionController
• effort_controllers/JointPositionController
• velocity_controllers/JointPositionController

Once we have decided how to solve the issues you pointed out, we should remember to update all three controllers!

[wxmerkt]

Hi @francofusco,
Thanks for the very detailed response and insights.

I am happy to remove the enforcing of joint limits. I noticed that for some cases with very, very large limits we had the opposite direction being returned as intended - an error is returned and defined in any case, but it may be pointing in the wrong direction.

For those cases, I initially had a version which if the return value was false, just used error = command_position - current_position - or alternatively first overrides the command position to be something within the closest limit. I am happy for whichever suggestion you feel is correct - or to just have the warning and remove the amendment for now until we identify a better solution.

Also happy to apply it to all three controllers once a consensus is reached.

Many thanks,
Wolfgang

[francofusco]

I noticed that for some cases with very, very large limits we had the opposite direction being returned as intended - an error is returned and defined in any case, but it may be pointing in the wrong direction.

I'll think about this, it might be that in some cases the error is "swapped". Clearly this should not happen, I was expecting something closer to your error = command_position - current_position. If you happen to have some precise values for which this happens, feel free to write them down and we'll try to "reverse engineer" the error 👍

or alternatively first overrides the command position to be something within the closest limit

This might be a good way to handle some of the issues. What about doing something similar to the following?

bool is_valid = angles::shortest_angular_distance_with_large_limits(
            current_position,
            command_position,
            joint_urdfs_[i]->limits->lower,
            joint_urdfs_[i]->limits->upper,
            error);

if(!is_valid) {
  double new_target = current_position + error;
  enforceJointLimits(new_target, i);
  error =  new_target - current_position;
}

[matthew-reynolds]

Thanks for jumping on this @francofusco and for the awesome breakdown.

current_position should contain the actual position of the motor

Thanks for articulating this more clearly that I did. I agree wholeheartedly, I don't want to clip the real joint position and hide out-of-bounds conditions. They can and do happen, and should be safely handled.

The important factor is right now it's broken by using undefined, [...] I've seen this lead to joint run-offs when they are outside the URDF-defined position limits.

If I understand correctly, you mean that the error was left undefined after the function call, leading to some motor misbehaving. If this is the case, do you have more information related to the issue?

I think the confusion here might be that the documentation of shorted_angular_distance_with_large_limits doesn't make it clear what shortest_angle will be when it returns false. From peeking at the code, it's clear it will just be the shortest angular distance (no limit checking), but the documentation doesn't explicitly state that.

I suggest to go for "solution nr.2", but we can think of better ways to deal with it

👍, I imagine sol 1 would be quite problematic unless we added some option that allowed the "freeze on out-of-bounds" to be toggled (Which might be worth considering anyways, might be a handy feature).

or alternatively first overrides the command position to be something within the closest limit

This might be a good way to handle some of the issues. What about doing something similar to the following?

As long as we mean "Closest limit" in the whole revolution space, rather than just "closest" around a 2pi range, I agree with the concept but I don't think we need to override the command position. The command position is guaranteed to be within the valid range, so an attempt to get to the command would also be trying to bring us back in range. I think the bigger concern here is, as you @wxmerkt mentioned, the possibility of getting an error that further violates the limits in an attempt to get back in range. If you're out of range by more than 2pi, shorted_angular_distance_with_large_limits will always return false and set shortest_angle = shortest_angular_distance(from, to), which will always be between [-pi, pi].

I like the idea of your if (!is_valid) but I'm a bit concerned there may still be edge cases where relying on error combined with using enforceJointLimits might push us in the wrong direction. I haven't walked through with some numbers, but I'm just a bit concerned about using error while out of bounds. Would there be issue with just using error = current_position - command_position to try to pull us back within bounds? I think by ignoring any angle wrapping and just using a simple subtraction we can ensure we will always get back in bounds (along the same positions that got us out of bounds), rather than things getting weird and the robot trying to get back in bounds by going further out of bounds.

[francofusco]

I think the confusion here might be that the documentation of shorted_angular_distance_with_large_limits doesn't make it clear what shortest_angle will be when it returns false. From peeking at the code, it's clear it will just be the shortest angular distance (no limit checking), but the documentation doesn't explicitly state that.

Yes, that's pretty much it: the distance should still be set to the shortest angle. I'll consider updating the documentation at ros/angles so that it is clear without having to look at the code.

I think the bigger concern here is, as you @wxmerkt mentioned, the possibility of getting an error that further violates the limits in an attempt to get back in range. If you're out of range by more than 2pi, shorted_angular_distance_with_large_limits will always return false and set shortest_angle = shortest_angular_distance(from, to), which will always be between [-pi, pi]

I did not think about it 😥 Now that you say that, I get the situation described by @wxmerkt in which the error is pointing in the wrong direction. Sorry, I was a little slow!

The command position is guaranteed to be within the valid range, so an attempt to get to the command would also be trying to bring us back in range. [...] Would there be issue with just using error = current_position - command_position to try to pull us back within bounds?

Yes, you are probably right, I was just overthinking it!

However, the more I reason about this issue, the more I realize that perhaps the whole idea of using angle wrapping leads to more issues than benefits? I'll try to elaborate here.

If no limits are used (continuous joints) then we can happily use shortest_angular_distance. However, if there are joint limits involved, we have potentially two cases: the total range of rotation is either less than 2*PI ("short limits") or greater than that ("large limits").

With short limits, we are sure that the command will be shifted within the valid range, and thus the "raw error" current_position - command_position will always be less than 2*PI. In addition, since the limits are short, the "complementary angle" will always lead to limits violations. Long story short: with short limits, shortest_angular_distance_with_large_limits will just return the raw error.

Coming now to the large limits, let's assume that the bounds are ridiculously large, like [-1e10,1e10]. The cool thing of using wrapping is that if my joint is currently at 350deg and I ask to move it to 0deg, it will simply make a short motion of 10deg and reach 360deg. The only downside I see is that, if I actually wanted it to go to 0deg, I'd have to "bring it there" gradually, but let's forget about it for a moment. The biggest benefit I see in wrapping is that it would allow to avoid sudden jumps in certain cases. As an example, the commands might be produced from functions like "atan2(y,x)", which is discontinuous when x<0 and y changes sign. In this case, wrapping would make sure that commands are "smooth". If wrapping was not available, the issue would be that the user would be required to monitor the current position of the joint and to properly shift the command accordingly.

Now consider the limits to be [-360PI,360PI]. Say that the joint was at 350deg. Say that I send a command that changes from -10deg to 10deg. During the first 10deg, the command would be "shifted" so that the joint moves from 350deg to 360deg. However, as soon as the command becomes positive, a jump happens. As an example, if the command was 1deg, the shortest wrapped motion would lead the joint to go at 361deg. However, this violates the upper limit, and thus shortest_angular_distance_with_large_limits would return an error equal to -359deg. In practice, it wants the arm to move from 360deg to the "real" 1deg position. Note that, if the original command was to go from 350deg to 370deg, values beyond 360 would have been "trimmed", and the joint would have stopped at 360deg. Long story short, the only way to avoid similar jumps is to monitor the current position of the joint and to shift the command manually.

To sum up:

• With short limits there is no practical difference between using the value returned by shortest_angular_distance_with_large_limits and error = current_position - command_position
• With large limits:
  • Angle wrapping saves from sudden jumps in some cases
  • Angle wrapping can introduce sudden jumps in other cases
  • As we are discussing in this PR, wrapping should be "ignored" anyway in some situations
  • If I really wanted a joint to move more than 180deg, I would generally have to "pilot it" to "bypass wrapping"

With these considerations in mind, I argue that it would be easier to just use error = current_position - command_position when limits of any kind are involved. It indirectly solves the current issue, since the call to shortest_angular_distance_with_large_limits would disappear. It would finally make the behavior of the controller easier to understand: if I am at 350deg and I command to go at 0deg, I move 350deg backward, not 10deg forward.

What do you think? Unfortunately, when I proposed #466 I did not think about it, but perhaps the "easy" solution is the best. I also think that this would be a minor issue in terms of compatibility, as robots with short limits (arguably the majority?) would not be affected. Finally, I can move the discussion somewhere else if you think this has become off-topic.